1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/moment_of_inertia.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated arm mechanism.
*/
class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
public:
/**
* Creates a simulated arm mechanism.
*
* @param system The system representing this arm. This system can
* be created with
* LinearSystemId::SingleJointedArmSystem().
* @param gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armLength The length of the arm.
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviations of the measurements.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 2>& system,
const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
/**
* Creates a simulated arm mechanism.
*
* @param gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param moi The moment of inertia of the arm. This can be
* calculated from CAD software.
* @param armLength The length of the arm.
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
SingleJointedArmSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
using LinearSystemSim::SetState;
/**
* Sets the arm's state. The new angle will be limited between the minimum and
* maximum allowed limits.
*
* @param angle The new angle.
* @param velocity The new angular velocity.
*/
void SetState(units::radian_t angle, units::radians_per_second_t velocity);
/**
* Returns whether the arm would hit the lower limit.
*
* @param armAngle The arm height.
* @return Whether the arm would hit the lower limit.
*/
bool WouldHitLowerLimit(units::radian_t armAngle) const;
/**
* Returns whether the arm would hit the upper limit.
*
* @param armAngle The arm height.
* @return Whether the arm would hit the upper limit.
*/
bool WouldHitUpperLimit(units::radian_t armAngle) const;
/**
* Returns whether the arm has hit the lower limit.
*
* @return Whether the arm has hit the lower limit.
*/
bool HasHitLowerLimit() const;
/**
* Returns whether the arm has hit the upper limit.
*
* @return Whether the arm has hit the upper limit.
*/
bool HasHitUpperLimit() const;
/**
* Returns the current arm angle.
*
* @return The current arm angle.
*/
units::radian_t GetAngle() const;
/**
* Returns the current arm velocity.
*
* @return The current arm velocity.
*/
units::radians_per_second_t GetVelocity() const;
/**
* Returns the arm current draw.
*
* @return The arm current draw.
*/
units::ampere_t GetCurrentDraw() const;
/**
* Sets the input voltage for the arm.
*
* @param voltage The input voltage.
*/
void SetInputVoltage(units::volt_t voltage);
/**
* Calculates a rough estimate of the moment of inertia of an arm given its
* length and mass.
*
* @param length The length of the arm.
* @param mass The mass of the arm.
*
* @return The calculated moment of inertia.
*/
static constexpr units::kilogram_square_meter_t EstimateMOI(
units::meter_t length, units::kilogram_t mass) {
return 1.0 / 3.0 * mass * length * length;
}
protected:
/**
* Updates the state estimate of the arm.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
units::meter_t m_armLen;
units::radian_t m_minAngle;
units::radian_t m_maxAngle;
const DCMotor m_gearbox;
double m_gearing;
bool m_simulateGravity;
};
} // namespace frc::sim