Calibrated the fridge more carefully.
Change-Id: I6b51b6c1f9a6762597551e36f626d81606e02666
diff --git a/frc971/control_loops/fridge/arm_motor_plant.cc b/frc971/control_loops/fridge/arm_motor_plant.cc
index ef09d98..c48d9b1 100644
--- a/frc971/control_loops/fridge/arm_motor_plant.cc
+++ b/frc971/control_loops/fridge/arm_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.986224520755, 0.00477375853056, 0.0, 0.0, -5.42143988176, 0.906292554417;
+ A << 1.0, 0.00385609864291, 0.0, 0.0, 0.0, 0.580316842139, 0.0, 0.0, 0.0, 0.0, 0.989675611002, 0.00384038140364, 0.0, 0.0, -3.77990295634, 0.571703057879;
Eigen::Matrix<double, 4, 2> B;
- B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.45917395578e-05, -2.45917395578e-05, 0.0096782576655, -0.0096782576655;
+ B << 0.000138504938452, 0.000138504938452, 0.050815736504, 0.050815736504, 0.000138231861567, -0.000138231861567, 0.0506086144454, -0.0506086144454;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 1, 0, 1, 0, -1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,11 +25,11 @@
StateFeedbackController<4, 2, 2> MakeArmController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.746258537586, -0.746258537586, 49.8002281115, -49.8002281115;
+ L << 0.59015842107, 0.59015842107, 19.0789855292, 19.0789855292, 0.58068933444, -0.58068933444, 16.4237455811, -16.4237455811;
Eigen::Matrix<double, 2, 4> K;
- K << 320.979606093, 21.0129517956, 245.279302877, 31.6949206914, 320.979606094, 21.0129517956, -245.279302875, -31.6949206915;
+ K << 132.284914985, 5.45575813704, 200.395433435, 6.0775850354, 132.284914985, 5.45575813705, -200.395433435, -6.0775850354;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.985434166708, -0.00519062496618, 0.0, 0.0, 5.89486481623, 1.07234615805;
+ A_inv << 1.0, -0.00664481600894, 0.0, 0.0, 0.0, 1.72319658398, 0.0, 0.0, 0.0, 0.0, 0.985156756388, -0.00661773211593, 0.0, 0.0, 6.51351586213, 1.70540563213;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
}
diff --git a/frc971/control_loops/fridge/elevator_motor_plant.cc b/frc971/control_loops/fridge/elevator_motor_plant.cc
index 93d00af..0ae9828 100644
--- a/frc971/control_loops/fridge/elevator_motor_plant.cc
+++ b/frc971/control_loops/fridge/elevator_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00431150605903, 0.0, 0.0, 0.0, 0.737863444837, 0.0, 0.0, 0.0, 0.0, 0.989566706498, 0.00429496790999, 0.0, 0.0, -3.96458576306, 0.728435659238;
+ A << 1.0, 0.00435668193669, 0.0, 0.0, 0.0, 0.754212786054, 0.0, 0.0, 0.0, 0.0, 0.997194498569, 0.00435222083164, 0.0, 0.0, -1.07131589702, 0.751658962986;
Eigen::Matrix<double, 4, 2> B;
- B << 3.79928442185e-05, 3.79928442185e-05, 0.0144653608576, 0.0144653608576, 3.79213511069e-05, -3.79213511069e-05, 0.0144098743777, -0.0144098743777;
+ B << 3.82580284276e-05, 3.82580284276e-05, 0.0146169286307, 0.0146169286307, 3.82387898839e-05, -3.82387898839e-05, 0.0146019613563, -0.0146019613563;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 1, 0, 1, 0, -1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,11 +25,11 @@
StateFeedbackController<4, 2, 2> MakeElevatorController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.668931722418, 0.668931722418, 33.8393453814, 33.8393453814, 0.659001182868, -0.659001182868, 30.8170494953, -30.8170494953;
+ L << 0.677106393027, 0.677106393027, 35.5375738607, 35.5375738607, 0.674426730777, -0.674426730777, 34.7138874344, -34.7138874344;
Eigen::Matrix<double, 2, 4> K;
- K << 323.659330424, 11.4297509698, 518.471207663, 11.9234359025, 323.659330423, 11.4297509698, -518.471207664, -11.9234359025;
+ K << 321.310606763, 11.7674534233, 601.047935717, 12.6977148843, 321.310606763, 11.7674534233, -601.047935716, -12.6977148843;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00584323032833, 0.0, 0.0, 0.0, 1.35526432024, 0.0, 0.0, 0.0, 0.0, 0.98722285856, -0.005820816765, 0.0, 0.0, 5.37306162923, 1.34112444982;
+ A_inv << 1.0, -0.00577646258091, 0.0, 0.0, 0.0, 1.32588576923, 0.0, 0.0, 0.0, 0.0, 0.996613922337, -0.00577054766522, 0.0, 0.0, 1.42044250221, 1.32216599481;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeElevatorPlantCoefficients());
}
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
index 974ae0f..f4a76a0 100755
--- a/frc971/control_loops/python/arm.py
+++ b/frc971/control_loops/python/arm.py
@@ -45,7 +45,7 @@
self.J = self.r * self.mass
# Arm left/right spring constant (N*m / radian)
- self.spring = 3000.0
+ self.spring = 400.0
# State is [average position, average velocity,
# position difference/2, velocity difference/2]
@@ -90,8 +90,8 @@
q_pos = 0.02
q_vel = 0.300
- q_pos_diff = 0.01
- q_vel_diff = 0.15
+ q_pos_diff = 0.005
+ q_vel_diff = 0.13
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
@@ -244,7 +244,7 @@
if len(argv) != 3:
print "Expected .h file name and .cc file name for the arm."
else:
- arm = Arm("Arm")
+ arm = Arm("Arm", 2)
loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/elevator.py b/frc971/control_loops/python/elevator.py
index eb3f20c..fba72c8 100755
--- a/frc971/control_loops/python/elevator.py
+++ b/frc971/control_loops/python/elevator.py
@@ -41,7 +41,7 @@
self.dt = 0.005
# Elevator left/right spring constant (N/m)
- self.spring = 3000.0
+ self.spring = 800.0
# State is [average position, average velocity,
# position difference/2, velocity difference/2]