Removed SHOOTER_PREPARE_FIRE state (no longer try to remove backlash).
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index f896f17..11a11b9 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -5,7 +5,6 @@
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
@@ -111,7 +110,6 @@
load_timeout_(0, 0),
shooter_brake_set_time_(0, 0),
unload_timeout_(0, 0),
- prepare_fire_end_time_(0, 0),
shot_end_time_(0, 0),
cycles_not_moved_(0) {}
@@ -176,10 +174,6 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- // Adds voltage to take up slack in gears before shot.
- bool apply_some_voltage = false;
-
-
const bool disabled = !::aos::robot_state->enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -410,9 +404,9 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ firing_starting_position_ = shooter_.absolute_position();
+ state_ = STATE_FIRE;
}
}
if (state_ == STATE_READY &&
@@ -436,29 +430,6 @@
}
break;
- case STATE_PREPARE_FIRE:
- // Apply a bit of voltage to bias the gears for a little bit of time, and
- // then fire.
- shooter_loop_disable = true;
- if (disabled) {
- // If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- break;
- }
- if (Time::Now() > prepare_fire_end_time_) {
- cycles_not_moved_ = 0;
- firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + kShotEndTimeout;
- state_ = STATE_FIRE;
- latch_piston_ = false;
- } else {
- apply_some_voltage = true;
- latch_piston_ = true;
- }
-
- brake_piston_ = true;
- break;
-
case STATE_FIRE:
if (disabled) {
if (position) {
@@ -562,7 +533,7 @@
if (goal->load_requested) {
state_ = STATE_REQUEST_LOAD;
}
- // If we are ready to load again,
+ // If we are ready to load again,
shooter_loop_disable = true;
latch_piston_ = false;
@@ -577,11 +548,7 @@
break;
}
- if (apply_some_voltage) {
- shooter_.Update(true);
- shooter_.ZeroPower();
- if (output) output->voltage = 2.0;
- } else if (!shooter_loop_disable) {
+ if (!shooter_loop_disable) {
LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
shooter_.goal_position(), shooter_.absolute_position());
if (!cap_goal) {
@@ -611,9 +578,9 @@
LOG(DEBUG, "pos > absolute: %f velocity: %f state= %d l= %d pp= %d, pd= %d "
"p= %d b=%d\n",
shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_, position->latch, position->pusher_proximal.current,
- position->pusher_distal.current,
- position->plunger, brake_piston_);
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current,
+ position->plunger, brake_piston_);
}
if (position) {
last_distal_posedge_count_ = position->pusher_distal.posedge_count;