commit f735b24061c65bc6cb8c2d99f63e76274ac85ad7
author Brian Silverman <brians> Sat Mar 21 00:00:02 2015 -0700
committer Brian Silverman <brians> Fri Mar 27 22:53:20 2015 -0400
tree e6bf1fb71509f44704f96535adb2221e996ac602
parent 654b5127c1c2050829f398332d2206504565caa5

speed up score_action

Change-Id: I6aaba6c946f0c5f7a560bf694075093dc76ab88b

frc971/actors/score_actor.cc

diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index baa7710..6294f4c 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -7,6 +7,7 @@
 #include "aos/common/util/phased_loop.h"
 #include "frc971/constants.h"
 #include "frc971/control_loops/fridge/fridge.q.h"
+#include "aos/common/logging/queue_logging.h"
 
 using ::frc971::control_loops::fridge_queue;
 
@@ -14,10 +15,19 @@
 namespace actors {
 namespace {
 
-const double kMaxXVelocity = 0.45;
-const double kMaxYVelocity = 0.20;
+const double kSlowMaxXVelocity = 0.60;
+const double kSlowMaxYVelocity = 0.25;
+const double kFastMaxXVelocity = 0.80;
+const double kFastMaxYVelocity = 0.30;
+const double kReallyFastMaxXVelocity = 1;
+const double kReallyFastMaxYVelocity = 0.30;
+
 const double kMaxXAcceleration = 0.5;
 const double kMaxYAcceleration = 0.5;
+const double kFastMaxXAcceleration = 1.2;
+const double kFastMaxYAcceleration = 1.2;
+const double kSlowMaxXAcceleration = 0.3;
+const double kSlowMaxYAcceleration = 0.5;
 
 }  // namespace
 
@@ -47,7 +57,8 @@
 
 bool ScoreActor::MoveStackIntoPosition(const ScoreParams& params) {
   // Move the fridge up a little bit.
-  if (!SendGoal(0.0, params.upper_move_height, true)) {
+  if (!SendGoal(0.0, params.upper_move_height, true, kSlowMaxXVelocity,
+                kSlowMaxYVelocity, kMaxXAcceleration, kMaxYAcceleration)) {
     LOG(ERROR, "Sending fridge message failed.\n");
     return false;
   }
@@ -60,13 +71,16 @@
 
     // Move on when it is clear of the tote knobs.
     if (CurrentGoalHeight() > params.begin_horizontal_move_height) {
+      LOG(INFO, "moving on to horizontal move\n");
       break;
     }
   }
 
   // Move the fridge out.
   if (!SendGoal(params.horizontal_move_target,
-                params.begin_horizontal_move_height, true)) {
+                params.begin_horizontal_move_height, true, kSlowMaxXVelocity,
+                kSlowMaxYVelocity, kSlowMaxXAcceleration,
+                kSlowMaxYAcceleration)) {
     LOG(ERROR, "Sending fridge message failed.\n");
     return false;
   }
@@ -91,7 +105,9 @@
 
 bool ScoreActor::PlaceTheStack(const ScoreParams& params) {
   // Once the fridge is way out, put it on the ground.
-  if (!SendGoal(params.horizontal_move_target, params.place_height, true)) {
+  if (!SendGoal(params.horizontal_move_target, params.place_height, true,
+                kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
+                kMaxYAcceleration)) {
     LOG(ERROR, "Sending fridge message failed.\n");
     return false;
   }
@@ -110,7 +126,9 @@
   if (ShouldCancel()) return true;
 
   // Release the grabbers.
-  if (!SendGoal(params.horizontal_move_target, params.place_height, false)) {
+  if (!SendGoal(params.horizontal_move_target, params.place_height, false,
+                kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
+                kMaxYAcceleration)) {
     LOG(ERROR, "Sending fridge message failed.\n");
     return false;
   }
@@ -118,7 +136,9 @@
   if (ShouldCancel()) return true;
 
   // Go back to the home position.
-  if (!SendGoal(0.0, params.home_return_height, false)) {
+  if (!SendGoal(0.0, params.home_return_height, false, kReallyFastMaxXVelocity,
+                kReallyFastMaxYVelocity, kFastMaxXAcceleration,
+                kFastMaxYAcceleration)) {
     LOG(ERROR, "Sending fridge message failed.\n");
     return false;
   }
@@ -180,20 +200,25 @@
   return results.fridge_x;
 }
 
-bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled) {
+bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled,
+                          double max_x_velocity, double max_y_velocity,
+                          double max_x_acceleration,
+                          double max_y_acceleration) {
   auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
   new_fridge_goal->x = x;
   new_fridge_goal->y = y;
   new_fridge_goal->profiling_type = 1;
   new_fridge_goal->angular_velocity = 0.0;
+  new_fridge_goal->velocity = 0.0;
   new_fridge_goal->grabbers.top_front = grabbers_enabled;
   new_fridge_goal->grabbers.top_back = grabbers_enabled;
   new_fridge_goal->grabbers.bottom_front = grabbers_enabled;
   new_fridge_goal->grabbers.bottom_back = grabbers_enabled;
-  new_fridge_goal->max_x_velocity = kMaxXVelocity;
-  new_fridge_goal->max_y_velocity = kMaxYVelocity;
-  new_fridge_goal->max_x_acceleration = kMaxXAcceleration;
-  new_fridge_goal->max_y_acceleration = kMaxYAcceleration;
+  new_fridge_goal->max_x_velocity = max_x_velocity;
+  new_fridge_goal->max_y_velocity = max_y_velocity;
+  new_fridge_goal->max_x_acceleration = max_x_acceleration;
+  new_fridge_goal->max_y_acceleration = max_y_acceleration;
+  LOG_STRUCT(INFO, "sending fridge goal", *new_fridge_goal);
 
   return new_fridge_goal.Send();
 }

frc971/actors/score_actor.h

diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
index a5677e5..c2d7868 100644
--- a/frc971/actors/score_actor.h
+++ b/frc971/actors/score_actor.h
@@ -22,7 +22,9 @@
   bool NearGoal(double x, double y);
   bool PlaceTheStack(const ScoreParams &params);
   bool MoveStackIntoPosition(const ScoreParams &params);
-  bool SendGoal(double x, double y, bool grabbers_enabled);
+  bool SendGoal(double x, double y, bool grabbers_enabled,
+                double max_x_velocity, double max_y_velocity,
+                double max_x_acceleration, double max_y_acceleration);
   double CurrentHeight();
   double CurrentGoalHeight();
   double CurrentX();