Merge "Log the pulse widths from the drivetrain encoders"
diff --git a/y2014/control_loops/drivetrain/drivetrain.q b/y2014/control_loops/drivetrain/drivetrain.q
index ddc2efc..96bd817 100644
--- a/y2014/control_loops/drivetrain/drivetrain.q
+++ b/y2014/control_loops/drivetrain/drivetrain.q
@@ -2,20 +2,37 @@
 
 import "aos/common/controls/control_loops.q";
 
+// For logging information about what the code is doing with the shifters.
 struct GearLogging {
+  // Which controller is being used.
   int8_t controller_index;
+  // Whether the left loop is the high-gear one.
   bool left_loop_high;
+  // Whether the right loop is the high-gear one.
   bool right_loop_high;
+  // The state of the left shifter.
   int8_t left_state;
+  // The state of the right shifter.
   int8_t right_state;
 };
 
+// For logging information about the state of the shifters.
 struct CIMLogging {
+  // Whether the code thinks the left side is currently in gear.
   bool left_in_gear;
+  // Whether the code thinks the right side is currently in gear.
   bool right_in_gear;
+  // The velocity in rad/s (positive forward) the code thinks the left motor
+  // is currently spinning at.
   double left_motor_speed;
+  // The velocity in rad/s (positive forward) the code thinks the right motor
+  // is currently spinning at.
   double right_motor_speed;
+  // The velocity estimate for the left side of the robot in m/s (positive
+  // forward) used for shifting.
   double left_velocity;
+  // The velocity estimate for the right side of the robot in m/s (positive
+  // forward) used for shifting.
   double right_velocity;
 };
 
@@ -23,21 +40,45 @@
   implements aos.control_loops.ControlLoop;
 
   message Goal {
+    // Position of the steering wheel (positive = turning left when going
+    // forwards).
     double steering;
+    // Position of the throttle (positive forwards).
     double throttle;
+    // True to shift into high, false to shift into low.
     bool highgear;
+    // True to activate quickturn.
     bool quickturn;
+    // True to have the closed-loop controller take over.
     bool control_loop_driving;
+    // Position goal for the left side in meters when the closed-loop controller
+    // is active.
     double left_goal;
+    // Velocity goal for the left side in m/s when the closed-loop controller
+    // is active.
     double left_velocity_goal;
+    // Position goal for the right side in meters when the closed-loop
+    // controller is active.
     double right_goal;
+    // Velocity goal for the right side in m/s when the closed-loop controller
+    // is active.
     double right_velocity_goal;
   };
 
   message Position {
+    // Relative position of the left side in meters.
     double left_encoder;
+    // Relative position of the right side in meters.
     double right_encoder;
+    // The speed in m/s of the left side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double left_speed;
+    // The speed in m/s of the right side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double right_speed;
+    // Position of the left shifter (smaller = towards low gear).
     double left_shifter_position;
+    // Position of the right shifter (smaller = towards low gear).
     double right_shifter_position;
     double low_left_hall;
     double high_left_hall;
@@ -46,21 +87,33 @@
   };
 
   message Output {
+    // Voltage to send to the left motor(s).
     double left_voltage;
+    // Voltage to send to the right motor(s).
     double right_voltage;
+    // True to set the left shifter piston for high gear.
     bool left_high;
+    // True to set the right shifter piston for high gear.
     bool right_high;
   };
 
   message Status {
+    // Estimated speed of the center of the robot in m/s (positive forwards).
     double robot_speed;
+    // Estimated relative position of the left side in meters.
     double filtered_left_position;
+    // Estimated relative position of the right side in meters.
     double filtered_right_position;
+    // Estimated velocity of the left side in m/s.
     double filtered_left_velocity;
+    // Estimated velocity of the left side in m/s.
     double filtered_right_velocity;
 
+    // The voltage we wanted to send to the left side last cycle.
     double uncapped_left_voltage;
+    // The voltage we wanted to send to the right side last cycle.
     double uncapped_right_voltage;
+    // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
 
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 50dee34..0278ebc 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -79,6 +79,12 @@
          (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
 }
 
+double drivetrain_velocity_translate(double in) {
+  return (1.0 / in) / 256.0 /*cpr*/ *
+         constants::GetValues().drivetrain_encoder_ratio *
+         (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+}
+
 float hall_translate(const constants::ShifterHallEffect &k, float in_low,
                      float in_high) {
   const float low_ratio =
@@ -130,10 +136,12 @@
 
   void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
     drivetrain_left_encoder_ = ::std::move(encoder);
+    drivetrain_left_encoder_->SetMaxPeriod(0.005);
   }
 
   void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
     drivetrain_right_encoder_ = ::std::move(encoder);
+    drivetrain_right_encoder_->SetMaxPeriod(0.005);
   }
 
   void set_high_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
@@ -274,6 +282,10 @@
           drivetrain_translate(drivetrain_right_encoder_->GetRaw());
       drivetrain_message->left_encoder =
           -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+      drivetrain_message->left_speed =
+          drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+      drivetrain_message->right_speed =
+          drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
 
       drivetrain_message->low_left_hall = low_left_drive_hall_->GetVoltage();
       drivetrain_message->high_left_hall = high_left_drive_hall_->GetVoltage();
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index cef50d0..f5f53bb 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -2,12 +2,23 @@
 
 import "aos/common/controls/control_loops.q";
 
+// For logging information about the state of the shifters.
 struct CIMLogging {
+  // Whether the code thinks the left side is currently in gear.
   bool left_in_gear;
+  // Whether the code thinks the right side is currently in gear.
   bool right_in_gear;
+  // The velocity in rad/s (positive forward) the code thinks the left motor
+  // is currently spinning at.
   double left_motor_speed;
+  // The velocity in rad/s (positive forward) the code thinks the right motor
+  // is currently spinning at.
   double right_motor_speed;
+  // The velocity estimate for the left side of the robot in m/s (positive
+  // forward) used for shifting.
   double left_velocity;
+  // The velocity estimate for the right side of the robot in m/s (positive
+  // forward) used for shifting.
   double right_velocity;
 };
 
@@ -15,38 +26,72 @@
   implements aos.control_loops.ControlLoop;
 
   message Goal {
+    // Position of the steering wheel (positive = turning left when going
+    // forwards).
     double steering;
+    // Position of the throttle (positive forwards).
     double throttle;
+    // True to shift into high, false to shift into low.
     bool highgear;
+    // True to activate quickturn.
     bool quickturn;
+    // True to have the closed-loop controller take over.
     bool control_loop_driving;
+    // Position goal for the left side in meters when the closed-loop controller
+    // is active.
     double left_goal;
+    // Velocity goal for the left side in m/s when the closed-loop controller
+    // is active.
     double left_velocity_goal;
+    // Position goal for the right side in meters when the closed-loop
+    // controller is active.
     double right_goal;
+    // Velocity goal for the right side in m/s when the closed-loop controller
+    // is active.
     double right_velocity_goal;
   };
 
   message Position {
+    // Relative position of the left side in meters.
     double left_encoder;
+    // Relative position of the right side in meters.
     double right_encoder;
+    // The speed in m/s of the left side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double left_speed;
+    // The speed in m/s of the right side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double right_speed;
   };
 
   message Output {
+    // Voltage to send to the left motor(s).
     double left_voltage;
+    // Voltage to send to the right motor(s).
     double right_voltage;
+    // True to set the left shifter piston for high gear.
     bool left_high;
+    // True to set the right shifter piston for high gear.
     bool right_high;
   };
 
   message Status {
+    // Estimated speed of the center of the robot in m/s (positive forwards).
     double robot_speed;
+    // Estimated relative position of the left side in meters.
     double filtered_left_position;
+    // Estimated relative position of the right side in meters.
     double filtered_right_position;
+    // Estimated velocity of the left side in m/s.
     double filtered_left_velocity;
+    // Estimated velocity of the right side in m/s.
     double filtered_right_velocity;
 
+    // The voltage we wanted to send to the left side last cycle.
     double uncapped_left_voltage;
+    // The voltage we wanted to send to the right side last cycle.
     double uncapped_right_voltage;
+    // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
 
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 9632160..4fde0f5 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -65,6 +65,12 @@
          (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
 }
 
+double drivetrain_velocity_translate(double in) {
+  return (1.0 / in) / 256.0 /*cpr*/ *
+         ::y2014_bot3::control_loops::kDrivetrainEncoderRatio *
+         (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
 // Reads in our inputs. (sensors, voltages, etc.)
 class SensorReader {
  public:
@@ -72,10 +78,12 @@
 
   void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
     drivetrain_left_encoder_ = ::std::move(encoder);
+    drivetrain_left_encoder_->SetMaxPeriod(0.005);
   }
 
   void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
     drivetrain_right_encoder_ = ::std::move(encoder);
+    drivetrain_right_encoder_->SetMaxPeriod(0.005);
   }
 
   void operator()() {
@@ -107,6 +115,10 @@
           drivetrain_translate(drivetrain_right_encoder_->GetRaw());
       drivetrain_message->left_encoder =
           -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+      drivetrain_message->left_speed =
+          drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+      drivetrain_message->right_speed =
+          drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
 
       drivetrain_message.Send();
     }
diff --git a/y2015/control_loops/drivetrain/drivetrain.q b/y2015/control_loops/drivetrain/drivetrain.q
index 0d804cb..824688c 100644
--- a/y2015/control_loops/drivetrain/drivetrain.q
+++ b/y2015/control_loops/drivetrain/drivetrain.q
@@ -2,20 +2,37 @@
 
 import "aos/common/controls/control_loops.q";
 
+// For logging information about what the code is doing with the shifters.
 struct GearLogging {
+  // Which controller is being used.
   int8_t controller_index;
+  // Whether the left loop is the high-gear one.
   bool left_loop_high;
+  // Whether the right loop is the high-gear one.
   bool right_loop_high;
+  // The state of the left shifter.
   int8_t left_state;
+  // The state of the right shifter.
   int8_t right_state;
 };
 
+// For logging information about the state of the shifters.
 struct CIMLogging {
+  // Whether the code thinks the left side is currently in gear.
   bool left_in_gear;
+  // Whether the code thinks the right side is currently in gear.
   bool right_in_gear;
+  // The velocity in rad/s (positive forward) the code thinks the left motor
+  // is currently spinning at.
   double left_motor_speed;
+  // The velocity in rad/s (positive forward) the code thinks the right motor
+  // is currently spinning at.
   double right_motor_speed;
+  // The velocity estimate for the left side of the robot in m/s (positive
+  // forward) used for shifting.
   double left_velocity;
+  // The velocity estimate for the right side of the robot in m/s (positive
+  // forward) used for shifting.
   double right_velocity;
 };
 
@@ -23,40 +40,76 @@
   implements aos.control_loops.ControlLoop;
 
   message Goal {
+    // Position of the steering wheel (positive = turning left when going
+    // forwards).
     double steering;
+    // Position of the throttle (positive forwards).
     double throttle;
+    // True to shift into high, false to shift into low.
     //bool highgear;
+    // True to activate quickturn.
     bool quickturn;
+    // True to have the closed-loop controller take over.
     bool control_loop_driving;
+    // Position goal for the left side in meters when the closed-loop controller
+    // is active.
     double left_goal;
+    // Velocity goal for the left side in m/s when the closed-loop controller
+    // is active.
     double left_velocity_goal;
+    // Position goal for the right side in meters when the closed-loop
+    // controller is active.
     double right_goal;
+    // Velocity goal for the right side in m/s when the closed-loop controller
+    // is active.
     double right_velocity_goal;
   };
 
   message Position {
+    // Relative position of the left side in meters.
     double left_encoder;
+    // Relative position of the right side in meters.
     double right_encoder;
+    // The speed in m/s of the left side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double left_speed;
+    // The speed in m/s of the right side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double right_speed;
+    // Position of the left shifter (smaller = towards low gear).
     //double left_shifter_position;
+    // Position of the right shifter (smaller = towards low gear).
     //double right_shifter_position;
   };
 
   message Output {
+    // Voltage to send to the left motor(s).
     double left_voltage;
+    // Voltage to send to the right motor(s).
     double right_voltage;
+    // True to set the left shifter piston for high gear.
     bool left_high;
+    // True to set the right shifter piston for high gear.
     bool right_high;
   };
 
   message Status {
+    // Estimated speed of the center of the robot in m/s (positive forwards).
     double robot_speed;
+    // Estimated relative position of the left side in meters.
     double filtered_left_position;
+    // Estimated relative position of the right side in meters.
     double filtered_right_position;
+    // Estimated velocity of the left side in m/s.
     double filtered_left_velocity;
+    // Estimated velocity of the right side in m/s.
     double filtered_right_velocity;
 
+    // The voltage we wanted to send to the left side last cycle.
     double uncapped_left_voltage;
+    // The voltage we wanted to send to the right side last cycle.
     double uncapped_right_voltage;
+    // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
 
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index 05ef948..0ea4b8d 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -69,6 +69,12 @@
          (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
 }
 
+double drivetrain_velocity_translate(double in) {
+  return (1.0 / in) / 256.0 /*cpr*/ *
+         constants::GetValues().drivetrain_encoder_ratio *
+         (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
 double arm_translate(int32_t in) {
   return -static_cast<double>(in) /
           (512.0 /*cpr*/ * 4.0 /*4x*/) *
@@ -203,10 +209,12 @@
 
   void set_left_encoder(::std::unique_ptr<Encoder> left_encoder) {
     left_encoder_ = ::std::move(left_encoder);
+    left_encoder_->SetMaxPeriod(0.005);
   }
 
   void set_right_encoder(::std::unique_ptr<Encoder> right_encoder) {
     right_encoder_ = ::std::move(right_encoder);
+    right_encoder_->SetMaxPeriod(0.005);
   }
 
   // All of the DMA-related set_* calls must be made before this, and it doesn't
@@ -252,6 +260,10 @@
           -drivetrain_translate(right_encoder_->GetRaw());
       drivetrain_message->left_encoder =
           drivetrain_translate(left_encoder_->GetRaw());
+      drivetrain_message->left_speed =
+          drivetrain_velocity_translate(left_encoder_->GetPeriod());
+      drivetrain_message->right_speed =
+          drivetrain_velocity_translate(right_encoder_->GetPeriod());
 
       drivetrain_message.Send();
     }
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.q b/y2015_bot3/control_loops/drivetrain/drivetrain.q
index 977f79e..7ff0498 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.q
@@ -2,20 +2,37 @@
 
 import "aos/common/controls/control_loops.q";
 
+// For logging information about what the code is doing with the shifters.
 struct GearLogging {
+  // Which controller is being used.
   int8_t controller_index;
+  // Whether the left loop is the high-gear one.
   bool left_loop_high;
+  // Whether the right loop is the high-gear one.
   bool right_loop_high;
+  // The state of the left shifter.
   int8_t left_state;
+  // The state of the right shifter.
   int8_t right_state;
 };
 
+// For logging information about the state of the shifters.
 struct CIMLogging {
+  // Whether the code thinks the left side is currently in gear.
   bool left_in_gear;
+  // Whether the code thinks the right side is currently in gear.
   bool right_in_gear;
+  // The velocity in rad/s (positive forward) the code thinks the left motor
+  // is currently spinning at.
   double left_motor_speed;
+  // The velocity in rad/s (positive forward) the code thinks the right motor
+  // is currently spinning at.
   double right_motor_speed;
+  // The velocity estimate for the left side of the robot in m/s (positive
+  // forward) used for shifting.
   double left_velocity;
+  // The velocity estimate for the right side of the robot in m/s (positive
+  // forward) used for shifting.
   double right_velocity;
 };
 
@@ -23,40 +40,76 @@
   implements aos.control_loops.ControlLoop;
 
   message Goal {
+    // Position of the steering wheel (positive = turning left when going
+    // forwards).
     double steering;
+    // Position of the throttle (positive forwards).
     double throttle;
+    // True to shift into high, false to shift into low.
     //bool highgear;
+    // True to activate quickturn.
     bool quickturn;
+    // True to have the closed-loop controller take over.
     bool control_loop_driving;
+    // Position goal for the left side in meters when the closed-loop controller
+    // is active.
     double left_goal;
+    // Velocity goal for the left side in m/s when the closed-loop controller
+    // is active.
     double left_velocity_goal;
+    // Position goal for the right side in meters when the closed-loop
+    // controller is active.
     double right_goal;
+    // Velocity goal for the right side in m/s when the closed-loop controller
+    // is active.
     double right_velocity_goal;
   };
 
   message Position {
+    // Relative position of the left side in meters.
     double left_encoder;
+    // Relative position of the right side in meters.
     double right_encoder;
+    // The speed in m/s of the left side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double left_speed;
+    // The speed in m/s of the right side from the most recent encoder pulse,
+    // or 0 if there was no edge within the last 5ms.
+    double right_speed;
+    // Position of the left shifter (smaller = towards low gear).
     //double left_shifter_position;
+    // Position of the right shifter (smaller = towards low gear).
     //double right_shifter_position;
   };
 
   message Output {
+    // Voltage to send to the left motor(s).
     double left_voltage;
+    // Voltage to send to the right motor(s).
     double right_voltage;
+    // True to set the left shifter piston for high gear.
     bool left_high;
+    // True to set the right shifter piston for high gear.
     bool right_high;
   };
 
   message Status {
+    // Estimated speed of the center of the robot in m/s (positive forwards).
     double robot_speed;
+    // Estimated relative position of the left side in meters.
     double filtered_left_position;
+    // Estimated relative position of the right side in meters.
     double filtered_right_position;
+    // Estimated velocity of the left side in m/s.
     double filtered_left_velocity;
+    // Estimated velocity of the right side in m/s.
     double filtered_right_velocity;
 
+    // The voltage we wanted to send to the left side last cycle.
     double uncapped_left_voltage;
+    // The voltage we wanted to send to the right side last cycle.
     double uncapped_right_voltage;
+    // True if the output voltage was capped last cycle.
     bool output_was_capped;
   };
 
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 1eeac1f..6b5c3db 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -73,6 +73,12 @@
          (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
 }
 
+double drivetrain_velocity_translate(double in) {
+  return (1.0 / in) / 256.0 /*cpr*/ *
+         ::y2015_bot3::control_loops::kDrivetrainEncoderRatio *
+         (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
 double elevator_translate(int32_t in) {
   return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
          ::y2015_bot3::control_loops::kElevEncoderRatio * (2 * M_PI /*radians*/) *
@@ -98,10 +104,12 @@
   // Drivetrain setters.
   void set_left_encoder(::std::unique_ptr<Encoder> left_encoder) {
     left_encoder_ = ::std::move(left_encoder);
+    left_encoder_->SetMaxPeriod(0.005);
   }
 
   void set_right_encoder(::std::unique_ptr<Encoder> right_encoder) {
     right_encoder_ = ::std::move(right_encoder);
+    right_encoder_->SetMaxPeriod(0.005);
   }
 
   // Elevator setters.
@@ -147,6 +155,10 @@
           -drivetrain_translate(right_encoder_->GetRaw());
       drivetrain_message->left_encoder =
           drivetrain_translate(left_encoder_->GetRaw());
+      drivetrain_message->left_speed =
+          drivetrain_velocity_translate(left_encoder_->GetPeriod());
+      drivetrain_message->right_speed =
+          drivetrain_velocity_translate(right_encoder_->GetPeriod());
 
       drivetrain_message.Send();
     }