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;
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index a23b167..2743bad 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -134,7 +134,6 @@
STATE_LOADING_PROBLEM = 4,
STATE_PREPARE_SHOT = 5,
STATE_READY = 6,
- STATE_PREPARE_FIRE = 7,
STATE_FIRE = 8,
STATE_UNLOAD = 9,
STATE_UNLOAD_MOVE = 10,
@@ -181,14 +180,10 @@
// wait for brake to set
Time shooter_brake_set_time_;
-
+
// The timeout for unloading.
Time unload_timeout_;
- // we are attempting to take up some of the backlash
- // in the gears before the plunger hits
- Time prepare_fire_end_time_;
-
// time that shot must have completed
Time shot_end_time_;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index c171912..99ecb78 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -186,10 +186,6 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
- if (GetAbsolutePosition() > 0.01) {
- EXPECT_GE(last_voltage_, 1)
- << ": Must preload the gearbox when firing.";
- }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -397,16 +393,12 @@
.shot_requested(true)
.Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
@@ -423,7 +415,6 @@
shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
@@ -439,16 +430,12 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
@@ -462,7 +449,6 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}