Filtered spurrious edges in shooter.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index c9bfa8f..8b9d427 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -112,7 +112,10 @@
unload_timeout_(0, 0),
shot_end_time_(0, 0),
cycles_not_moved_(0),
- shot_count_(0) {}
+ shot_count_(0),
+ zeroed_(false),
+ distal_posedge_validation_cycles_left_(0),
+ proximal_posedge_validation_cycles_left_(0) {}
double ShooterMotor::PowerToPosition(double power) {
const frc971::constants::Values &values = constants::GetValues();
@@ -244,6 +247,7 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
+ zeroed_ = false;
if (position->pusher_distal.current ||
position->pusher_proximal.current) {
// We started on the sensor, back up until we are found.
@@ -311,18 +315,46 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
+ // TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
if (position->pusher_proximal.posedge_count !=
last_proximal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
- shooter_.SetCalibration(position->pusher_proximal.posedge_value,
- values.shooter.pusher_proximal.upper_angle);
+ proximal_posedge_validation_cycles_left_ = 2;
}
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
if (position->pusher_distal.posedge_count !=
last_distal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using distal sensor\n");
- shooter_.SetCalibration(position->pusher_distal.posedge_value,
- values.shooter.pusher_distal.upper_angle);
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
}
// Latch if the plunger is far enough back to trigger the hall effect.
@@ -333,7 +365,11 @@
// way back as well.
if (position->plunger && position->latch &&
position->pusher_distal.current) {
- state_ = STATE_PREPARE_SHOT;
+ if (!zeroed_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ state_ = STATE_PREPARE_SHOT;
+ }
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
@@ -356,7 +392,8 @@
break;
case STATE_LOADING_PROBLEM:
if (disabled) {
- Load();
+ state_ = STATE_REQUEST_LOAD;
+ break;
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.