Worked through the shooter loop and fixed a bunch of bugs. Switched the internal state in the loop to use the 0 point of the spring, fixed problems with the shooter, and pushed calibratoin into the loop. Lots better now.
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index ea3aab8..c5d795c 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -154,6 +154,7 @@
else:
unaug_loop_writer.Write(argv[3], argv[4])
+ shooter = ShooterDeltaU()
loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 08138ff..c22b899 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -42,21 +42,47 @@
U(0, 0) = voltage_ - old_voltage;
//LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
//LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+ LOG(DEBUG, "Voltage sums up by %f\n", U(0, 0));
last_voltage_ = voltage_;
}
+void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
+ double known_position) {
+ LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
+ known_position);
+ LOG(INFO, "Position was %f\n", absolute_position());
+ double previous_offset = offset_;
+ offset_ = known_position - encoder_val;
+ double doffset = offset_ - previous_offset;
+ LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
+ X_hat(0, 0) += doffset;
+ // Offset our measurements because the offset is baked into them.
+ Y_(0, 0) += doffset;
+ // Offset the goal so we don't move.
+ R(0, 0) += doffset;
+ LOG(INFO, "Validation: position is %f\n", absolute_position());
+}
+
ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
: aos::control_loops::ControlLoop<control_loops::ShooterGroup>(my_shooter),
shooter_(MakeShooterLoop()),
- calibration_position_(0.0),
state_(STATE_INITIALIZE),
loading_problem_end_time_(0, 0),
+ 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),
- initial_loop_(true) {}
+ cycles_not_moved_(0) {}
+
+double ShooterMotor::PowerToPosition(double power) {
+ // LOG(WARNING, "power to position not correctly implemented\n");
+ const frc971::constants::Values &values = constants::GetValues();
+ double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ values.shooter.upper_limit);
+ return new_pos;
+}
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
@@ -73,12 +99,11 @@
return;
}
- if (initial_loop_) {
- // TODO(austin): If 'reset()', we are lost, start over.
- initial_loop_ = false;
- shooter_.SetPositionDirectly(position->position);
- } else {
- shooter_.SetPositionValues(position->position);
+ if (reset()) {
+ state_ = STATE_INITIALIZE;
+ }
+ if (position) {
+ shooter_.CorrectPosition(position->position);
}
// Disable the motors now so that all early returns will return with the
@@ -87,16 +112,8 @@
const frc971::constants::Values &values = constants::GetValues();
- double real_position = shooter_.position();
- double adjusted_position = shooter_.position() - calibration_position_;
-
- if (position) {
- last_position_ = *position;
- LOG(DEBUG,
- "pos > real: %.2f adjusted: %.2f raw: %.2f calib: %.2f state= %d\n",
- real_position, adjusted_position, position->position,
- calibration_position_, state_);
- }
+ double relative_position = shooter_.position();
+ double absolute_position = shooter_.absolute_position();
// Don't even let the control loops run.
bool shooter_loop_disable = false;
@@ -104,217 +121,357 @@
// Adds voltage to take up slack in gears before shot.
bool apply_some_voltage = false;
+ // TODO(austin): Observe not seeing the sensors when we should by moving the
+ // calibration offset correclty.
+ const bool disabled = !::aos::robot_state->enabled;
+
switch (state_) {
case STATE_INITIALIZE:
- // Start off with the assumption that we are at the value
- // futhest back given our sensors.
- if (position && position->pusher_distal.current) {
- //TODO(ben): use posedge
- calibration_position_ =
- position->position - values.shooter.pusher_distal.lower_angle;
- } else if (position && position->pusher_proximal.current) {
- //TODO(ben): use posedge
- calibration_position_ =
- position->position - values.shooter.pusher_proximal.lower_angle;
- }
-
- state_ = STATE_REQUEST_LOAD;
-
- // Zero out initial goal.
- shooter_.SetGoalPosition(real_position, 0.0);
if (position) {
- output->latch_piston = position->plunger;
+ // Reinitialize the internal filter state.
+ shooter_.InitializeState(position->position);
+ // TODO(austin): Test all of these initial states.
+
+ // Start off with the assumption that we are at the value
+ // futhest back given our sensors.
+ if (position->pusher_distal.current) {
+ shooter_.SetCalibration(position->position,
+ values.shooter.pusher_distal.lower_angle);
+ } else if (position->pusher_proximal.current) {
+ shooter_.SetCalibration(position->position,
+ values.shooter.pusher_proximal.lower_angle);
+ } else {
+ shooter_.SetCalibration(position->position,
+ values.shooter.upper_limit);
+ }
+
+ state_ = STATE_REQUEST_LOAD;
+
+ // Go to the current position.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ // If the plunger is all the way back, we want to be latched.
+ latch_piston_ = position->plunger;
+ brake_piston_ = false;
} else {
- // We don't know what is going on so just close the latch to be safe.
- output->latch_piston = true;
+ // If we can't start yet because we don't know where we are, set the
+ // latch and brake to their defaults.
+ latch_piston_ = true;
+ brake_piston_ = true;
}
- output->brake_piston = false;
break;
case STATE_REQUEST_LOAD:
- if (position->plunger && position->latch) {
- // Already latched.
- state_ = STATE_PREPARE_SHOT;
- } else if (position->pusher_distal.current || (adjusted_position) < 0) {
- state_ = STATE_LOAD_BACKTRACK;
- // TODO(ben): double check that rezero is the right thing to do here
- if (position) {
- calibration_position_ = position->position;
+ if (position) {
+ // Need to go forward a little if we are starting with the
+ // back_distal_sensor triggered
+ if (position->plunger && position->latch) {
+ // The plunger is back and we are latched, get ready to shoot.
+ state_ = STATE_PREPARE_SHOT;
+ latch_piston_ = true;
+ } else if (position->pusher_distal.current) {
+ // We started on the sensor, back up until we are found.
+ // If the plunger is all the way back and not latched, it won't be
+ // there for long.
+ state_ = STATE_LOAD_BACKTRACK;
+ latch_piston_ = false;
+ } else {
+ // Off the sensor, start loading.
+ Load();
+ latch_piston_ = false;
}
- } else {
- state_ = STATE_LOAD;
}
- shooter_.SetGoalPosition(0.0, 0.0);
- if (position && output) {
- output->latch_piston = position->plunger;
- }
- if (output) {
- output->brake_piston = false;
- }
+ // Hold our current position.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ brake_piston_ = false;
break;
case STATE_LOAD_BACKTRACK:
- if (adjusted_position > values.shooter.pusher_distal.upper_angle + 0.01) {
+ // If we are here, then that means we started past the edge where we want
+ // to zero. Move backwards until we don't see the sensor anymore.
+ // The plunger is contacting the pusher (or will be shortly).
+
+ // TODO(austin): Windup here and below!
+ if (!disabled) {
shooter_.SetGoalPosition(
- real_position - values.shooter.zeroing_speed * dt,
- values.shooter.zeroing_speed);
- } else {
- state_ = STATE_LOAD;
+ shooter_.goal_position() - values.shooter.zeroing_speed * dt,
+ -values.shooter.zeroing_speed);
}
- if (output) output->latch_piston = false;
- if (output) output->brake_piston = true;
+ if (position) {
+ if (!position->pusher_distal.current) {
+ Load();
+ }
+ }
+
+ latch_piston_ = false;
+ brake_piston_ = false;
break;
case STATE_LOAD:
- if (position && position->pusher_proximal.current &&
- !last_position_.pusher_proximal.current) {
- //TODO(ben): use posedge
- calibration_position_ =
- position->position - values.shooter.pusher_proximal.upper_angle;
+ // If we are disabled right now, reset the timer.
+ if (disabled) {
+ Load();
+ // Latch defaults to true when disabled. Leave it latched until we have
+ // useful sensor data.
+ latch_piston_ = true;
}
- if (position && position->pusher_distal.current &&
- !last_position_.pusher_distal.current) {
- //TODO(ben): use posedge
- calibration_position_ =
- position->position - values.shooter.pusher_distal.lower_angle;
- }
+ // Go to 0, which should be the latch position, or trigger a hall effect
+ // on the way. If we don't see edges where we are supposed to, the
+ // offset will be updated by code above.
+ shooter_.SetGoalPosition(0.0, 0.0);
- shooter_.SetGoalPosition(calibration_position_, 0.0);
- if (position && output) {
- output->latch_piston = position->plunger;
- }
+ if (position) {
+ // 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);
+ }
+ 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);
+ }
- if (position->plunger && position->latch) {
- state_ = STATE_PREPARE_SHOT;
- } else if (position->plunger &&
- fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
- 0.05) {
- state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
+ // Latch if the plunger is far enough back to trigger the hall effect.
+ // This happens when the distal sensor is triggered.
+ latch_piston_ = position->pusher_distal.current;
+
+ // Check if we are latched and back.
+ if (position->plunger && position->latch) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (position->plunger &&
+ ::std::abs(shooter_.absolute_position() -
+ shooter_.goal_position()) < 0.001) {
+ // We are at the goal, but not latched.
+ state_ = STATE_LOADING_PROBLEM;
+ loading_problem_end_time_ = Time::Now() + Time::InSeconds(0.5);
+ }
}
- if (output) output->brake_piston = false;
+ if (load_timeout_ < Time::Now()) {
+ if (position) {
+ if (!position->pusher_distal.current ||
+ !position->pusher_proximal.current) {
+ state_ = STATE_ESTOP;
+ }
+ }
+ } else if (goal->unload_requested) {
+ Unload();
+ }
+ brake_piston_ = false;
break;
case STATE_LOADING_PROBLEM:
+ if (disabled) {
+ Load();
+ }
+ // 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.
if (Time::Now() > loading_problem_end_time_) {
- state_ = STATE_UNLOAD;
- } else if (position->plunger && position->latch) {
+ // Timeout by unloading.
+ Unload();
+ } else if (position && position->plunger && position->latch) {
+ // If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
}
- shooter_.SetGoalPosition(calibration_position_, 0.0);
+ // Move a bit further back to help it trigger.
+ // If the latch is slow due to the air flowing through the tubes or
+ // inertia, but is otherwise free, this won't have much time to do
+ // anything and is safe. Otherwise this gives us a bit more room to free
+ // up the latch.
+ shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
position->plunger, position->latch);
- if (output) output->latch_piston = true;
- if (output) output->brake_piston = false;
+ latch_piston_ = true;
+ brake_piston_ = false;
break;
case STATE_PREPARE_SHOT:
+ // Move the shooter to the shot power set point and then lock the brake.
+ // TODO(austin): Timeout. Low priority.
+
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
- 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) {
+
+ LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
+ absolute_position, PowerToPosition(goal->shot_power));
+ // TODO(austin): Goal velocity too...
+ if (::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) < 0.001) {
+ // We are there, set the brake and move on.
+ latch_piston_ = true;
+ brake_piston_ = true;
+ shooter_brake_set_time_ = Time::Now() + Time::InSeconds(0.05);
state_ = STATE_READY;
- output->latch_piston = true;
- output->brake_piston = true;
- shooter_brake_set_time_ =
- Time::Now(Time::kDefaultClock) + Time::InSeconds(0.03);
} else {
- output->latch_piston = true;
- output->brake_piston = false;
+ latch_piston_ = true;
+ brake_piston_ = false;
+ }
+ if (goal->unload_requested) {
+ Unload();
}
break;
case STATE_READY:
+ LOG(DEBUG, "In ready\n");
+ // Wait until the brake is set, and a shot is requested or the shot power is changed.
if (Time::Now() > shooter_brake_set_time_) {
+ // We have waited long enough for the brake to set, turn the shooter control loop off.
shooter_loop_disable = true;
- if (goal->unload_requested) {
- printf("GHA\n");
- state_ = STATE_UNLOAD;
- } 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;
+ LOG(DEBUG, "Brake is now set\n");
+ if (goal->shot_requested && !disabled) {
+ LOG(DEBUG, "Shooting now\n");
+ shooter_loop_disable = true;
+ prepare_fire_end_time_ =
+ Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ apply_some_voltage = true;
+ state_ = STATE_PREPARE_FIRE;
}
+ } else if (::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.001) {
+ // TODO(austin): If the goal has changed, go back to prepare shot, not if we are off.
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ } else {
+ LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
}
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
- output->latch_piston = true;
- output->brake_piston = true;
+ latch_piston_ = true;
+ brake_piston_ = true;
+
+ if (goal->unload_requested) {
+ Unload();
+ }
break;
- case STATE_REQUEST_FIRE:
+
+ 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 (position->plunger) {
+ if (disabled) {
+ // If we are disabled, reset the backlash bias timer.
prepare_fire_end_time_ =
Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
- } else {
- state_ = STATE_REQUEST_LOAD;
+ break;
}
- break;
- case STATE_PREPARE_FIRE:
- shooter_loop_disable = true;
- if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
- apply_some_voltage = true;
- } else {
+ if (Time::Now() > prepare_fire_end_time_) {
+ cycles_not_moved_ = 0;
+ firing_starting_position_ = shooter_.absolute_position();
+ shot_end_time_ = Time::Now() + Time::InSeconds(1);
state_ = STATE_FIRE;
- cycles_not_moved_ = 0;
- shot_end_time_ = Time::Now(Time::kDefaultClock) + Time::InMS(500);
+ } else {
+ apply_some_voltage = true;
}
- output->latch_piston = true;
- output->brake_piston = true;
-
+ latch_piston_ = true;
+ brake_piston_ = true;
break;
+
case STATE_FIRE:
+ if (disabled) {
+ if (position) {
+ if (position->plunger) {
+ // If disabled and the plunger is still back there, reset the
+ // timeout.
+ shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ }
+ }
+ }
shooter_loop_disable = true;
- //TODO(ben): need approamately equal
- if (fabs(last_position_.position - adjusted_position) < 0.07) {
- cycles_not_moved_++;
+ // Count the number of contiguous cycles during which we haven't moved.
+ if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+ 0.0005) {
+ ++cycles_not_moved_;
} else {
cycles_not_moved_ = 0;
}
- if ((adjusted_position < 0.10 && cycles_not_moved_ > 5) ||
- Time::Now(Time::kDefaultClock) > shot_end_time_) {
+
+ // If we have moved any amount since the start and the shooter has now
+ // been still for a couple cycles, the shot finished.
+ // Also move on if it times out.
+ if ((::std::abs(firing_starting_position_ -
+ shooter_.absolute_position()) > 0.0005 &&
+ cycles_not_moved_ > 3) ||
+ Time::Now() > shot_end_time_) {
state_ = STATE_REQUEST_LOAD;
}
- output->latch_piston = true;
- output->brake_piston = true;
+ latch_piston_ = false;
+ brake_piston_ = true;
break;
case STATE_UNLOAD:
+ // Reset the timeouts.
+ if (disabled) Unload();
+
+ // If it is latched and the plunger is back, move the pusher back to catch
+ // the plunger.
if (position->plunger && position->latch) {
- shooter_.SetGoalPosition(0.02, 0.0);
- if (adjusted_position < 0.04) {
- output->latch_piston = false;
+ // Pull back to 0, 0.
+ shooter_.SetGoalPosition(0.0, 0.0);
+ if (shooter_.absolute_position() < 0.005) {
+ // When we are close enough, 'fire'.
+ latch_piston_ = false;
+ } else {
+ latch_piston_ = true;
}
} else {
- output->latch_piston = false;
+ // The plunger isn't all the way back, or it is and it is unlatched, so
+ // we can now unload.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
+ unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
}
- output->brake_piston = false;
+ if (Time::Now() > unload_timeout_) {
+ // We have been stuck trying to unload for way too long, give up and
+ // turn everything off.
+ state_ = STATE_ESTOP;
+ }
+
+ brake_piston_ = false;
break;
- case STATE_UNLOAD_MOVE:
- if (adjusted_position > values.shooter.upper_limit - 0.03) {
- shooter_.SetGoalPosition(real_position, 0.0);
+ case STATE_UNLOAD_MOVE: {
+ if (disabled) {
+ unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ }
+ // TODO(austin): Windup...
+
+ // Slowly move back until we hit the upper limit.
+ double goal_position =
+ shooter_.goal_position() + values.shooter.zeroing_speed * dt;
+ // If during this cycle, we would move past the limit, we are done
+ // unloading.
+ if (goal_position > values.shooter.upper_limit) {
+ shooter_.SetGoalPosition(values.shooter.upper_limit, 0.0);
+ // We don't want the loop fighting the spring when we are unloaded.
+ // Turn it off.
+ shooter_loop_disable = true;
state_ = STATE_READY_UNLOAD;
} else {
- shooter_.SetGoalPosition(
- real_position + values.shooter.zeroing_speed * dt,
- values.shooter.zeroing_speed);
+ shooter_.SetGoalPosition(goal_position, values.shooter.zeroing_speed);
}
- output->latch_piston = false;
- output->brake_piston = false;
- break;
+ latch_piston_ = false;
+ brake_piston_ = false;
+ } break;
case STATE_READY_UNLOAD:
- if (!goal->unload_requested) {
+ if (goal->load_requested) {
state_ = STATE_REQUEST_LOAD;
}
+ // If we are ready to load again,
+ shooter_loop_disable = true;
- output->latch_piston = false;
- output->brake_piston = false;
+ latch_piston_ = false;
+ brake_piston_ = false;
+ break;
+
+ case STATE_ESTOP:
+ // Totally lost, go to a safe state.
+ shooter_loop_disable = true;
+ latch_piston_ = true;
+ brake_piston_ = true;
break;
}
@@ -322,7 +479,8 @@
shooter_.Update(true);
if (output) output->voltage = 2.0;
} else if (!shooter_loop_disable) {
- LOG(DEBUG, "Running the loop, goal is %f\n", shooter_.R(0, 0));
+ LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
+ shooter_.goal_position(), shooter_.absolute_position());
shooter_.Update(output == NULL);
if (output) output->voltage = shooter_.voltage();
} else {
@@ -330,8 +488,25 @@
if (output) output->voltage = 0.0;
}
+ if (output) {
+ output->latch_piston = latch_piston_;
+ output->brake_piston = brake_piston_;
+ }
+
status->done =
- ::std::fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
+ ::std::abs(absolute_position - PowerToPosition(goal->shot_power)) < 0.004;
+
+ if (position) {
+ last_position_ = *position;
+ LOG(DEBUG,
+ "pos > real: %.2f adjusted: %.2f raw: %.2f state= %d\n",
+ relative_position, absolute_position, position->position,
+ state_);
+ }
+ if (position) {
+ last_distal_posedge_count_ = position->pusher_distal.posedge_count;
+ last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ }
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 89109dd..7e99bfc 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -36,9 +36,7 @@
voltage_(0.0),
last_voltage_(0.0),
uncapped_voltage_(0.0),
- offset_(0.0),
- encoder_(0.0),
- last_encoder_(0.0) {}
+ offset_(0.0) {}
const static int kZeroingMaxVoltage = 5;
@@ -54,78 +52,46 @@
// Zeros the accumulator.
void ZeroPower() { voltage_ = 0.0; }
- enum JointZeroingState {
- // We don't know where the joint is at all.
- UNKNOWN_POSITION,
- // Ready for use during teleop.
- CALIBRATED
- };
-
- void set_zeroing_state(JointZeroingState zeroing_state) {
- zeroing_state_ = zeroing_state;
- }
-
- JointZeroingState zeroing_state() const { return zeroing_state_; }
-
// Sets the calibration offset given the absolute angle and the corrisponding
// encoder value.
- void SetCalibration(double encoder_val, double known_position) {
- offset_ = known_position - encoder_val;
- }
+ void SetCalibration(double encoder_val, double known_position);
- bool SetCalibrationOnEdge(const constants::Values::Shooter &shooter_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double known_position;
- if (GetPositionOfEdge(shooter_values, &edge_encoder, &known_position)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, known_position);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ double offset() const { return offset_; }
- void SetPositionDirectly(double position) { X_hat(0, 0) = position; }
+ double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
- void SetPositionValues(double position) {
+ void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
- Y << position;
+ Y << position + offset_ - kPositionOffset;
LOG(DEBUG, "Setting position to %f\n", position);
Correct(Y);
}
- void SetGoalPosition(double desired_position, double desired_velocity) {
- // austin said something about which matrix to set, but I didn't under
- // very much of it
- //some_matrix = {desired_position, desired_velocity};
- LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
- R << desired_position, desired_velocity, 0;
+ double goal_position() const { return R(0, 0) + kPositionOffset; }
+ double goal_velocity() const { return R(1, 0); }
+ void InitializeState(double position) {
+ X_hat(0, 0) = position - kPositionOffset;
}
- double position() const { return X_hat(0, 0); }
- double encoder() const { return encoder_; }
- double last_encoder() const { return last_encoder_; }
+ void SetGoalPosition(double desired_position, double desired_velocity) {
+ LOG(DEBUG, "Goal position: %.2f Goal velocity: %.2f\n", desired_position, desired_velocity);
- // Returns true if an edge was detected in the last cycle and then sets the
- // edge_position to the absolute position of the edge.
- bool GetPositionOfEdge(const constants::Values::Shooter &shooter,
- double *edge_encoder, double *known_position);
+ R << desired_position - kPositionOffset, desired_velocity,
+ -A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+ A(1, 1) / A(1, 2) * desired_velocity;
+ }
-#undef COUNT_SETTER_GETTER
+ double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
private:
+ // The offset between what is '0' (0 rate on the spring) and the 0 (all the
+ // way cocked).
+ constexpr static double kPositionOffset = 0.305054 + 0.0254;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
double uncapped_voltage_;
double offset_;
-
- double previous_position_;
-
- JointZeroingState zeroing_state_;
- double encoder_;
- double last_encoder_;
};
class ShooterMotor
@@ -137,17 +103,7 @@
// True if the goal was moved to avoid goal windup.
//bool capped_goal() const { return shooter_.capped_goal(); }
- double PowerToPosition(double power) {
- //LOG(WARNING, "power to position not correctly implemented\n");
- const frc971::constants::Values &values = constants::GetValues();
- double new_pos =
- (power > values.shooter.upper_limit) ? values.shooter.upper_limit
- : (power < 0.0)
- ? 0.0
- : power;
-
- return new_pos;
- }
+ double PowerToPosition(double power);
typedef enum {
STATE_INITIALIZE = 0,
@@ -157,20 +113,17 @@
STATE_LOADING_PROBLEM = 4,
STATE_PREPARE_SHOT = 5,
STATE_READY = 6,
- STATE_REQUEST_FIRE = 7,
- STATE_PREPARE_FIRE = 8,
- STATE_FIRE = 9,
- STATE_UNLOAD = 10,
- STATE_UNLOAD_MOVE = 11,
- STATE_READY_UNLOAD = 12
+ STATE_PREPARE_FIRE = 7,
+ STATE_FIRE = 8,
+ STATE_UNLOAD = 9,
+ STATE_UNLOAD_MOVE = 10,
+ STATE_READY_UNLOAD = 11,
+ STATE_ESTOP = 12
} State;
- State GetState() {return state_;}
-
- double GetPosition() { return shooter_.position() - calibration_position_; }
+ State state() { return state_; }
protected:
-
virtual void RunIteration(
const ShooterGroup::Goal *goal,
const control_loops::ShooterGroup::Position *position,
@@ -181,21 +134,35 @@
friend class testing::ShooterTest_NoWindupPositive_Test;
friend class testing::ShooterTest_NoWindupNegative_Test;
+ // Enter state STATE_UNLOAD
+ void Unload() {
+ state_ = STATE_UNLOAD;
+ unload_timeout_ = Time::Now() + Time::InSeconds(1);
+ }
+ // Enter state STATE_LOAD
+ void Load() {
+ state_ = STATE_LOAD;
+ load_timeout_ = Time::Now() + Time::InSeconds(1);
+ }
+
control_loops::ShooterGroup::Position last_position_;
ZeroedStateFeedbackLoop shooter_;
- // position need to zero
- double calibration_position_;
-
// state machine state
State state_;
// time to giving up on loading problem
Time loading_problem_end_time_;
+ // The end time when loading for it to timeout.
+ Time load_timeout_;
+
// 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
@@ -207,8 +174,13 @@
// track cycles that we are stuck to detect errors
int cycles_not_moved_;
- // setup on the intial loop may involve shortcuts
- bool initial_loop_;
+ double firing_starting_position_;
+
+ // True if the latch should be engaged and the brake should be engaged.
+ bool latch_piston_;
+ bool brake_piston_;
+ int32_t last_distal_posedge_count_;
+ int32_t last_proximal_posedge_count_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 830ab84..6310320 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -19,10 +19,14 @@
// Shoots as soon as this is true.
bool shot_requested;
bool unload_requested;
+ bool load_requested;
};
// Back is when the springs are all the way stretched.
message Position {
+ // In meters, out is positive.
+ double position;
+
// If the latch piston is fired and this hall effect has been triggered, the
// plunger is all the way back and latched.
bool plunger;
@@ -32,9 +36,6 @@
PosedgeOnlyCountedHallEffectStruct pusher_proximal;
// Triggers when the latch engages.
bool latch;
-
- // In meters, out is positive.
- double position;
};
message Status {
// Whether it's ready to shoot right now.
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index cad47af..2ea3afe 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -43,11 +43,15 @@
Reinitialize(initial_position);
}
+ // The difference between the position with 0 at the back, and the position
+ // with 0 measured where the spring has 0 force.
+ constexpr static double kPositionOffset = 0.3;
+
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
initial_position_ = initial_position;
- plant->X(0, 0) = initial_position_;
+ plant->X(0, 0) = initial_position_ - kPositionOffset;
plant->X(1, 0) = 0.0;
plant->Y = plant->C() * plant->X;
last_voltage_ = 0.0;
@@ -56,7 +60,9 @@
}
// Returns the absolute angle of the wrist.
- double GetAbsolutePosition() const { return shooter_plant_->Y(0, 0); }
+ double GetAbsolutePosition() const {
+ return shooter_plant_->Y(0, 0) + kPositionOffset;
+ }
// Returns the adjusted angle of the wrist.
double GetPosition() const {
@@ -72,18 +78,18 @@
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
const frc971::constants::Values &values = constants::GetValues();
- position->position = GetAbsolutePosition();
+ position->position = GetPosition();
- LOG(DEBUG, "Physical shooter at {%f}\n", position->position);
+ LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
position->plunger =
- CheckRange(position->position, values.shooter.plunger_back);
+ CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
position->pusher_distal.current =
- CheckRange(position->position, values.shooter.pusher_distal);
+ CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
position->pusher_proximal.current =
- CheckRange(position->position, values.shooter.pusher_proximal);
+ CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
}
void UpdateEffectEdge(
@@ -91,11 +97,18 @@
const PosedgeOnlyCountedHallEffectStruct &last_sensor,
const constants::Values::AnglePair &limits,
const control_loops::ShooterGroup::Position &last_position) {
+ sensor->posedge_count = last_sensor.posedge_count;
+ sensor->negedge_count = last_sensor.negedge_count;
+
+ sensor->posedge_value = last_sensor.posedge_value;
+
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
- if (last_position.position < limits.lower_angle) {
+ if (last_position.position + initial_position_ < limits.lower_angle) {
+ LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n", sensor->posedge_count);
sensor->posedge_value = limits.lower_angle - initial_position_;
} else {
+ LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n", sensor->posedge_count);
sensor->posedge_value = limits.upper_angle - initial_position_;
}
}
@@ -112,47 +125,17 @@
SetPhysicalSensors(position.get());
- // Handle latch hall effect
- if (!latch_piston_state_ && latch_delay_count_ > 0) {
- LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
- if (latch_delay_count_ == 1) {
- latch_piston_state_ = true;
- position->latch = true;
- }
- latch_delay_count_--;
- } else if (latch_piston_state_ && latch_delay_count_ < 0) {
- LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
- if (latch_delay_count_ == -1) {
- latch_piston_state_ = false;
- position->latch = false;
- }
- latch_delay_count_++;
- }
-
- // Handle brake internal state
- if (!brake_piston_state_ && brake_delay_count_ > 0) {
- if (brake_delay_count_ == 1) {
- brake_piston_state_ = true;
- }
- brake_delay_count_--;
- } else if (brake_piston_state_ && brake_delay_count_ < 0) {
- if (brake_delay_count_ == -1) {
- brake_piston_state_ = false;
- }
- brake_delay_count_++;
- }
+ position->latch = latch_piston_state_;
// Handle pusher distal hall effect
- UpdateEffectEdge(
- &position->pusher_distal, last_position_message_.pusher_distal,
- values.shooter.pusher_distal, last_position_message_);
- LOG(INFO, "seteffect: pusher distal: %d\n", position->plunger);
+ UpdateEffectEdge(&position->pusher_distal,
+ last_position_message_.pusher_distal,
+ values.shooter.pusher_distal, last_position_message_);
// Handle pusher proximal hall effect
- UpdateEffectEdge(
- &position->pusher_proximal, last_position_message_.pusher_proximal,
- values.shooter.pusher_proximal, last_position_message_);
- LOG(INFO, "seteffect: pusher proximal: %d\n", position->plunger);
+ UpdateEffectEdge(&position->pusher_proximal,
+ last_position_message_.pusher_proximal,
+ values.shooter.pusher_proximal, last_position_message_);
last_position_message_ = *position;
position.Send();
@@ -160,10 +143,8 @@
// Simulates the claw moving for one timestep.
void Simulate() {
- last_plant_position_ = shooter_plant_->Y(0, 0);
+ last_plant_position_ = GetAbsolutePosition();
EXPECT_TRUE(shooter_queue_group_.output.FetchLatest());
- shooter_plant_->U << last_voltage_;
- shooter_plant_->Update();
if (shooter_queue_group_.output->latch_piston && !latch_piston_state_ &&
latch_delay_count_ == 0) {
latch_delay_count_ = 6;
@@ -180,11 +161,55 @@
brake_delay_count_ = -5;
}
- EXPECT_GE(constants::GetValues().shooter.upper_limit,
- shooter_plant_->Y(0, 0));
- // we okay within a millimeter
- EXPECT_LE(constants::GetValues().shooter.lower_limit - 0.001,
- shooter_plant_->Y(0, 0));
+ // Handle brake internal state
+ if (!brake_piston_state_ && brake_delay_count_ > 0) {
+ if (brake_delay_count_ == 1) {
+ brake_piston_state_ = true;
+ }
+ brake_delay_count_--;
+ } else if (brake_piston_state_ && brake_delay_count_ < 0) {
+ if (brake_delay_count_ == -1) {
+ brake_piston_state_ = false;
+ }
+ brake_delay_count_++;
+ }
+
+ if (brake_piston_state_) {
+ shooter_plant_->U << 0.0;
+ shooter_plant_->X(1, 0) = 0.0;
+ shooter_plant_->Y = shooter_plant_->C() * shooter_plant_->X +
+ shooter_plant_->D() * shooter_plant_->U;
+ } else {
+ shooter_plant_->U << last_voltage_;
+ shooter_plant_->Update();
+ }
+
+ // Handle latch hall effect
+ if (!latch_piston_state_ && latch_delay_count_ > 0) {
+ LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
+ if (latch_delay_count_ == 1) {
+ latch_piston_state_ = true;
+ EXPECT_GE(constants::GetValues().shooter.latch_max_safe_position,
+ GetAbsolutePosition());
+ }
+ latch_delay_count_--;
+ } else if (latch_piston_state_ && latch_delay_count_ < 0) {
+ LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
+ if (latch_delay_count_ == -1) {
+ latch_piston_state_ = false;
+ EXPECT_TRUE(brake_piston_state_)
+ << ": Must have the brake set when releasing the latch.";
+ // TODO(austin): The brake should be set for a number of cycles after
+ // this as well.
+ }
+ latch_delay_count_++;
+ }
+
+ EXPECT_GE(constants::GetValues().shooter.upper_hard_limit,
+ GetAbsolutePosition());
+ EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
+ GetAbsolutePosition());
+
last_voltage_ = shooter_queue_group_.output->voltage;
::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
@@ -232,7 +257,7 @@
".frc971.control_loops.shooter_queue_group.output",
".frc971.control_loops.shooter_queue_group.status"),
shooter_motor_(&shooter_queue_group_),
- shooter_motor_plant_(0.5) {
+ shooter_motor_plant_(0.2) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -271,20 +296,20 @@
TEST_F(ShooterTest, PowerConversion) {
// test a couple of values return the right thing
EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
- EXPECT_EQ(0.475, shooter_motor_.PowerToPosition(0.475));
+ EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
const frc971::constants::Values &values = constants::GetValues();
// value too large should get max
EXPECT_EQ(values.shooter.upper_limit,
shooter_motor_.PowerToPosition(505050.99));
// negative values should zero
- EXPECT_EQ(0.0, shooter_motor_.PowerToPosition(-123.4));
+ EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
- for (int i = 0; i < 100; ++i) {
+ for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -293,9 +318,8 @@
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
- EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
-
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
@@ -305,10 +329,9 @@
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
- EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
- bool hit_requestfire = false;
bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 100; ++i) {
@@ -316,26 +339,24 @@
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- printf("MOTORSTATE = %d\n", shooter_motor_.GetState());
- if (shooter_motor_.GetState() == ShooterMotor::STATE_REQUEST_FIRE) {
- hit_requestfire = true;
- }
- if (shooter_motor_.GetState() == ShooterMotor::STATE_PREPARE_FIRE) {
+ LOG(DEBUG, "MOTORSTATE = %d\n", shooter_motor_.state());
+ if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
hit_preparefire = true;
}
- if (shooter_motor_.GetState() == ShooterMotor::STATE_FIRE) {
+ if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
hit_fire = true;
}
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
- EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
- EXPECT_TRUE(hit_requestfire);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
+// TODO(austin): Test all the timeouts...
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 1b0e13c..de33fa7 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -9,7 +9,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00988697090637, 0.000120553991591, 0.0, 0.977479674375, 0.0240196135246, 0.0, 0.0, 1.0;
+ A << 0.998324052598, 0.0007783475087, 0.000278701304898, -0.181614418697, -0.000138907346386, 0.0302015298419, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
B << 0.0, 0.0, 1.0;
Eigen::Matrix<double, 1, 3> C;
@@ -25,9 +25,9 @@
StateFeedbackController<3, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 1.97747967438, 101.419434198, 375.761249895;
+ L << 0.998185145251, 11.8167175789, 298.617717297;
Eigen::Matrix<double, 1, 3> K;
- K << 243.5509628, 18.9166116502, 1.27747967438;
+ K << 162.58140285, 6.68264124674, 0.198185145251;
return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index d395200..fce7579 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00988697090637, 0.0, 0.977479674375;
+ A << 0.998324052598, 0.0007783475087, -0.181614418697, -0.000138907346386;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000120553991591, 0.0240196135246;
+ B << 0.000278701304898, 0.0302015298419;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,9 +25,9 @@
StateFeedbackController<2, 1, 1> MakeRawShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 1.87747967438, 87.0117404538;
+ L << 0.898185145251, 3.04818975205;
Eigen::Matrix<double, 1, 2> K;
- K << 343.469306513, 26.4814035344;
+ K << 994.822639009, -5.92927654062;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
}
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0384747..c58d7e5 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -314,6 +314,7 @@
void Update(bool stop_motors) {
if (stop_motors) {
U.setZero();
+ U_uncapped.setZero();
} else {
U = U_uncapped = K() * (R - X_hat);
CapU();