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/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()) {