commit 9f77fd2fa3d522f050daedcc5217475872d11d4c
author Austin Schuh <austin.linux@gmail.com> Sun Feb 21 02:53:58 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Feb 21 17:14:15 2016 -0800
tree 35e42f33e5e5fa2a60c53d9e67c3da37634b26d3
parent ccf32807adff080fc7518271cbfc02a642542801

Calibrated superstructure sensors.

Change-Id: I423537e51e13b5836d31cfe6f859c9d98afe2d91

y2016/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 9eb328d..4eb2e0d 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -83,13 +83,13 @@
 double drivetrain_translate(int32_t in) {
   return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
          constants::Values::kDrivetrainEncoderRatio *
-         control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
+         control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
 }
 
 double drivetrain_velocity_translate(double in) {
   return (1.0 / in) / 256.0 /*cpr*/ *
          constants::Values::kDrivetrainEncoderRatio *
-         control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
+         control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
 }
 
 double shooter_translate(int32_t in) {
@@ -98,7 +98,7 @@
 }
 
 double intake_translate(int32_t in) {
-  return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+  return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
          constants::Values::kIntakeEncoderRatio * (2 * M_PI /*radians*/);
 }
 
@@ -286,7 +286,7 @@
     {
       auto drivetrain_message = drivetrain_queue.position.MakeMessage();
       drivetrain_message->right_encoder =
-          drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+          drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
       drivetrain_message->left_encoder =
           -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
       drivetrain_message->left_speed =
@@ -307,7 +307,7 @@
     {
       auto shooter_message = shooter_queue.position.MakeMessage();
       shooter_message->theta_left =
-          shooter_translate(shooter_left_encoder_->GetRaw());
+          shooter_translate(-shooter_left_encoder_->GetRaw());
       shooter_message->theta_right =
           shooter_translate(shooter_right_encoder_->GetRaw());
       shooter_message.Send();
@@ -323,7 +323,7 @@
                               shoulder_translate, shoulder_pot_translate, false,
                               values.shoulder.pot_offset);
       CopyPotAndIndexPosition(wrist_encoder_, &superstructure_message->wrist,
-                              wrist_translate, wrist_pot_translate, false,
+                              wrist_translate, wrist_pot_translate, true,
                               values.wrist.pot_offset);
 
       superstructure_message.Send();
@@ -589,25 +589,25 @@
     SensorReader reader;
 
     // TODO(constants): Update these input numbers.
-    reader.set_drivetrain_left_encoder(make_encoder(0));
-    reader.set_drivetrain_right_encoder(make_encoder(1));
-    reader.set_drivetrain_left_hall(make_unique<AnalogInput>(1));
-    reader.set_drivetrain_right_hall(make_unique<AnalogInput>(2));
+    reader.set_drivetrain_left_encoder(make_encoder(5));
+    reader.set_drivetrain_right_encoder(make_encoder(6));
+    reader.set_drivetrain_left_hall(make_unique<AnalogInput>(5));
+    reader.set_drivetrain_right_hall(make_unique<AnalogInput>(6));
 
-    reader.set_shooter_left_encoder(make_encoder(0));
-    reader.set_shooter_right_encoder(make_encoder(0));
+    reader.set_shooter_left_encoder(make_encoder(3));
+    reader.set_shooter_right_encoder(make_encoder(4));
 
     reader.set_intake_encoder(make_encoder(0));
     reader.set_intake_index(make_unique<DigitalInput>(0));
     reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
 
-    reader.set_shoulder_encoder(make_encoder(0));
-    reader.set_shoulder_index(make_unique<DigitalInput>(0));
-    reader.set_shoulder_potentiometer(make_unique<AnalogInput>(0));
+    reader.set_shoulder_encoder(make_encoder(2));
+    reader.set_shoulder_index(make_unique<DigitalInput>(2));
+    reader.set_shoulder_potentiometer(make_unique<AnalogInput>(2));
 
-    reader.set_wrist_encoder(make_encoder(0));
-    reader.set_wrist_index(make_unique<DigitalInput>(0));
-    reader.set_wrist_potentiometer(make_unique<AnalogInput>(0));
+    reader.set_wrist_encoder(make_encoder(1));
+    reader.set_wrist_index(make_unique<DigitalInput>(1));
+    reader.set_wrist_potentiometer(make_unique<AnalogInput>(1));
 
     reader.set_dma(make_unique<DMA>());
     ::std::thread reader_thread(::std::ref(reader));