merging in shooter code with less bugs
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 672d5ef..08d0355 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -43,7 +43,7 @@
const double shooter_voltage=0.0;
const double shooter_hall_effect_start_position=0.0;
const double shooter_zeroing_off_speed=0.0;
-const double shooter_zeroing_speed=0.0;
+const double shooter_zeroing_speed=1.0;
const double position=0.0;
const Values *DoGetValues() {
@@ -62,10 +62,10 @@
control_loops::MakeClutchDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {0.0, 100.0, {-3.0, 2.0}, {-1, 4.0}, {2.0, 7.0}},
+ {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07}}
shooter_voltage,
// shooter_total_length
- 100.0,
+ 1.0,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
@@ -95,10 +95,10 @@
control_loops::MakeDogDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {0.0, 100.0, {-3.0, 2.0}, {-1, 4.0}, {2.0, 7.0}},
+ {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07}}
shooter_voltage,
// shooter_total_length
- 100.0,
+ 1.0,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 86991e8..831ef19 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -15,31 +15,33 @@
namespace control_loops {
using ::aos::time::Time;
-
+
void ZeroedStateFeedbackLoop::CapU() {
const double old_voltage = voltage_;
voltage_ += U(0, 0);
uncapped_voltage_ = voltage_;
- double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+ // TODO(ben): Limit the voltage if we are ever not certain that things are
+ // working.
+ double limit = 12.0;
// 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) {
+ //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));
+ //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));
+ //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_;
}
@@ -47,28 +49,35 @@
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),
- shooter_brake_set_time_(0,0),
- prepare_fire_end_time_(0,0),
- shot_end_time_(0,0),
- cycles_not_moved_(0) { }
-
+ calibration_position_(0.0),
+ state_(STATE_INITIALIZE),
+ loading_problem_end_time_(0, 0),
+ shooter_brake_set_time_(0, 0),
+ prepare_fire_end_time_(0, 0),
+ shot_end_time_(0, 0),
+ cycles_not_moved_(0),
+ initial_loop_(true) {}
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
const control_loops::ShooterGroup::Position *position,
control_loops::ShooterGroup::Output *output,
- control_loops::ShooterGroup::Status * status) {
+ control_loops::ShooterGroup::Status *status) {
constexpr double dt = 0.01;
// we must always have these or we have issues.
if (goal == NULL || status == NULL) {
- if (output) output->voltage = 0;
- LOG(ERROR, "Thought I would just check for null and die.\n");
- return;
+ if (output) output->voltage = 0;
+ LOG(ERROR, "Thought I would just check for null and die.\n");
+ return;
+ }
+
+ if (initial_loop_) {
+ initial_loop_ = false;
+ shooter_.SetPositionDirectly(position->position);
+ } else {
+ shooter_.SetPositionValues(position->position);
}
// Disable the motors now so that all early returns will return with the
@@ -77,7 +86,16 @@
const frc971::constants::Values &values = constants::GetValues();
- double real_position = position->position - calibration_position_;
+ 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_);
+ }
// don't even let the control loops run
bool shooter_loop_disable = false;
@@ -86,220 +104,232 @@
bool apply_some_voltage = false;
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_hall_effect){
- calibration_position_ = position->position -
- values.shooter.pusher_distal.lower_limit;
- } else if (position && position->pusher_proximal_hall_effect) {
- calibration_position_ = position->position -
- values.shooter.pusher_proximal.lower_limit;
- } else {
- calibration_position_ = values.shooter_total_length;
- }
+ case STATE_INITIALIZE:
+ // start off with the assumption that we are at the value
+ // futhest back given our sensors
+ if (position && position->pusher_distal_hall_effect) {
+ //TODO_ben: use posedge
+ calibration_position_ =
+ position->position - values.shooter.pusher_distal.lower_limit;
+ } else if (position && position->pusher_proximal_hall_effect) {
+ //TODO_ben: use posedge
+ calibration_position_ =
+ position->position - values.shooter.pusher_proximal.lower_limit;
+ }
state_ = STATE_REQUEST_LOAD;
- // zero out initial goal
- shooter_.SetGoalPosition(0.0, 0.0);
+ // zero out initial goal
+ shooter_.SetGoalPosition(real_position, 0.0);
if (position) {
output->latch_piston = 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;
+ // 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;
+ case STATE_REQUEST_LOAD:
if (position->plunger_back_hall_effect && position->latch_hall_effect) {
- // already latched
- state_ = STATE_PREPARE_SHOT;
+ // already latched
+ state_ = STATE_PREPARE_SHOT;
} else if (position->pusher_distal_hall_effect ||
- (real_position) < 0) {
- state_ = STATE_LOAD_BACKTRACK;
- if (position) {
- calibration_position_ = position->position;
- }
+ (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;
+ }
} else {
- state_ = STATE_LOAD;
+ state_ = STATE_LOAD;
}
- shooter_.SetGoalPosition(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 (real_position < values.shooter.pusher_distal.upper_limit + 0.01) {
- shooter_.SetGoalPosition(position->position + values.shooter_zeroing_speed*dt,
- values.shooter_zeroing_speed);
+ shooter_.SetGoalPosition(0.0, 0.0);
+ if (position && output)
+ output->latch_piston = position->plunger_back_hall_effect;
+ if (output) output->brake_piston = false;
+ break;
+ case STATE_LOAD_BACKTRACK:
+ if (adjusted_position > values.shooter.pusher_distal.upper_limit + 0.01) {
+ shooter_.SetGoalPosition(
+ real_position - values.shooter_zeroing_speed * dt,
+ values.shooter_zeroing_speed);
} else {
- state_ = STATE_LOAD;
+ state_ = STATE_LOAD;
+ }
+
+ if (output) output->latch_piston = false;
+ if (output) output->brake_piston = true;
+ break;
+ case STATE_LOAD:
+ if (position && position->pusher_proximal_hall_effect &&
+ !last_position_.pusher_proximal_hall_effect) {
+ //TODO_ben: use posedge
+ calibration_position_ =
+ position->position - values.shooter.pusher_proximal.upper_limit;
+ }
+ if (position && position->pusher_distal_hall_effect &&
+ !last_position_.pusher_distal_hall_effect) {
+ //TODO_ben: use posedge
+ calibration_position_ =
+ position->position - values.shooter.pusher_distal.lower_limit;
+ }
+
+ shooter_.SetGoalPosition(calibration_position_, 0.0);
+ if (position && output) {
+ output->latch_piston = position->plunger_back_hall_effect;
+ }
+
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (position->plunger_back_hall_effect &&
+ 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);
+ }
+ if (output) output->brake_piston = false;
+ break;
+ case STATE_LOADING_PROBLEM:
+ if (Time::Now() > loading_problem_end_time_) {
+ state_ = STATE_UNLOAD;
+ } else if (position->plunger_back_hall_effect &&
+ position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ }
+ shooter_.SetGoalPosition(calibration_position_, 0.0);
+ LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
+ position->plunger_back_hall_effect, position->latch_hall_effect);
+
+ if (output) output->latch_piston = true;
+ if (output) output->brake_piston = false;
+ break;
+ case STATE_PREPARE_SHOT:
+ 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) {
+ 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;
+ }
+ break;
+ case STATE_READY:
+ if (Time::Now() > shooter_brake_set_time_) {
+ 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;
+ }
+ }
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+
+ 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_ =
+ Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ apply_some_voltage = true;
+ state_ = STATE_PREPARE_FIRE;
+ } else {
+ state_ = STATE_REQUEST_LOAD;
+ }
+ break;
+ case STATE_PREPARE_FIRE:
+ shooter_loop_disable = true;
+ if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
+ apply_some_voltage = true;
+ } else {
+ state_ = STATE_FIRE;
+ cycles_not_moved_ = 0;
+ shot_end_time_ = Time::Now(Time::kDefaultClock) + Time::InMS(500);
+ }
+
+ output->latch_piston = true;
+ output->brake_piston = true;
+
+ break;
+ case STATE_FIRE:
+ shooter_loop_disable = true;
+ //TODO_ben: need approamately equal
+ if (fabs(last_position_.position - adjusted_position) < 0.07) {
+ cycles_not_moved_++;
+ } else {
+ cycles_not_moved_ = 0;
+ }
+ if ((adjusted_position < 0.10 && cycles_not_moved_ > 5) ||
+ Time::Now(Time::kDefaultClock) > shot_end_time_) {
+ state_ = STATE_REQUEST_LOAD;
+ }
+ output->latch_piston = true;
+ output->brake_piston = true;
+ break;
+ case STATE_UNLOAD:
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ shooter_.SetGoalPosition(0.02, 0.0);
+ if (adjusted_position < 0.04) {
+ output->latch_piston = false;
+ }
+ } else {
+ output->latch_piston = false;
+ state_ = STATE_UNLOAD_MOVE;
+ }
+
+ output->brake_piston = false;
+ break;
+ case STATE_UNLOAD_MOVE:
+ if (adjusted_position > values.shooter_total_length - 0.03) {
+ shooter_.SetGoalPosition(real_position, 0.0);
+ state_ = STATE_READY_UNLOAD;
+ } else {
+ shooter_.SetGoalPosition(
+ real_position + values.shooter_zeroing_speed * dt,
+ values.shooter_zeroing_speed);
}
output->latch_piston = false;
- output->brake_piston = true;
- break;
- case STATE_LOAD:
- if (position->pusher_proximal_hall_effect &&
- !last_position_.pusher_proximal_hall_effect) {
- calibration_position_ = position->position -
- values.shooter.pusher_proximal.upper_limit;
- }
- if (position->pusher_distal_hall_effect &&
- !last_position_.pusher_distal_hall_effect) {
- calibration_position_ = position->position -
- values.shooter.pusher_distal.lower_limit;
- }
-
- shooter_.SetGoalPosition(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_ = Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
- }
- break;
- case STATE_LOADING_PROBLEM:
- if (position->plunger_back_hall_effect && position->latch_hall_effect) {
- state_ = STATE_PREPARE_SHOT;
- } else if (real_position < -0.02 || Time::Now() > loading_problem_end_time_) {
- state_ = STATE_UNLOAD;
+ output->brake_piston = false;
+ break;
+ case STATE_READY_UNLOAD:
+ if (!goal->unload_requested) {
+ state_ = STATE_REQUEST_LOAD;
}
- shooter_.SetGoalPosition(position->position - values.shooter_zeroing_speed*dt,
- values.shooter_zeroing_speed);
- if (output) output->latch_piston = true;
- if (output) output->brake_piston = false;
- break;
- case STATE_PREPARE_SHOT:
- shooter_.SetGoalPosition(
- PowerToPosition(goal->shot_power), 0.0);
- //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
- if (position->position == PowerToPosition(goal->shot_power)) {
- state_ = STATE_READY;
- output->latch_piston = true;
- output->brake_piston = true;
- shooter_brake_set_time_ = Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
- } else {
- output->latch_piston =true;
- output->brake_piston = false;
- }
- break;
- case STATE_READY:
- if (Time::Now() > 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_ = Time::Now(Time::kDefaultClock)
- + Time::InMS(40.0);
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
- } else {
- state_ = STATE_REQUEST_LOAD;
- }
- break;
- case STATE_PREPARE_FIRE:
- shooter_loop_disable = true;
- if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
- apply_some_voltage = true;
- } else {
- state_ = STATE_FIRE;
- cycles_not_moved_ = 0;
- shot_end_time_ = Time::Now(Time::kDefaultClock) +
- Time::InMS(500);
- }
-
- 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;
- }
- if ((real_position < 10.0 && cycles_not_moved_ > 5) ||
- Time::Now(Time::kDefaultClock) > shot_end_time_) {
- state_ = STATE_REQUEST_LOAD;
- }
- output->latch_piston = true;
- output->brake_piston = true;
- break;
- case STATE_UNLOAD:
- if (position->plunger_back_hall_effect && position->latch_hall_effect) {
- shooter_.SetGoalPosition(0.02, 0.0);
- if (real_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_total_length - 0.03) {
- shooter_.SetGoalPosition(position->position, 0.0);
- state_ = STATE_READY_UNLOAD;
- } else {
- shooter_.SetGoalPosition(
- 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;
- }
-
- if (position) {
- last_position_ = *position;
- LOG(DEBUG, "pos: hall: real: %.2f absolute: %.2f\n",
- real_position, position->position);
+ output->latch_piston = false;
+ output->brake_piston = false;
+ break;
}
if (apply_some_voltage) {
- output->voltage = 2.0;
+ shooter_.Update(true);
+ if (output) output->voltage = 2.0;
} else if (!shooter_loop_disable) {
- output->voltage = shooter_.voltage();
+ LOG(DEBUG, "Running the loop, goal is %f\n", shooter_.R(0, 0));
+ shooter_.Update(output == NULL);
+ if (output) output->voltage = shooter_.voltage();
} else {
- output->voltage = 0.0;
+ shooter_.Update(true);
+ if (output) output->voltage = 0.0;
}
- status->done = ::std::abs(position->position - PowerToPosition(goal->shot_power)) < 0.004;
+ status->done =
+ ::std::fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 4705f86..9d134af 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -38,7 +38,7 @@
uncapped_voltage_(0.0),
offset_(0.0),
encoder_(0.0),
- last_encoder_(0.0){}
+ last_encoder_(0.0) {}
const static int kZeroingMaxVoltage = 5;
@@ -61,24 +61,21 @@
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::ShooterLimits &shooter_values,
- JointZeroingState zeroing_state) {
+ bool SetCalibrationOnEdge(
+ const constants::Values::ShooterLimits &shooter_values,
+ JointZeroingState zeroing_state) {
double edge_encoder;
double known_position;
if (GetPositionOfEdge(shooter_values, &edge_encoder, &known_position)) {
@@ -90,20 +87,21 @@
return false;
}
+ void SetPositionDirectly(double position) { X_hat(0, 0) = position; }
void SetPositionValues(double position) {
Eigen::Matrix<double, 1, 1> Y;
Y << position;
+ LOG(INFO, "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};
- R << desired_position, desired_velocity, 0;
+ 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(INFO, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
+ R << desired_position, desired_velocity, 0;
}
double position() const { return X_hat(0, 0); }
@@ -131,49 +129,53 @@
double last_encoder_;
};
-
class ShooterMotor
: public aos::control_loops::ControlLoop<control_loops::ShooterGroup> {
public:
- explicit ShooterMotor(
- control_loops::ShooterGroup *my_shooter = &control_loops::shooter_queue_group);
+ explicit ShooterMotor(control_loops::ShooterGroup *my_shooter =
+ &control_loops::shooter_queue_group);
// 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");
+ //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;
+ : (power < 0.0)
+ ? 0.0
+ : power;
return new_pos;
}
-typedef enum {
- STATE_INITIALIZE,
- STATE_REQUEST_LOAD,
- STATE_LOAD_BACKTRACK,
- STATE_LOAD,
- STATE_LOADING_PROBLEM,
- STATE_PREPARE_SHOT,
- STATE_READY,
- STATE_REQUEST_FIRE,
- STATE_PREPARE_FIRE,
- STATE_FIRE,
- STATE_UNLOAD,
- STATE_UNLOAD_MOVE,
- STATE_READY_UNLOAD
-} State;
+ typedef enum {
+ STATE_INITIALIZE = 0,
+ STATE_REQUEST_LOAD = 1,
+ STATE_LOAD_BACKTRACK = 2,
+ STATE_LOAD = 3,
+ 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;
+
+ State GetState() {return state_;}
+
+ double GetPosition() { return shooter_.position() - calibration_position_; }
protected:
virtual void RunIteration(
const ShooterGroup::Goal *goal,
const control_loops::ShooterGroup::Position *position,
- ShooterGroup::Output *output,
- ShooterGroup::Status *status);
+ ShooterGroup::Output *output, ShooterGroup::Status *status);
private:
// Friend the test classes for acces to the internal state.
@@ -206,6 +208,9 @@
// track cycles that we are stuck to detect errors
int cycles_not_moved_;
+ // setup on the intial loop may involve shortcuts
+ bool initial_loop_;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 670b10f..4350bd2 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -28,8 +28,6 @@
bool pusher_proximal_hall_effect;
// the latch is closed
bool latch_hall_effect;
- // the brake is closed
- bool brake_hall_effect;
// count of positive edges
int64_t plunger_back_hall_effect_posedge_count;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index b2da13e..4371f17 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -10,7 +10,6 @@
#include "frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h"
#include "frc971/constants.h"
-
using ::aos::time::Time;
namespace frc971 {
@@ -23,7 +22,7 @@
public:
// Constructs a motor simulation.
ShooterSimulation(double initial_position)
- : shooter_plant_(new StateFeedbackPlant<3, 1, 1>(MakeShooterPlant())),
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
shooter_queue_group_(
".frc971.control_loops.shooter_queue_group", 0xcbf22ba9,
".frc971.control_loops.shooter_queue_group.goal",
@@ -32,9 +31,10 @@
".frc971.control_loops.shooter_queue_group.status") {
Reinitialize(initial_position);
}
+
void Reinitialize(double initial_position) {
- LOG(INFO, "Reinitializing to {top: %f}\n", initial_position);
- StateFeedbackPlant<3, 1, 1>* plant = shooter_plant_.get();
+ 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(1, 0) = 0.0;
@@ -44,30 +44,24 @@
SetPhysicalSensors(&last_position_message_);
}
-
// 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); }
// Returns the adjusted angle of the wrist.
double GetPosition() const {
return GetAbsolutePosition() - initial_position_;
}
-
// Makes sure pos is inside range (inclusive)
bool CheckRange(double pos, struct constants::Values::Pair pair) {
return (pos >= pair.lower_limit && pos <= pair.upper_limit);
}
-
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
- const frc971::constants::Values& values = constants::GetValues();
- position->position = GetPosition();
+ const frc971::constants::Values &values = constants::GetValues();
+ position->position = GetAbsolutePosition();
LOG(DEBUG, "Physical shooter at {%f}\n", position->position);
@@ -81,39 +75,68 @@
CheckRange(position->position, values.shooter.pusher_proximal);
}
- void UpdateEffectEdge(bool effect, bool last_effect, double upper_angle,
- double lower_angle, double position,
+ void UpdateEffectEdge(bool &effect, bool last_effect, double upper_limit,
+ double lower_limit, double position,
double &posedge_value, double &negedge_value,
int64_t &posedge_count, int64_t &negedge_count) {
- if (effect && !last_effect) {
- ++posedge_count;
- if (last_position_message_.position < lower_angle) {
- posedge_value = lower_angle - initial_position_;
- } else {
- posedge_value = upper_angle - initial_position_;
- }
- }
+ if (effect && !last_effect) {
+ ++posedge_count;
+ if (last_position_message_.position < lower_limit) {
+ posedge_value = lower_limit - initial_position_;
+ } else {
+ posedge_value = upper_limit - initial_position_;
+ }
+ }
- if (!effect && last_effect) {
- ++negedge_count;
- if (position < lower_angle) {
- negedge_value = lower_angle - initial_position_;
- } else {
- negedge_value = upper_angle - initial_position_;
- }
- }
+ if (!effect && last_effect) {
+ ++negedge_count;
+ if (position < lower_limit) {
+ negedge_value = lower_limit - initial_position_;
+ } else {
+ negedge_value = upper_limit - initial_position_;
+ }
+ }
}
-
// Sends out the position queue messages.
void SendPositionMessage() {
- const frc971::constants::Values& values = constants::GetValues();
+ const frc971::constants::Values &values = constants::GetValues();
::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
shooter_queue_group_.position.MakeMessage();
SetPhysicalSensors(position.get());
- // Handle plunger hall effect
+ // 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_hall_effect = 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_hall_effect = 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_++;
+ }
+
+ // Handle plunger hall effect
UpdateEffectEdge(position->plunger_back_hall_effect,
last_position_message_.plunger_back_hall_effect,
values.shooter.plunger_back.upper_limit,
@@ -122,8 +145,10 @@
position->negedge_value,
position->plunger_back_hall_effect_posedge_count,
position->plunger_back_hall_effect_negedge_count);
+ LOG(INFO, "seteffect: plunger back: %d\n",
+ position->plunger_back_hall_effect);
- // Handle pusher distal hall effect
+ // Handle pusher distal hall effect
UpdateEffectEdge(position->pusher_distal_hall_effect,
last_position_message_.pusher_distal_hall_effect,
values.shooter.pusher_distal.upper_limit,
@@ -132,8 +157,10 @@
position->negedge_value,
position->pusher_distal_hall_effect_posedge_count,
position->pusher_distal_hall_effect_negedge_count);
+ LOG(INFO, "seteffect: pusher distal: %d\n",
+ position->plunger_back_hall_effect);
- // Handle pusher proximal hall effect
+ // Handle pusher proximal hall effect
UpdateEffectEdge(position->pusher_proximal_hall_effect,
last_position_message_.pusher_proximal_hall_effect,
values.shooter.pusher_proximal.upper_limit,
@@ -142,30 +169,58 @@
position->negedge_value,
position->pusher_proximal_hall_effect_posedge_count,
position->pusher_proximal_hall_effect_negedge_count);
+ LOG(INFO, "seteffect: pusher proximal: %d\n",
+ position->plunger_back_hall_effect);
+
+
last_position_message_ = *position;
position.Send();
}
-
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = shooter_plant_->Y(0, 0);
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;
+ } else if (!shooter_queue_group_.output->latch_piston &&
+ latch_piston_state_ && latch_delay_count_ == 0) {
+ latch_delay_count_ = -6;
+ }
+
+ if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
+ brake_delay_count_ == 0) {
+ brake_delay_count_ = 5;
+ } else if (!shooter_queue_group_.output->brake_piston &&
+ brake_piston_state_ && brake_delay_count_ == 0) {
+ brake_delay_count_ = -5;
+ }
EXPECT_GE(constants::GetValues().shooter.upper_limit,
shooter_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().shooter.lower_limit,
+ // we okay within a millimeter
+ EXPECT_LE(constants::GetValues().shooter.lower_limit - 1.0,
shooter_plant_->Y(0, 0));
last_voltage_ = shooter_queue_group_.output->voltage;
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
-
// pointer to plant
- ::std::unique_ptr<StateFeedbackPlant<3, 1, 1>> shooter_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ // true latch closed
+ int latch_piston_state_;
+ // greater than zero, delaying close. less than zero delaying open
+ int latch_delay_count_;
+
+ // true brake locked
+ int brake_piston_state_;
+ // greater than zero, delaying close. less than zero delaying open
+ int brake_delay_count_;
private:
@@ -177,7 +232,6 @@
double last_plant_position_;
};
-
class ShooterTest : public ::testing::Test {
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -204,13 +258,12 @@
// test.
::aos::robot_state.Clear();
SendDSPacket(true);
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
-
void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled).autonomous(false)
+ .team_id(971).Send();
::aos::robot_state.FetchLatest();
}
@@ -221,14 +274,16 @@
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 1e-4);
}
- virtual ~ShooterTest() { ::aos::robot_state.Clear(); }
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ ::aos::time::Time::DisableMockTime();
+ }
};
-
TEST_F(ShooterTest, PowerConversion) {
// test a couple of values return the right thing
- EXPECT_EQ(2.1, shooter_motor_.PowerToPosition(2.1));
- EXPECT_EQ(50.99, shooter_motor_.PowerToPosition(50.99));
+ EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
+ EXPECT_EQ(0.475, shooter_motor_.PowerToPosition(0.475));
const frc971::constants::Values &values = constants::GetValues();
// value too large should get max
@@ -239,15 +294,58 @@
}
// Tests that the wrist zeros correctly and goes to a position.
-TEST_F(ShooterTest, ZerosCorrectly) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(2.1).Send();
- for (int i = 0; i < 400; ++i) {
+TEST_F(ShooterTest, GoesToValue) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
- VerifyNearGoal();
+ //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());
+}
+
+// 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();
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+ 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) {
+ shooter_motor_plant_.SendPositionMessage();
+ 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){
+ hit_preparefire = true;
+ }
+ if (shooter_motor_.GetState() == 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_TRUE(hit_preparefire);
+ EXPECT_TRUE(hit_fire);
}
} // namespace testing