commit adf2cde48445f7947b98c1995ec44ec2ac39a1e9
author Austin Schuh <austin@peloton-tech.com> Sun Nov 08 20:35:16 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Sun Nov 08 21:24:25 2015 -0800
tree 0875ab31dd44209fb3a0e9bbe6c02d26f11cbeb4
parent 0e997732d025d9b2d471b8e3989122bd62020199

Switched to 5ms cycles on the control loops.

Change-Id: I1aae3b30a9c422f1920ccb1c15e035ae847f85a9

y2014/actors/BUILD

diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 086d8b4..6fea9eb 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -82,6 +82,7 @@
     '//third_party/eigen',
     '//aos/common/util:trapezoid_profile',
     '//y2014/control_loops/drivetrain:drivetrain_queue',
+    '//frc971/control_loops:state_feedback_loop',
   ],
 )
 

y2014/actors/drivetrain_actor.cc

diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 1f1a678..aa1f6f2 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -19,10 +19,13 @@
 namespace actors {
 
 DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
-    : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
+    : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
+      loop_(constants::GetValues().make_drivetrain_loop()) {
+  loop_.set_controller_index(3);
+}
 
 bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams &params) {
-  static const auto K = constants::GetValues().make_drivetrain_loop().K();
+  static const auto K = loop_.K();
 
   const double yoffset = params.y_offset;
   const double turn_offset =
@@ -30,8 +33,8 @@
   LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
 
   // Measured conversion to get the distance right.
-  ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
-  ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
+  ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
+  ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
   const double goal_velocity = 0.0;
   const double epsilon = 0.01;
   ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
@@ -45,7 +48,7 @@
                                     constants::GetValues().turn_width / 2.0);
 
   while (true) {
-    ::aos::time::PhasedLoopXMS(10, 5000);
+    ::aos::time::PhasedLoopXMS(5, 2500);
 
     control_loops::drivetrain_queue.status.FetchLatest();
     if (control_loops::drivetrain_queue.status.get()) {

y2014/actors/drivetrain_actor.h

diff --git a/y2014/actors/drivetrain_actor.h b/y2014/actors/drivetrain_actor.h
index 583121b..888b2c0 100644
--- a/y2014/actors/drivetrain_actor.h
+++ b/y2014/actors/drivetrain_actor.h
@@ -5,6 +5,7 @@
 
 #include "aos/common/actions/actor.h"
 #include "aos/common/actions/actions.h"
+#include "frc971/control_loops/state_feedback_loop.h"
 
 #include "y2014/actors/drivetrain_action.q.h"
 
@@ -17,6 +18,9 @@
   explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
 
   bool RunAction(const actors::DrivetrainActionParams &params) override;
+
+ private:
+  StateFeedbackLoop<4, 2, 2> loop_;
 };
 
 typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>