Tuned elevator.
Change-Id: Id8698bfe4275185bf1b08786de9ab0f69a813d1d
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.cc b/bot3/control_loops/elevator/elevator_motor_plant.cc
index d3985b6..e1e87d9 100644
--- a/bot3/control_loops/elevator/elevator_motor_plant.cc
+++ b/bot3/control_loops/elevator/elevator_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00329835431624, 0.0, 0.407009515002;
+ A << 1.0, 0.00457884608813, 0.0, 0.836406580139;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000238685884904, 0.0831773970353;
+ B << 5.90742803242e-05, 0.0229468687616;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,11 +25,11 @@
StateFeedbackController<2, 1, 1> MakeElevatorController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.843942422954, 1.30830010079;
+ L << 0.937593947018, 16.1192495093;
Eigen::Matrix<double, 1, 2> K;
- K << 327.58799433, 4.77067036522;
+ K << 597.919635715, 17.5389953523;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0, -0.00810387520358, 0.0, 2.4569450176;
+ A_inv << 1.0, -0.0054744261904, 0.0, 1.19559078533;
return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeElevatorPlantCoefficients());
}
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.cc b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
index fba91c1..eb8a95c 100644
--- a/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00329835431624, 0.000238685884904, 0.0, 0.407009515002, 0.0831773970353, 0.0, 0.0, 1.0;
+ A << 1.0, 0.00457884608813, 5.90742803242e-05, 0.0, 0.836406580139, 0.0229468687616, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
- B << 0.000238685884904, 0.0831773970353, 0.0;
+ B << 5.90742803242e-05, 0.0229468687616, 0.0;
Eigen::Matrix<double, 1, 3> C;
C << 1.0, 0.0, 0.0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,11 +25,11 @@
StateFeedbackController<3, 1, 1> MakeIntegralElevatorController() {
Eigen::Matrix<double, 3, 1> L;
- L << 0.819604251485, 7.84671425417, 56.221891957;
+ L << 0.897632535808, 18.4574278406, 53.6443529697;
Eigen::Matrix<double, 1, 3> K;
- K << 327.58799433, 4.77067036522, 1.0;
+ K << 597.919635715, 17.5389953523, 1.0;
Eigen::Matrix<double, 3, 3> A_inv;
- A_inv << 1.0, -0.00810387520358, 0.000435373360429, 0.0, 2.4569450176, -0.204362291223, 0.0, 0.0, 1.0;
+ A_inv << 1.0, -0.0054744261904, 6.65466590119e-05, 0.0, 1.19559078533, -0.0274350648434, 0.0, 0.0, 1.0;
return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeIntegralElevatorPlantCoefficients());
}
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
index 04edd53..c9ded12 100755
--- a/bot3/control_loops/python/elevator3.py
+++ b/bot3/control_loops/python/elevator3.py
@@ -125,7 +125,7 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
q_pos = 0.08
- q_vel = 0.40
+ q_vel = 4.00
q_voltage = 6.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
@@ -262,7 +262,8 @@
if len(argv) != 5:
print "Expected .h file name and .cc file name for the Elevator and integral elevator."
else:
- elevator = Elevator("Elevator")
+ design_mass = 4*totemass + loaded_mass
+ elevator = Elevator("Elevator", mass=design_mass)
loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator],
namespaces=['bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
@@ -270,7 +271,7 @@
else:
loop_writer.Write(argv[1], argv[2])
- integral_elevator = IntegralElevator("IntegralElevator")
+ integral_elevator = IntegralElevator("IntegralElevator", mass=design_mass)
integral_loop_writer = control_loop.ControlLoopWriter("IntegralElevator", [integral_elevator],
namespaces=['bot3', 'control_loops'])
if argv[3][-3:] == '.cc':
diff --git a/bot3/control_loops/update_elevator.sh b/bot3/control_loops/update_elevator.sh
index ee4d6ed..4f0e359 100755
--- a/bot3/control_loops/update_elevator.sh
+++ b/bot3/control_loops/update_elevator.sh
@@ -7,4 +7,4 @@
export PYTHONPATH=../../frc971/control_loops/python
./python/elevator3.py elevator/elevator_motor_plant.h \
- elevator/elevator_motor_plant.cc
+ elevator/elevator_motor_plant.cc elevator/integral_elevator_motor_plant.cc elevator/integral_elevator_motor_plant.h
diff --git a/bot3/joystick_reader.cc b/bot3/joystick_reader.cc
index 21e912d..826c5f0 100644
--- a/bot3/joystick_reader.cc
+++ b/bot3/joystick_reader.cc
@@ -173,7 +173,7 @@
if (data.PosEdge(kSkyscraper)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 1.0;
- elevator_params_ = {0.8, 0.5};
+ elevator_params_ = {1.0, 5.0};
}
if (data.PosEdge(kScoreBegin)) {