commit a6d77542317adc838e4da8f7231d0fc568b1bcd6
author Ben Fredrickson <ben.fred12345@gmail.com> Mon Feb 17 07:54:43 2014 +0000
committer Ben Fredrickson <ben.fred12345@gmail.com> Mon Feb 17 07:54:43 2014 +0000
tree cb985b19994b967dd643c69ca8d808b8786b6889
parent 22c9332c9f831b17f5aa071dd23840444c98ee95

Fire is not quite working, I believe the problem is in the simulation. the plunger effect is not set in STATE_LOADING_PROBLEM.

frc971/control_loops/shooter/shooter.cc

diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index f951e06..831ef19 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -183,7 +183,7 @@
       if (position->plunger_back_hall_effect && position->latch_hall_effect) {
         state_ = STATE_PREPARE_SHOT;
       } else if (position->plunger_back_hall_effect &&
-                 abs(adjusted_position - PowerToPosition(goal->shot_power)) <
+                 fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
                  0.05) {
         state_ = STATE_LOADING_PROBLEM;
         loading_problem_end_time_ =
@@ -207,12 +207,14 @@
       break;
     case STATE_PREPARE_SHOT:
       shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
-      if (abs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.05) {
+      LOG(DEBUG, "PDIFF: adjusted_position: %.2f, pow: %.2f\n",
+          adjusted_position, PowerToPosition(goal->shot_power));
+      if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.05) {
         state_ = STATE_READY;
         output->latch_piston = true;
         output->brake_piston = true;
         shooter_brake_set_time_ =
-            Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
+            Time::Now(Time::kDefaultClock) + Time::InSeconds(0.03);
       } else {
         output->latch_piston = true;
         output->brake_piston = false;
@@ -222,15 +224,19 @@
       if (Time::Now() > shooter_brake_set_time_) {
         shooter_loop_disable = true;
         if (goal->unload_requested) {
+			printf("GHA\n");
           state_ = STATE_UNLOAD;
-        } else if (abs(adjusted_position - PowerToPosition(goal->shot_power)) >
+        } else if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) >
                    0.05) {
+			printf("GHB\n");
           state_ = STATE_PREPARE_SHOT;
         } else if (goal->shot_requested) {
+			printf("GHC\n");
           state_ = STATE_REQUEST_FIRE;
         }
-
       }
+      shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+
       output->latch_piston = true;
       output->brake_piston = true;
       break;
@@ -262,7 +268,7 @@
     case STATE_FIRE:
       shooter_loop_disable = true;
       //TODO_ben: need approamately equal
-      if (abs(last_position_.position - adjusted_position) < 0.07) {
+      if (fabs(last_position_.position - adjusted_position) < 0.07) {
         cycles_not_moved_++;
       } else {
         cycles_not_moved_ = 0;
@@ -323,7 +329,7 @@
   }
 
   status->done =
-      ::std::abs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
+      ::std::fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
 }
 
 }  // namespace control_loops