Calibrated the fridge more carefully.
Change-Id: I6b51b6c1f9a6762597551e36f626d81606e02666
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 8d4afed..3e7b4c7 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -237,40 +237,38 @@
// Motion ranges: hard_lower_limit, hard_upper_limit,
// soft_lower_limit, soft_upper_limit
// TODO(sensors): Get actual bounds before turning on robot.
- {
- // Claw values, in radians.
- // 0 is level with the ground.
- // Positive moves in the direction of positive encoder values.
- {-0.05, M_PI / 2.0 + 0.1,
- 0.0, M_PI / 2.0},
+ {// Claw values, in radians.
+ // 0 is level with the ground.
+ // Positive moves in the direction of positive encoder values.
+ {-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
- // Zeroing constants for wrist.
- // TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.977913},
- 6.1663463999999992,
+ // Zeroing constants for wrist.
+ // TODO(sensors): Get actual offsets for these.
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.977913},
+ 6.1663463999999992,
},
- {
- // Elevator values, in meters.
- // 0 is at the top of the elevator frame.
- // Positive is down towards the drivebase.
- {-0.00500, 0.679000,
- 0.010000, 0.650000},
+ {// Elevator values, in meters.
+ // 0 is at the top of the elevator frame.
+ // Positive is down towards the drivebase.
+ {-0.00500, 0.679000, 0.010000, 0.650000},
- // Arm values, in radians.
- // 0 is sticking straight out horizontally over the intake/front.
- // Positive is rotating up and into the robot (towards the back).
- {-M_PI/2 - 0.05, M_PI/2 + 0.05,
- -M_PI/2, M_PI/2},
+ // Arm values, in radians.
+ // 0 is sticking straight out horizontally over the intake/front.
+ // Positive is rotating up and into the robot (towards the back).
+ {-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
- // Elevator zeroing constants: left, right.
- // TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.016041},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.011367},
- // Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, -0.312677},
- {kZeroingSampleSize, kArmEncoderIndexDifference, -0.396278},
- 0.72069366666666679, -0.078959636363636357, -3.4952331578947375, 3.5263507647058816,
+ // Elevator zeroing constants: left, right.
+ // TODO(sensors): Get actual offsets for these.
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.016041 + 0.001290},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.011367 + 0.003216},
+ // Arm zeroing constants: left, right.
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.312677},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.40855},
+ 0.72069366666666679 - 0.026008,
+ -0.078959636363636357 - 0.024646,
+ -3.4952331578947375 + 0.011776,
+ 3.5263507647058816 - 0.018921 + 0.006545,
},
// TODO(sensors): End "sensor" values.
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]