commit 2fc10faad04a13afd823a62795311a3add24fbfd
author Austin Schuh <austin.linux@gmail.com> Mon Feb 08 00:44:34 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Feb 10 21:30:23 2016 -0800
tree 592ca0b234732ffdd4e989799b61560c4f383475
parent a88c407429851c2abef91ff5bf3c5ba15691e0af

Split out superstructure into arm and intake state feedback loops.

This is more code for the structure of the superstructure control loop.

Change-Id: I4abc83b04c57174ce087be0932e777cafdce8373

y2016/constants.cc

diff --git a/y2016/constants.cc b/y2016/constants.cc
index 52285f7..4f87312 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -50,6 +50,21 @@
 
 const double kRobotWidth = 25.0 / 100.0 * 2.54;
 
+// TODO(comran): Remove comments from ratios for pots when they are actually
+// used.
+const double kIntakeEncoderRatio = 18.0 / 48.0 * 16.0 / 72.0;
+//const double kIntakePotRatio = 48.0 / 48.0 * 16.0 / 72.0;
+const double kShoulderEncoderRatio = 18.0 / 48.0 * 16.0 / 72.0;
+//const double kShoulderPotRatio = 48.0 / 48.0 * 16.0 / 72.0;
+const double kWristEncoderRatio = 18.0 / 48.0 * 16.0 / 72.0;
+//const double kWristPotRatio = 48.0 / 48.0 * 16.0 / 72.0;
+
+const double kIntakeEncoderIndexDifference = 2.0 * M_PI * kIntakeEncoderRatio;
+const double kShoulderEncoderIndexDifference = 2.0 * M_PI * kShoulderEncoderRatio;
+const double kWristEncoderIndexDifference = 2.0 * M_PI * kWristEncoderRatio;
+
+const int kZeroingSampleSize = 200;
+
 const Values *DoGetValuesForTeam(uint16_t team) {
   switch (team) {
     case 1:  // for tests
@@ -63,6 +78,24 @@
           ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
           ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
           5.0,  // drivetrain max speed
+
+          // Intake
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Shoulder
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kShoulderEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Wrist
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kWristEncoderIndexDifference, 0.9, 0.3},
+          },
       };
       break;
     case kCompTeamNumber:
@@ -76,6 +109,24 @@
           ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
           ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
           5.0,  // drivetrain max speed
+
+          // Intake
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Shoulder
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kShoulderEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Wrist
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kWristEncoderIndexDifference, 0.9, 0.3},
+          },
       };
       break;
     case kPracticeTeamNumber:
@@ -89,6 +140,24 @@
           ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
           ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
           5.0,  // drivetrain max speed
+
+          // Intake
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Shoulder
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kShoulderEncoderIndexDifference, 0.9, 0.3},
+          },
+
+          // Wrist
+          {
+            {-M_PI - 0.05, M_PI + 0.05, -M_PI, M_PI},
+            {kZeroingSampleSize, kWristEncoderIndexDifference, 0.9, 0.3},
+          },
       };
       break;
     default: