commit 993fe28d9e906f4f188d7fff01b8a8bfec48d62c
author Tyler Chatow <tchatow@gmail.com> Sat Apr 06 22:24:36 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 10 19:50:56 2019 -0700
tree 77a2959c1f3e1cd8f8754f31f319ef0b5c94e79e
parent 3f60ff9efeb74900faea96032350a077c25c6a6e

Gain schedule elevator and wrist.

Change-Id: I4326bc494737f3592f39aee8adb7cf8d92b0dff6

y2019/control_loops/python/elevator.py

diff --git a/y2019/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index 5859814..0a4e208 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -31,15 +31,20 @@
     kalman_q_voltage=35.0,
     kalman_r_position=0.05)
 
+kElevatorBall = copy.copy(kElevator)
+kElevatorBall.q_pos = 0.15
+kElevatorBall.q_vel = 1.5
+
 kElevatorModel = copy.copy(kElevator)
 kElevatorModel.mass = carriage_mass + first_stage_mass + 1.0
 
+
 def main(argv):
     if FLAGS.plot:
         R = numpy.matrix([[1.5], [0.0]])
-        linear_system.PlotKick(kElevator, R, plant_params=kElevatorModel)
+        linear_system.PlotKick(kElevatorBall, R, plant_params=kElevatorModel)
         linear_system.PlotMotion(
-            kElevator, R, max_velocity=5.0, plant_params=kElevatorModel)
+            kElevatorBall, R, max_velocity=5.0, plant_params=kElevatorModel)
 
     # Write the generated constants out to a file.
     if len(argv) != 5:
@@ -48,8 +53,8 @@
         )
     else:
         namespaces = ['y2019', 'control_loops', 'superstructure', 'elevator']
-        linear_system.WriteLinearSystem(kElevator, argv[1:3], argv[3:5],
-                                        namespaces)
+        linear_system.WriteLinearSystem([kElevator, kElevatorBall, kElevator],
+                                        argv[1:3], argv[3:5], namespaces)
 
 
 if __name__ == '__main__':

y2019/control_loops/python/wrist.py

diff --git a/y2019/control_loops/python/wrist.py b/y2019/control_loops/python/wrist.py
index 66b7b26..127def7 100755
--- a/y2019/control_loops/python/wrist.py
+++ b/y2019/control_loops/python/wrist.py
@@ -29,7 +29,7 @@
     name='Wrist',
     motor=control_loop.BAG(),
     G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
-    J=0.27,
+    J=0.30,
     q_pos=0.20,
     q_vel=5.0,
     kalman_q_pos=0.12,
@@ -37,6 +37,14 @@
     kalman_q_voltage=4.0,
     kalman_r_position=0.05)
 
+kWristBall = copy.copy(kWrist)
+kWristBall.J = 0.4007
+kWristBall.q_pos = 0.55
+kWristBall.q_vel = 5.0
+
+kWristPanel = copy.copy(kWrist)
+kWristPanel.J = 0.446
+
 kWristModel = copy.copy(kWrist)
 kWristModel.J = 0.1348
 
@@ -44,8 +52,8 @@
 def main(argv):
     if FLAGS.plot:
         R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
-        angular_system.PlotKick(kWrist, R, plant_params=kWristModel)
-        angular_system.PlotMotion(kWrist, R, plant_params=kWristModel)
+        angular_system.PlotKick(kWristBall, R, plant_params=kWristBall)
+        angular_system.PlotMotion(kWristBall, R, plant_params=kWristBall)
 
     # Write the generated constants out to a file.
     if len(argv) != 5:
@@ -54,8 +62,8 @@
         )
     else:
         namespaces = ['y2019', 'control_loops', 'superstructure', 'wrist']
-        angular_system.WriteAngularSystem(kWrist, argv[1:3], argv[3:5],
-                                          namespaces)
+        angular_system.WriteAngularSystem([kWrist, kWristBall, kWristPanel],
+                                          argv[1:3], argv[3:5], namespaces)
 
 
 if __name__ == '__main__':

y2019/control_loops/superstructure/superstructure.cc

diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 8fe02a3..b6ad9e8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -83,6 +83,19 @@
     }
   }
 
+  if (unsafe_goal) {
+    if (!unsafe_goal->suction.grab_piece) {
+      wrist_.set_controller_index(0);
+      elevator_.set_controller_index(0);
+    } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+      wrist_.set_controller_index(1);
+      elevator_.set_controller_index(1);
+    } else {
+      wrist_.set_controller_index(2);
+      elevator_.set_controller_index(2);
+    }
+  }
+
   // TODO(theo) move these up when Iterate() is split
   // update the goals
   collision_avoidance_.UpdateGoal(status, unsafe_goal);