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