commit 093535c2c3b82fefdc45e143e981a99442d35b19
author Austin Schuh <austin.linux@gmail.com> Sat Mar 05 23:21:00 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 17:13:21 2016 -0800
tree 5f04496c73452dbbb8d520f863ed04d50c511d42
parent 5b1a4cd5817927e9f7c17e0f39c406427755669f

Added position control and profiling to drivetrain.

Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95

frc971/control_loops/drivetrain/drivetrain.h

diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 30548be..a9541b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -3,14 +3,15 @@
 
 #include "Eigen/Dense"
 
-#include "aos/common/controls/polytope.h"
 #include "aos/common/controls/control_loop.h"
 #include "aos/common/controls/polytope.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/util/log_interval.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "aos/common/util/log_interval.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -26,6 +27,8 @@
       ::frc971::control_loops::DrivetrainQueue *my_drivetrain =
           &::frc971::control_loops::drivetrain_queue);
 
+  int ControllerIndexFromGears();
+
  protected:
   // Executes one cycle of the control loop.
   void RunIteration(
@@ -36,9 +39,6 @@
 
   void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
 
-  typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
-  SimpleLogInterval no_position_ = SimpleLogInterval(
-      ::aos::time::Time::InSeconds(0.25), WARNING, "no position");
   double last_gyro_heading_ = 0.0;
   double last_gyro_rate_ = 0.0;
 
@@ -48,10 +48,17 @@
   PolyDrivetrain dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
 
+  // Current gears for each drive side.
+  Gear left_gear_;
+  Gear right_gear_;
+
   double last_left_voltage_ = 0;
   double last_right_voltage_ = 0;
 
   double integrated_kf_heading_ = 0;
+
+  bool left_high_requested_;
+  bool right_high_requested_;
 };
 
 }  // namespace drivetrain