commit bd5308ac4e9715a3d73d0dbaec9eb604ea16f44b
author Austin Schuh <austin@peloton-tech.com> Sat Feb 28 21:48:44 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Sun Mar 01 00:04:48 2015 -0800
tree cf9d0436781b94f2debb41fba1d5a21f055c01ab
parent f78c933f4751adf62a5fc51d7534eabd6d0a7592

Claw control loop now is only profiled.

Change-Id: I862b2abdd61ef70a483ebb7066dde13e946274d8

frc971/control_loops/claw/claw.cc

diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 004ea7a..64ba98e 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -7,6 +7,7 @@
 
 #include "frc971/constants.h"
 #include "frc971/control_loops/claw/claw_motor_plant.h"
+#include "aos/common/util/trapezoid_profile.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -33,7 +34,8 @@
     : aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
       last_piston_edge_(Time::Now()),
       claw_loop_(new ClawCappedStateFeedbackLoop(MakeClawLoop())),
-      claw_estimator_(constants::GetValues().claw.zeroing) {}
+      claw_estimator_(constants::GetValues().claw.zeroing),
+      profile_(::aos::controls::kLoopFrequency) {}
 
 void Claw::UpdateZeroingState() {
   if (claw_estimator_.offset_ratio_ready() < 1.0) {
@@ -169,6 +171,14 @@
         claw_goal_ +=
             claw_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
       }
+
+      // Clear the current profile state if we are zeroing.
+      {
+        Eigen::Matrix<double, 2, 1> current;
+        current.setZero();
+        current << claw_goal_, claw_goal_velocity;
+        profile_.MoveCurrentState(current);
+      }
       break;
 
     case RUNNING:
@@ -177,8 +187,25 @@
       // Update state_.
       UpdateZeroingState();
       if (unsafe_goal) {
-        claw_goal_ = unsafe_goal->angle;
-        claw_goal_velocity = unsafe_goal->angular_velocity;
+        // Pick a set of sane defaults if none are specified.
+        if (unsafe_goal->max_velocity != 0.0) {
+          profile_.set_maximum_velocity(unsafe_goal->max_velocity);
+        } else {
+          profile_.set_maximum_velocity(2.5);
+        }
+        if (unsafe_goal->max_acceleration != 0.0) {
+          profile_.set_maximum_acceleration(unsafe_goal->max_acceleration);
+        } else {
+          profile_.set_maximum_acceleration(4.0);
+        }
+
+        const double unfiltered_goal = ::std::max(
+            ::std::min(unsafe_goal->angle, values.claw.wrist.upper_limit),
+            values.claw.wrist.lower_limit);
+        ::Eigen::Matrix<double, 2, 1> goal_state =
+            profile_.Update(unfiltered_goal, unsafe_goal->angular_velocity);
+        claw_goal_ = goal_state(0, 0);
+        claw_goal_velocity = goal_state(1, 0);
       }
 
       if (state_ != RUNNING && state_ != ESTOP) {
@@ -249,12 +276,15 @@
   zeroing::PopulateEstimatorState(claw_estimator_, &status->zeroing_state);
 
   status->angle = claw_loop_->X_hat(0, 0);
+  status->angular_velocity = claw_loop_->X_hat(1, 0);
+
   if (output) {
     status->intake = output->intake_voltage;
   } else {
     status->intake = 0;
   }
   status->goal_angle = claw_goal_;
+  status->goal_velocity = claw_goal_velocity;
 
   if (output) {
     status->rollers_open =