Skip to content

Commit 2c94fff

Browse files
thenetworkgrinchthenetworkgrinch
committed
Add LQR Controller.
Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
1 parent 8d2c756 commit 2c94fff

File tree

1 file changed

+175
-0
lines changed

1 file changed

+175
-0
lines changed

‎yams/java/yams/math/LQRController.java‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
package yams.math;
2+
3+
import static edu.wpi.first.units.Units.KilogramSquareMeters;
4+
import static edu.wpi.first.units.Units.Kilograms;
5+
import static edu.wpi.first.units.Units.Meters;
6+
import static edu.wpi.first.units.Units.MetersPerSecond;
7+
import static edu.wpi.first.units.Units.RadiansPerSecond;
8+
import static edu.wpi.first.units.Units.Seconds;
9+
import static edu.wpi.first.units.Units.Volts;
10+
11+
import edu.wpi.first.math.Nat;
12+
import edu.wpi.first.math.VecBuilder;
13+
import edu.wpi.first.math.controller.LinearQuadraticRegulator;
14+
import edu.wpi.first.math.estimator.KalmanFilter;
15+
import edu.wpi.first.math.numbers.N1;
16+
import edu.wpi.first.math.numbers.N2;
17+
import edu.wpi.first.math.system.LinearSystem;
18+
import edu.wpi.first.math.system.LinearSystemLoop;
19+
import edu.wpi.first.math.system.plant.DCMotor;
20+
import edu.wpi.first.math.system.plant.LinearSystemId;
21+
import edu.wpi.first.units.measure.AngularVelocity;
22+
import edu.wpi.first.units.measure.Distance;
23+
import edu.wpi.first.units.measure.LinearVelocity;
24+
import edu.wpi.first.units.measure.Mass;
25+
import edu.wpi.first.units.measure.MomentOfInertia;
26+
import edu.wpi.first.units.measure.Time;
27+
import edu.wpi.first.units.measure.Voltage;
28+
import yams.gearing.MechanismGearing;
29+
30+
/**
31+
* Linear Quadratic Regulator (Regulator is a type of controller)
32+
*/
33+
public class LQRController
34+
{
35+
36+
/**
37+
* Get the {@link LinearSystem} for an elevator.
38+
*
39+
* @param motor {@link DCMotor} of the elevator.
40+
* @param mass {@link Mass} of the elevator carriage.
41+
* @param drumRadius {@link Distance} of the elevator drum radius.
42+
* @param gearing {@link MechanismGearing} of the elevator from the drum to the rotor.
43+
* @return {@link LinearSystem}
44+
*/
45+
public static LinearSystemLoop<N2, N1, N1> getElevatorSystem(DCMotor motor, Mass mass,
46+
Distance drumRadius,
47+
MechanismGearing gearing,
48+
Distance positionTolerance,
49+
LinearVelocity velocityTolerance, Voltage maxVoltage,
50+
Time loopPeriod, Distance modelTrustPosition,
51+
LinearVelocity modelTrustVelocity, double encoderTrust)
52+
{
53+
var elevatorPlant = LinearSystemId.createElevatorSystem(motor,
54+
mass.in(Kilograms),
55+
drumRadius.in(Meters),
56+
gearing.getMechanismToRotorRatio());
57+
LinearQuadraticRegulator<N2, N1, N1> controller =
58+
new LinearQuadraticRegulator<>(
59+
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0),
60+
VecBuilder.fill(positionTolerance.in(Meters), velocityTolerance.in(MetersPerSecond)), // qelms. Position
61+
// and velocity error tolerances, in meters and meters per second. Decrease this to more
62+
// heavily penalize state excursion, or make the controller behave more aggressively. In
63+
// this example we weight position much more highly than velocity, but this can be
64+
// tuned to balance the two.
65+
VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
66+
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
67+
// starting point because that is the (approximate) maximum voltage of a battery.
68+
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
69+
KalmanFilter<N2, N1, N1> observer =
70+
new KalmanFilter<>(
71+
Nat.N2(),
72+
Nat.N1(),
73+
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0),
74+
VecBuilder.fill(modelTrustPosition.in(Meters), modelTrustVelocity.in(MetersPerSecond)), // How accurate we
75+
// think our model is, in meters and meters/second.
76+
VecBuilder.fill(encoderTrust), // How accurate we think our encoder position
77+
// data is. In this case we very highly trust our encoder position reading.
78+
0.020);
79+
LinearSystemLoop<N2, N1, N1> loop =
80+
new LinearSystemLoop<>(
81+
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0),
82+
controller,
83+
observer,
84+
maxVoltage.in(Volts),
85+
loopPeriod.in(Seconds));
86+
return loop;
87+
}
88+
89+
/**
90+
* Get the {@link LinearSystem} for an arm.
91+
*
92+
* @param motor {@link DCMotor} of the arm.
93+
* @param moi {@link MomentOfInertia} of the arm. Imperial units are {@link yams.units.YUnits#PoundSquareFeet} or
94+
* {@link yams.units.YUnits#PoundSquareInches}
95+
* @param gearing {@link MechanismGearing} of the arm from the rotor to the drum.
96+
* {@code gearing.getMechanismToRotorRatio()}
97+
* @return {@link LinearSystem}
98+
*/
99+
public static LinearSystemLoop getArmSystem(DCMotor motor, MomentOfInertia moi,
100+
MechanismGearing gearing, Distance positionTolerance,
101+
LinearVelocity velocityTolerance, Voltage maxVoltage,
102+
Time loopPeriod, double modelTrust, double encoderTrust)
103+
{
104+
var armPlant = LinearSystemId.createSingleJointedArmSystem(motor,
105+
moi.in(KilogramSquareMeters),
106+
gearing.getMechanismToRotorRatio());
107+
return null;
108+
}
109+
110+
/**
111+
* Get the {@link LinearSystem} for an Flywheel.
112+
*
113+
* @param motor {@link DCMotor} of the flywheel.
114+
* @param moi {@link MomentOfInertia} of the flywheel. Imperial units are
115+
* {@link yams.units.YUnits#PoundSquareFeet} or {@link yams.units.YUnits#PoundSquareInches}
116+
* @param gearing {@link MechanismGearing} of the flywheel from the rotor to the drum.
117+
* {@code gearing.getMechanismToRotorRatio()}
118+
* @param qelms Velocity error tolerance. Decrease this to more heavily penalize state excursion, or make the
119+
* controller behave more starting point because that is the (approximate) maximum voltage of a
120+
* battery.
121+
* @param relms Control effort (voltage) tolerance. Decrease this to more heavily penalize control effort, or
122+
* make the controller less aggressive. 12 is a good starting point because that is the
123+
* (approximate) maximum voltage of a battery.
124+
* @param maxVoltage Maximum voltage that can be applied.
125+
* @param loopPeriod Nominal time between loops, normally 20ms.
126+
* @param modelTrust Arbitrary scale of how accurate we think our model is (normally 3)
127+
* @param encoderTrust Arbitrary scale of how accurate we think our encoder is. (normally 0.01)
128+
* @return {@link LinearSystem}
129+
*/
130+
public static LinearSystemLoop<N1, N1, N1> getFlywheelSystem(DCMotor motor, MomentOfInertia moi,
131+
MechanismGearing gearing, AngularVelocity qelms,
132+
Voltage relms,
133+
Voltage maxVoltage,
134+
Time loopPeriod, double modelTrust, double encoderTrust)
135+
{
136+
// The plant holds a state-space model of our flywheel. This system has the following properties:
137+
//
138+
// States: [velocity], in radians per second.
139+
// Inputs (what we can "put in"): [voltage], in volts.
140+
// Outputs (what we can measure): [velocity], in radians per second.
141+
var flywheelPlant = LinearSystemId.createFlywheelSystem(motor,
142+
moi.in(KilogramSquareMeters),
143+
gearing.getMechanismToRotorRatio());
144+
LinearQuadraticRegulator<N1, N1, N1> controller =
145+
new LinearQuadraticRegulator<>(
146+
flywheelPlant,
147+
VecBuilder.fill(qelms.in(RadiansPerSecond)),
148+
// qelms. Velocity error tolerance, in radians per second. Decrease
149+
// this to more heavily penalize state excursion, or make the controller behave more
150+
// aggressively.
151+
VecBuilder.fill(relms.in(Volts)),
152+
// relms. Control effort (voltage) tolerance. Decrease this to more
153+
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
154+
// starting point because that is the (approximate) maximum voltage of a battery.
155+
loopPeriod.in(Seconds)); // Nominal time between loops. 0.020 for TimedRobot, but can be
156+
// lower if using notifiers.
157+
158+
KalmanFilter<N1, N1, N1> m_observer =
159+
new KalmanFilter<>(
160+
Nat.N1(),
161+
Nat.N1(),
162+
flywheelPlant,
163+
VecBuilder.fill(modelTrust), // How accurate we think our model is
164+
VecBuilder.fill(encoderTrust), // How accurate we think our encoder
165+
// data is
166+
loopPeriod.in(Seconds));
167+
168+
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
169+
LinearSystemLoop<N1, N1, N1> loop =
170+
new LinearSystemLoop<>(flywheelPlant, controller, m_observer, maxVoltage.in(Volts), loopPeriod.in(Seconds));
171+
return loop;
172+
}
173+
174+
175+
}

0 commit comments

Comments
 (0)