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;
}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
old mode 100644
new mode 100755
index a6615bf..4ea7a7b
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -18,6 +18,119 @@
class ShooterTest_NoWindupNegative_Test;
};
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+// It does not have any zeroing logic in it, only logic to deal with a delta U
+// controller.
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ voltage_(0.0),
+ last_voltage_(0.0),
+ uncapped_voltage_(0.0),
+ offset_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0){}
+
+ const static int kZeroingMaxVoltage = 5;
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // 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;
+ }
+
+
+ bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double known_position;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &known_position)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, known_position);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+ }
+
+
+ void SetPositionValues(double poistion) {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << position;
+ Correct(Y);
+ }
+
+
+ void SetGoalPositionVelocity(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};
+ printf("%s:%d : seg fault?\n", __FILE__, __LINE__);
+ *(const char **)(NULL) = "seg fault";
+ }
+
+ double position() const { return X_hat(0, 0); }
+ double encoder() const { return encoder_; }
+ double last_encoder() const { return last_encoder_; }
+
+ // 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);
+
+#undef COUNT_SETTER_GETTER
+
+ private:
+ // 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
: public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
public:
@@ -39,6 +152,23 @@
// True if the state machine is ready.
bool is_ready() const { return zeroed_joint_.is_ready(); }
+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;
+
protected:
virtual void RunIteration(
const ShooterLoop::Goal *goal,
@@ -53,6 +183,7 @@
// The zeroed joint to use.
ZeroedJoint<1> zeroed_joint_;
+ ZeroedStateFeedbackLoop shooter_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 0470f56..c110b8e 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -8,9 +8,10 @@
message Output {
// The energy to load to in joules.
double voltage;
- // Shoots as soon as this is true.
- bool latched;
- bool locked; //Disc brake locked
+ // true: latch engaged, false: latch open
+ bool latch_piston;
+ // true: brake engaged false: brake released
+ bool brake_piston;
};
message Goal {
// encoder ticks of shot energy.
@@ -48,7 +49,6 @@
// In meters, out is positive.
double position;
- double back_calibration;
// last positive edge
double posedge_value;
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100644
new mode 100755