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/polydrivetrain.h

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 689e76f..1ea38a7 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -3,6 +3,7 @@
 
 #include "aos/common/controls/polytope.h"
 
+#include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -13,32 +14,21 @@
 
 class PolyDrivetrain {
  public:
-  enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
-
   PolyDrivetrain(const DrivetrainConfig &dt_config,
                  StateFeedbackLoop<7, 2, 3> *kf);
 
   int controller_index() const { return loop_->controller_index(); }
 
-  bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
-
   // Computes the speed of the motor given the hall effect position and the
   // speed of the robot.
   double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
                     double shifter_position, double velocity, Gear gear);
 
-  // Computes the states of the shifters for the left and right drivetrain sides
-  // given a requested state.
-  void UpdateGears(Gear requested_gear);
-
-  // Computes the next state of a shifter given the current state and the
-  // requested state.
-  Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
-
-  void SetGoal(double wheel, double throttle, bool quickturn, bool highgear);
+  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
   void SetPosition(
-      const ::frc971::control_loops::DrivetrainQueue::Position *position);
+      const ::frc971::control_loops::DrivetrainQueue::Position *position,
+      Gear left_gear, Gear right_gear);
 
   double FilterVelocity(double throttle) const;
 
@@ -46,10 +36,14 @@
 
   void Update();
 
-  void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
+  void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
 
   void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
 
+  // Computes the next state of a shifter given the current state and the
+  // requested state.
+  Gear UpdateSingleGear(Gear requested_gear, Gear current_gear);
+
  private:
   StateFeedbackLoop<7, 2, 3> *kf_;
 
@@ -61,10 +55,10 @@
   double wheel_;
   double throttle_;
   bool quickturn_;
-  int stale_count_;
-  double position_time_delta_;
+
   Gear left_gear_;
   Gear right_gear_;
+
   ::frc971::control_loops::DrivetrainQueue::Position last_position_;
   ::frc971::control_loops::DrivetrainQueue::Position position_;
   int counter_;