got all the typing done for shooter state machine. Need to merge joes stuff so I can build
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 11e3e84..a776ee8 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -13,9 +13,38 @@
namespace frc971 {
namespace control_loops {
+
+void ZeroedStateFeedbackLoop::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ }
+
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ 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));
+
+ last_voltage_ = voltage_;
+}
+
ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
: aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
- zeroed_joint_(MakeShooterLoop()) {
+ shooter_(MakeShooterLoop()) {
{
using ::frc971::constants::GetValues;
ZeroedJoint<1>::ConfigurationData config_data;
@@ -33,22 +62,6 @@
}
}
-enum {
- STATE_INITIALIZE,
- STATE_REQUEST_LOAD,
- STATE_LOAD_BACKTRACK,
- STATE_LOAD,
- STATE_LOADING_PROBLEM,
- STATE_PREPARE_SHOT,
- STATE_BRAKE_SET,
- STATE_READY,
- STATE_REQUEST_FIRE,
- STATE_PREPARE_FIRE,
- STATE_FIRE,
- STATE_UNLOAD,
- STATE_UNLOAD_MOVE,
- STATE_READY_UNLOAD
-} State;
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
@@ -58,66 +71,228 @@
::aos::control_loops::Status * status) {
constexpr double dt = 0.01;
- if (goal == NULL || position == NULL ||
- output == NULL || status == NULL) {
+ // we must always have these or we have issues.
+ if (goal == NULL || status == NULL) {
transform-position_ptr = NULL;
if (output) output->voltage = 0;
LOG(ERROR, "Thought I would just check for null and die.\n");
+ return;
}
// Disable the motors now so that all early returns will return with the
// motors disabled.
- output->voltage = 0;
+ if (output) output->voltage = 0;
ZeroedJoint<1>::PositionData transformed_position;
ZeroedJoint<1>::PositionData *transformed_position_ptr =
&transformed_position;
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
+ if (position) {
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
+ }
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ const double voltage = shooter_.Update(transformed_position_ptr,
output != NULL,
goal->goal, 0.0);
const frc971::constants::Values &values = constants::GetValues();
+ double absolute_position = postiion->position - calibration_position_;
+
+
switch (state_) {
case STATE_INITIALIZE:
shooter_.zeroing_state() = ZeroedStateFeedbackLoop::UNKNOWN_POSITION;
- if (position->pusher_distal_hall_effect){
- } else if (position->pusher_proximal_hall_effect) {
+
+ // start off with the assumption that we are at the value
+ // futhest back given our sensors
+ if (position && position->pusher_distal_hall_effect){
calibration_position_ = position->position -
+ values.pusher_distal_heffect.lower_edge;
+ } else if (position && position->pusher_proximal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_proximal_heffect.lower_edge;
} else {
+ calibration_position_ = values.shooter_total_length;
}
+ state_ = STATE_REQUEST_LOAD;
- break;
+ // zero out initial goal
+ shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ if (position) {
+ output->latch_pistion = position->plunger_back_hall_effect;
+ } else {
+ // we don't know what is going on so just close the latch to be safe
+ output->latch_piston = true;
+ }
+ output->brake_piston = false;
+ break;
case STATE_REQUEST_LOAD:
- break;
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ // already latched
+ state_ = STATE_PREPARE_SHOT;
+ } else if (postion->pusher_back_distal_hall_effect ||
+ (relative_position) < 0) {
+ state_ = STATE_LOADING_BACKTRACK;
+ if (relative_position) {
+ calibration_position_ = position->position;
+ }
+ } else {
+ state_ = STATE_LOAD;
+ }
+
+ shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ if (position && output) output->latch_piston = position->plunger_back_hall_effect;
+ output->brake_piston = false;
+ break;
case STATE_LOAD_BACKTRACK:
+ if (absolute_position < values.pusher_back_distal_heffect.lower_edge + 0.01) {
+ shooter_.SetGoalPositionVelocity(position->position + values.shooter_zero_speed*dt,
+ values.shooter_zero_speed);
+ } else {
+ state = STATE_LOAD;
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = true;
break;
case STATE_LOAD:
+ if (position->pusher_proximal_hall_effect &&
+ !last_position_.pusher_back_proximal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_promimal_heffect.lower_edge;
+ }
+ if (position->pusher_distal_hall_effect &&
+ !last_position_.pusher_back_distal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_distal_heffect.lower_edge;
+
+ }
+
+ shooter_.SetGoalPositionVelocity(calibration_position_, 0.0);
+ if (position && output) output->latch_piston = position->plunger_back_hall_effect;
+ if(output) output->brake_piston = false;
+
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (position->plunger_back_hall_effect &&
+ position->position == PowerToPosition(goal->shot_power)) {
+ //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
+ state_ = STATE_LOADING_PROBLEM;
+ loading_problem_end_time_ = clock() + 3 * CLOCKS_PER_SECOND;
+ }
break;
case STATE_LOADING_PROBLEM:
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (absolute_position < -0.02 || clock() > loading_problem_end_time_) {
+ state = STATE_UNLOAD;
+ }
+
+ shooter_.SetGoalPositionVelocity(position->position - values.shooter_zero_speed*dt,
+ values.shooter_zero_speed);
+ if (output) output->latch_piston = true;
+ if (output) output->brake_piston = false;
break;
case STATE_PREPARE_SHOT:
- break;
- case STATE_BRAKE_SET:
+ shooter_.SetGoalPosition(
+ PowerToPosition(shot_power), 0.0);
+ if (position->position == shooter.goal_position) {
+ state_ = STATE_READY;
+ output->latch_piston = true;
+ output->brake_piston = true;
+ shooter_brake_set_time_ = clock() + 5 * CLOCKS_PER_SECOND;
+ } else {
+ output->latch_piston =true;
+ output->brake_piston = false;
+ }
break;
case STATE_READY:
+ if (clock() > shooter_brake_set_time_) {
+ shooter_loop_disable = true;
+ if (goal->unload_requested) {
+ state_ = STATE_UNLOAD;
+ } else if (PowerToPosition(goal->shot_power)
+ != position->position) {
+ //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
+ state_ = STATE_PREPARE_SHOT;
+ }else if (goal->shot_requested) {
+ state_ = STATE_REQUEST_FIRE;
+ }
+
+ }
+ output->latch_piston = true;
+ output->brake_piston = true;
break;
case STATE_REQUEST_FIRE:
+ shooter_loop_disable = true;
+ if (position->plunger_back_hall_effect) {
+ prepare_fire_end_time_ = clock() + 10;
+ state_ = STATE_PREPARE_FIRE;
+ } else {
+ state_ = STATE_REQUEST_LOAD;
+ }
break;
case STATE_PREPARE_FIRE:
+ shooter_loop_disable = true;
+ if (clock() < prepare_fire_end_time_) {
+ shooter_.ApplySomeVoltage();
+ } else {
+ State_ = STATE_FIRE;
+ cycles_not_moved_ = 0;
+ shot_end_time_ = clock() + 0.5 * CLOCKS_PER_SECOND;
+ }
+
+ output->latch_piston = true;
+ output->brake_piston = true;
+
break;
case STATE_FIRE:
+ shooter_loop_disable = true;
+ //TODO_ben: need approamately equal
+ if (last_position->position - position->position < 7) {
+ cycles_not_moved++;
+ } else {
+ cycles_not_moved = 0;
+ }
+ output->latch_piston = true;
+ ouput->brake_piston = true;
break;
case STATE_UNLOAD:
+ if (position->plunger_back_hall_effect && position->latch_piston) {
+ shooter_SetGoalPosition(0.02, 0.0);
+ if (ablsolute_position == 0.02) {
+ output->latch_piston = false;
+ }
+ } else {
+ output->latch_piston = false;
+ state_ = STATE_UNLOAD_MOVE;
+ }
+
+ output->brake_piston = false;
break;
case STATE_UNLOAD_MOVE:
+ if (position->position > values.shooter_length - 0.03) {
+ shooter_.SetPosition(position->position, 0.0);
+ state_ = STATE_READY_UNLOADED;
+ } else {
+ shooter_.SetPosition(
+ position->position + values.shooter_zeroing_speed*dt
+ values.shooter_zeroing_speed);
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = false;
break;
case STATE_READY_UNLOAD:
+ if (!goal->unload_requested) {
+ state_ = STATE_REQUEST_LOAD;
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = false;
break;
}