commit 6ca0f797a4760be0d4ac834773f2ca773f05ea08
author Austin Schuh <austin.linux@gmail.com> Sat Mar 12 14:06:14 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 23:02:15 2016 -0800
tree ed69b289c5891a7a27e105d75e0dbdcbca5b9f17
parent 3f0b1197fd7948af5525a86a742cef62be41e92c

Fixed collision detection.

Tests all pass, added gain scheduling to arm, etc.

Change-Id: I4de5427b816961ead5b23c7d9287ff40dcc10f46

y2016/control_loops/python/arm.py

diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 70740d5..75f0c59 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -379,8 +379,12 @@
 
   scenario_plotter = ScenarioPlotter()
 
-  arm = Arm()
-  arm_controller = IntegralArm(name='AcceleratingIntegralArm', J=12)
+  J_accelerating = 12
+  J_decelerating = 5
+
+  arm = Arm(name='AcceleratingArm', J=J_accelerating)
+  arm_integral_controller = IntegralArm(
+      name='AcceleratingIntegralArm', J=J_accelerating)
   arm_observer = IntegralArm()
 
   # Test moving the shoulder with constant separation.
@@ -397,23 +401,27 @@
   scenario_plotter.run_test(arm=arm,
                             end_goal=R,
                             iterations=300,
-                            controller=arm_controller,
+                            controller=arm_integral_controller,
                             observer=arm_observer)
 
   if len(argv) != 5:
     glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
   else:
     namespaces = ['y2016', 'control_loops', 'superstructure']
-    loop_writer = control_loop.ControlLoopWriter('Arm', [arm],
-                                                 namespaces=namespaces)
+    decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
+    loop_writer = control_loop.ControlLoopWriter(
+        'Arm', [arm, decelerating_arm], namespaces=namespaces)
     loop_writer.Write(argv[1], argv[2])
 
-    decelerating_arm_controller = IntegralArm(name='DeceleratingIntegralArm', J=5)
+    decelerating_integral_arm_controller = IntegralArm(
+        name='DeceleratingIntegralArm', J=J_decelerating)
+
     integral_loop_writer = control_loop.ControlLoopWriter(
-        'IntegralArm', [arm_controller, decelerating_arm_controller],
+        'IntegralArm',
+        [arm_integral_controller, decelerating_integral_arm_controller],
         namespaces=namespaces)
     integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
-          arm_controller.shoulder_Kv))
+          arm_integral_controller.shoulder_Kv))
     integral_loop_writer.Write(argv[3], argv[4])
 
   if FLAGS.plot: