commit f0c057630eb08a838e82affa1fd9b0fa013a2dc6
author Austin Schuh <austin.linux@gmail.com> Sun Apr 03 16:06:49 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 03 16:06:49 2016 -0700
tree ce2dc23a96ca3ff1b3fb6334cb852ba212114753
parent dcef350a0a6af455b6f693b21b591b9819fba080

Wired up IMU on the robot.

Change-Id: Id6719439c942be5a230b85966120e19febd8d152

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7c4f760..2280648 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -123,15 +123,15 @@
                                      ::frc971::imu_values->accelerometer_y +
                                  ::frc971::imu_values->accelerometer_z *
                                      ::frc971::imu_values->accelerometer_z;
-    const double angle = ::std::atan2(-::frc971::imu_values->accelerometer_x,
-                                      -::frc971::imu_values->accelerometer_z) +
+    const double angle = ::std::atan2(::frc971::imu_values->accelerometer_x,
+                                      ::frc971::imu_values->accelerometer_z) +
                          0.008;
     if (accel_squared > 1.03 || accel_squared < 0.97) {
       LOG(DEBUG, "New IMU value, rejecting reading\n");
     } else {
       // -y is our gyro.
-      // -z accel is down
-      // -x accel is the front of the robot pointed down.
+      // z accel is down
+      // x accel is the front of the robot pointed down.
       Eigen::Matrix<double, 1, 1> Y;
       Y << angle;
       down_estimator_.Correct(Y);

y2016/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 8273bf9..d169b42 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -685,14 +685,14 @@
     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(3));
-    reader.set_shooter_right_encoder(make_encoder(4));
+    reader.set_shooter_left_encoder(make_encoder(7));
+    reader.set_shooter_right_encoder(make_encoder(-3));
 
     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(2));
+    reader.set_shoulder_encoder(make_encoder(4));
     reader.set_shoulder_index(make_unique<DigitalInput>(2));
     reader.set_shoulder_potentiometer(make_unique<AnalogInput>(2));
 
@@ -713,11 +713,9 @@
     ::frc971::wpilib::GyroSender gyro_sender;
     ::std::thread gyro_thread(::std::ref(gyro_sender));
 
-#if 0
-    auto imu_trigger = make_unique<DigitalInput>(100);
-    ::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
+    auto imu_trigger = make_unique<DigitalInput>(3);
+    ::frc971::wpilib::ADIS16448 imu(SPI::Port::kMXP, imu_trigger.get());
     ::std::thread imu_thread(::std::ref(imu));
-#endif
 
     DrivetrainWriter drivetrain_writer;
     drivetrain_writer.set_drivetrain_left_talon(
@@ -783,10 +781,8 @@
     reader_thread.join();
     gyro_sender.Quit();
     gyro_thread.join();
-#if 0
     imu.Quit();
     imu_thread.join();
-#endif
 
     drivetrain_writer.Quit();
     drivetrain_writer_thread.join();