Fixed shooter to not have a spring some times.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index fb9df74..8f435fb 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -22,10 +22,6 @@
uncapped_voltage_ = voltage_;
- // 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.
@@ -37,13 +33,54 @@
LOG(INFO, "Capping due to runawway\n");
}
- voltage_ = std::min(limit, voltage_);
- voltage_ = std::max(-limit, voltage_);
+ voltage_ = std::min(max_voltage_, voltage_);
+ voltage_ = std::max(-max_voltage_, voltage_);
U(0, 0) = voltage_ - old_voltage;
LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
last_voltage_ = voltage_;
+ capped_goal_ = false;
+}
+
+void ZeroedStateFeedbackLoop::CapGoal() {
+ if (uncapped_voltage() > max_voltage_) {
+ double dx;
+ if (controller_index() == 0) {
+ dx = (uncapped_voltage() - max_voltage_) /
+ (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ R(0, 0) -= dx;
+ R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ } else {
+ dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ R(0, 0) -= dx;
+ }
+ capped_goal_ = true;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ } else if (uncapped_voltage() < -max_voltage_) {
+ double dx;
+ if (controller_index() == 0) {
+ dx = (uncapped_voltage() + max_voltage_) /
+ (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ R(0, 0) -= dx;
+ R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ } else {
+ dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ R(0, 0) -= dx;
+ }
+ capped_goal_ = true;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ } else {
+ capped_goal_ = false;
+ }
+}
+
+void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
+ if (controller_index() == 0) {
+ R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ } else {
+ R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ }
}
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
@@ -60,6 +97,9 @@
Y_(0, 0) += doffset;
// Offset the goal so we don't move.
R(0, 0) += doffset;
+ if (controller_index() == 0) {
+ R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ }
LOG(INFO, "Validation: position is %f\n", absolute_position());
}
@@ -117,16 +157,35 @@
// 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;
+ // If true, move the goal if we saturate.
+ bool cap_goal = false;
+
+ // TODO(austin): Move the offset if we see or don't see a hall effect when we
+ // expect to see one.
+ // Probably not needed yet.
+
+ if (position) {
+ int last_controller_index = shooter_.controller_index();
+ if (position->plunger && position->latch) {
+ // Use the controller without the spring if the latch is set and the
+ // plunger is back
+ shooter_.set_controller_index(1);
+ } else {
+ // Otherwise use the controller with the spring.
+ shooter_.set_controller_index(0);
+ }
+ if (shooter_.controller_index() != last_controller_index) {
+ shooter_.RecalculatePowerGoal();
+ }
+ }
switch (state_) {
case STATE_INITIALIZE:
if (position) {
// 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.
@@ -135,7 +194,7 @@
values.shooter.pusher_distal.lower_angle);
} else if (position->pusher_proximal.current) {
shooter_.SetCalibration(position->position,
- values.shooter.pusher_proximal.lower_angle);
+ values.shooter.pusher_proximal.upper_angle);
} else {
shooter_.SetCalibration(position->position,
values.shooter.upper_limit);
@@ -183,12 +242,13 @@
// 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(
shooter_.goal_position() + values.shooter.zeroing_speed * dt,
values.shooter.zeroing_speed);
}
+ cap_goal = true;
+ shooter_.set_max_voltage(12.0);
if (position) {
if (!position->pusher_distal.current) {
@@ -434,21 +494,25 @@
unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
- // TODO(austin): Windup...
+ cap_goal = true;
+ shooter_.set_max_voltage(6.0);
// 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) {
+ // If we were at the limit last cycle, we are done unloading.
+ // This is because if we saturate, we might hit the limit before we are
+ // actually there.
+ if (shooter_.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(goal_position, values.shooter.zeroing_speed);
+ shooter_.SetGoalPosition(
+ ::std::min(
+ values.shooter.upper_limit,
+ shooter_.goal_position() + values.shooter.zeroing_speed * dt),
+ values.shooter.zeroing_speed);
}
latch_piston_ = false;
@@ -475,14 +539,22 @@
if (apply_some_voltage) {
shooter_.Update(true);
+ shooter_.ZeroPower();
if (output) output->voltage = 2.0;
} else if (!shooter_loop_disable) {
LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
shooter_.goal_position(), shooter_.absolute_position());
+ if (!cap_goal) {
+ shooter_.set_max_voltage(12.0);
+ }
shooter_.Update(output == NULL);
+ if (cap_goal) {
+ shooter_.CapGoal();
+ }
if (output) output->voltage = shooter_.voltage();
} else {
shooter_.Update(true);
+ shooter_.ZeroPower();
if (output) output->voltage = 0.0;
}
@@ -491,8 +563,8 @@
output->brake_piston = brake_piston_;
}
- status->done =
- ::std::abs(shooter_.absolute_position() - PowerToPosition(goal->shot_power)) < 0.004;
+ status->done = ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) < 0.004;
if (position) {
last_position_ = *position;