commit 3f35474523c7cb000786a24a14b5f7f7c841eb11
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 10 17:27:56 2013 -0700
committer Brian Silverman <brians> Sat Mar 16 19:59:34 2013 -0700
tree 388483e1e592806d993df0e7accea6fac1ca183e
parent 9f42e8b4933d91f6513edc88962d53bbce4928d3

Got the drivetrain to pass the disable/bad pos tests.

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index af04086..297b6dd 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -263,10 +263,16 @@
 
   bool bad_pos = false;
   if (position == NULL) {
-    LOG(WARNING, "no pos\n");
+    LOG(WARNING, "no position\n");
     bad_pos = true;
   }
 
+  bool bad_output = false;
+  if (output == NULL) {
+    LOG(WARNING, "no output\n");
+    bad_output = true;
+  }
+
   double wheel = goal->steering;
   double throttle = goal->throttle;
   bool quickturn = goal->quickturn;
@@ -287,13 +293,15 @@
       dt_closedloop.SetRawPosition(left_encoder, right_encoder);
     }
   }
-  dt_closedloop.Update(!bad_pos, bad_pos || (output == NULL));
+  dt_closedloop.Update(!bad_pos, bad_pos || bad_output);
   dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
   dt_openloop.Update();
-  if (control_loop_driving) {
-    dt_closedloop.SendMotors(output);
-  } else {
-    dt_openloop.SendMotors(output);
+  if (!bad_output) {
+    if (control_loop_driving) {
+      dt_closedloop.SendMotors(output);
+    } else {
+      dt_openloop.SendMotors(output);
+    }
   }
 }
 

frc971/control_loops/drivetrain/drivetrain.gyp

diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 84c4a3a..e7e4e16 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -52,6 +52,7 @@
         'drivetrain_lib',
         '<(AOS)/common/common.gyp:queue_testutils',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/queues/queues.gyp:queues',
       ],
     },
     {

frc971/control_loops/drivetrain/drivetrain_lib_test.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 96b3b9f..7b28795 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -9,6 +9,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/queues/GyroAngle.q.h"
 
 
 using ::aos::time::Time;
@@ -102,8 +103,9 @@
                 drivetrain_motor_(&my_drivetrain_loop_),
                 drivetrain_motor_plant_() {
     // Flush the robot state queue so we can use clean shared memory for this
-    // test.
+    // test, also for the gyro.
     ::aos::robot_state.Clear();
+    ::frc971::sensors::gyro.Clear();
     SendDSPacket(true);
   }
 
@@ -127,6 +129,7 @@
 
   virtual ~DrivetrainTest() {
     ::aos::robot_state.Clear();
+    ::frc971::sensors::gyro.Clear();
   }
 };
 
@@ -150,14 +153,15 @@
       .left_goal(-1.0)
       .right_goal(1.0).Send();
   for (int i = 0; i < 500; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
     if (i > 20 && i < 200) {
       SendDSPacket(false);
     } else {
       SendDSPacket(true);
     }
-    drivetrain_motor_plant_.SendPositionMessage();
-    drivetrain_motor_.Iterate();
-    drivetrain_motor_plant_.Simulate();
+    // frc971::sensors::gyro.MakeWithBuilder().angle(0.0).Send();
   }
   VerifyNearGoal();
 }