commit d0586650941bc4b0ddbc33eb9551bfc0a07bf688
author Philipp Schrader <philipp.schrader@gmail.com> Tue Feb 16 00:17:36 2016 +0000
committer Philipp Schrader <philipp.schrader@gmail.com> Tue Feb 16 00:17:36 2016 +0000
tree 9697b71b09d084ab90281daa7dc612f89640826e
parent 9fa521a88832e1e0ce87e02b7bf2d21b961c2e53

Clarify that wrist initialization takes relative angles

I originally assumed that they were absolute angles (i.e. with
reference to the ground).

Change-Id: I574a878cdec4420b26ec90581f231a5eb9f15efb

y2016/control_loops/superstructure/superstructure_lib_test.cc

diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 2479cc3..9dafdee 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -92,7 +92,7 @@
                               ".y2016.control_loops.superstructure.status") {
     InitializeIntakePosition(0.0);
     InitializeShoulderPosition(0.0);
-    InitializeWristPosition(0.0);
+    InitializeRelativeWristPosition(0.0);
   }
 
   void InitializeIntakePosition(double start_pos) {
@@ -110,7 +110,7 @@
   }
 
   // Must be called after any changes to InitializeShoulderPosition.
-  void InitializeWristPosition(double start_pos) {
+  void InitializeRelativeWristPosition(double start_pos) {
     arm_plant_->mutable_X(2, 0) = start_pos + arm_plant_->X(0, 0);
     arm_plant_->mutable_X(3, 0) = 0.0;
 
@@ -417,7 +417,7 @@
       constants::Values::kIntakeRange.lower);
   superstructure_plant_.InitializeShoulderPosition(
       constants::Values::kShoulderRange.lower);
-  superstructure_plant_.InitializeWristPosition(
+  superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.lower);
   ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
                   .angle_intake(constants::Values::kIntakeRange.upper)
@@ -436,7 +436,7 @@
       constants::Values::kIntakeRange.upper);
   superstructure_plant_.InitializeShoulderPosition(
       constants::Values::kShoulderRange.upper);
-  superstructure_plant_.InitializeWristPosition(
+  superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.upper);
   ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
                   .angle_intake(constants::Values::kIntakeRange.lower)
@@ -455,7 +455,7 @@
       constants::Values::kIntakeRange.upper);
   superstructure_plant_.InitializeShoulderPosition(
       constants::Values::kShoulderRange.upper);
-  superstructure_plant_.InitializeWristPosition(
+  superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.upper);
   ASSERT_TRUE(
       superstructure_queue_.goal.MakeWithBuilder()
@@ -510,7 +510,7 @@
       constants::Values::kIntakeRange.lower);
   superstructure_plant_.InitializeShoulderPosition(
       constants::Values::kShoulderRange.lower);
-  superstructure_plant_.InitializeWristPosition(
+  superstructure_plant_.InitializeRelativeWristPosition(
       constants::Values::kWristRange.lower);
   superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
   superstructure_queue_.goal.MakeWithBuilder()
@@ -531,7 +531,7 @@
   superstructure_plant_.InitializeIntakePosition(-0.001);
   superstructure_plant_.InitializeShoulderPosition(
       constants::Values::kShoulderEncoderIndexDifference * 10 - 0.001);
-  superstructure_plant_.InitializeWristPosition(-0.001);
+  superstructure_plant_.InitializeRelativeWristPosition(-0.001);
 
   superstructure_queue_.goal.MakeWithBuilder()
       .angle_intake(0.0)