Merge remote-tracking branch 'brian/devel' into claw
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 3bc8231..4fea843 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -38,8 +38,8 @@
0.47};
const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
0.55};
-const double shooter_zeroing_off_speed=0.0;
-const double shooter_zeroing_speed=1.0;
+const double shooter_zeroing_off_speed = 0.0;
+const double shooter_zeroing_speed = 0.1;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 967be3c..64bd52d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -85,7 +85,7 @@
def c2d(A, B, dt):
"""Converts from continuous time state space representation to discrete time.
- Evaluates e^(A dt) for the discrete time version of A, and
+ Evaluates e^(A dt) - I for the discrete time version of A, and
integral(e^(A t) * B, 0, dt).
Returns (A, B). C and D are unchanged."""
e, P = numpy.linalg.eig(A)
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index c5d795c..c2b9dbb 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -5,9 +5,9 @@
import sys
from matplotlib import pylab
-class Shooter(control_loop.ControlLoop):
- def __init__(self, name="RawShooter"):
- super(Shooter, self).__init__(name)
+class SprungShooter(control_loop.ControlLoop):
+ def __init__(self, name="RawSprungShooter"):
+ super(SprungShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = .4982
# Stall Current in Amps
@@ -20,7 +20,7 @@
# This rough estimate should about include the effect of the masses
# of the gears. If this number is too low, the eigen values of self.A
# will start to become extremely small.
- self.J = 12
+ self.J = 20
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2.0
# Motor velocity constant
@@ -31,13 +31,12 @@
# Spring constant for the springs, N/m
self.Ks = 2800.0
# Gear ratio multiplied by radius of final sprocket.
- self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 0.0182
+ self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
+
# Control loop time step
self.dt = 0.01
-
# State feedback matrices
- # TODO(james): Make this work with origins other than at kx = 0.
self.A_continuous = numpy.matrix(
[[0, 1],
[-self.Ks / self.J,
@@ -64,6 +63,73 @@
self.InitializeState()
+class Shooter(SprungShooter):
+ def __init__(self, name="RawShooter"):
+ super(Shooter, self).__init__(name)
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.45, 0.45])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl,
+ self.rpl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+class SprungShooterDeltaU(SprungShooter):
+ def __init__(self, name="SprungShooter"):
+ super(SprungShooterDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+
+ self.A = numpy.matrix([[0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
+
+ self.B = numpy.matrix([[0.0],
+ [0.0],
+ [1.0]])
+
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.PlaceControllerPoles([0.55, 0.45, 0.80])
+
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, 0.90])
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
class ShooterDeltaU(Shooter):
def __init__(self, name="Shooter"):
super(ShooterDeltaU, self).__init__(name)
@@ -103,39 +169,50 @@
self.InitializeState()
-def ClipDeltaU(shooter, delta_u):
- old_u = numpy.matrix([[shooter.X[2, 0]]])
+def ClipDeltaU(shooter, old_voltage, delta_u):
+ old_u = old_voltage
new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
return new_u - old_u
def main(argv):
- # Simulate the response of the system to a step input.
- shooter = Shooter()
- simulated_x = []
- for _ in xrange(2000):
- U = 2.0
- shooter.Update(numpy.matrix([[U]]))
- simulated_x.append(shooter.X[0, 0])
-
- pylab.plot(range(2000), simulated_x)
- pylab.show()
-
# Simulate the response of the system to a goal.
- shooter = Shooter()
+ sprung_shooter = SprungShooterDeltaU()
+ raw_sprung_shooter = SprungShooter()
close_loop_x = []
close_loop_u = []
- R = numpy.matrix([[0.3], [0.0]])
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0], [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
for _ in xrange(500):
- augment = (-numpy.linalg.lstsq(shooter.B_continuous, numpy.identity(
- shooter.B_continuous.shape[0]))[0] *
- shooter.A_continuous * R)
- U = numpy.clip(shooter.K * (R - shooter.X_hat) + augment,
- shooter.U_min, shooter.U_max)
-#U = ClipDeltaU(shooter, U)
+ U = sprung_shooter.K * (R - sprung_shooter.X_hat)
+ U = ClipDeltaU(sprung_shooter, voltage, U)
+ sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
+ sprung_shooter.UpdateObserver(U)
+ voltage += U;
+ raw_sprung_shooter.Update(voltage)
+ close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
+
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
+
+ shooter = ShooterDeltaU()
+ raw_shooter = Shooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0], [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = shooter.K * (R - shooter.X_hat)
+ U = ClipDeltaU(shooter, voltage, U)
+ shooter.Y = raw_shooter.Y + 0.01
shooter.UpdateObserver(U)
- shooter.Update(U)
- close_loop_x.append(shooter.X[0, 0] * 10)
- close_loop_u.append(U[0, 0])
+ voltage += U;
+ raw_shooter.Update(voltage)
+ close_loop_x.append(raw_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
pylab.plot(range(500), close_loop_x)
pylab.plot(range(500), close_loop_u)
@@ -146,16 +223,19 @@
print "Expected .h file name and .cc file name for"
print "both the plant and unaugmented plant"
else:
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
unaug_shooter = Shooter("RawShooter")
unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
- [unaug_shooter])
+ [unaug_sprung_shooter,
+ unaug_shooter])
if argv[3][-3:] == '.cc':
unaug_loop_writer.Write(argv[4], argv[3])
else:
unaug_loop_writer.Write(argv[3], argv[4])
+ sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index c22b899..8f435fb 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -22,29 +22,65 @@
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.
- 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));
+ if (X_hat(2, 0) > last_voltage_ + 4.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
+ LOG(INFO, "Capping due to runawway\n");
+ } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
+ 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(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));
+
+ 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,
@@ -61,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());
}
@@ -112,25 +151,41 @@
const frc971::constants::Values &values = constants::GetValues();
- double relative_position = shooter_.position();
- double absolute_position = shooter_.absolute_position();
-
// Don't even let the control loops run.
bool shooter_loop_disable = false;
// 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.
@@ -139,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);
@@ -161,8 +216,6 @@
break;
case STATE_REQUEST_LOAD:
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;
@@ -189,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);
+ 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) {
@@ -292,10 +346,11 @@
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
- absolute_position, PowerToPosition(goal->shot_power));
- // TODO(austin): Goal velocity too...
+ shooter_.absolute_position(), PowerToPosition(goal->shot_power));
if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.001) {
+ PowerToPosition(goal->shot_power)) +
+ ::std::abs(shooter_.absolute_velocity()) <
+ 0.001) {
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
@@ -311,9 +366,19 @@
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.
+ // Wait until the brake is set, and a shot is requested or the shot power
+ // is changed.
+ if (::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ } else 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;
LOG(DEBUG, "Brake is now set\n");
if (goal->shot_requested && !disabled) {
@@ -324,14 +389,6 @@
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);
}
@@ -360,11 +417,12 @@
firing_starting_position_ = shooter_.absolute_position();
shot_end_time_ = Time::Now() + Time::InSeconds(1);
state_ = STATE_FIRE;
+ latch_piston_ = false;
} else {
apply_some_voltage = true;
+ latch_piston_ = true;
}
- latch_piston_ = true;
brake_piston_ = true;
break;
@@ -436,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;
@@ -477,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;
}
@@ -493,14 +563,14 @@
output->brake_piston = brake_piston_;
}
- status->done =
- ::std::abs(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;
LOG(DEBUG,
- "pos > real: %.2f adjusted: %.2f raw: %.2f state= %d\n",
- relative_position, absolute_position, position->position,
+ "pos > absolute: %f velocity: %f state= %d\n",
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
state_);
}
if (position) {
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 7e99bfc..322a556 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -14,8 +14,8 @@
namespace frc971 {
namespace control_loops {
namespace testing {
-class ShooterTest_NoWindupPositive_Test;
-class ShooterTest_NoWindupNegative_Test;
+class ShooterTest_UnloadWindupPositive_Test;
+class ShooterTest_UnloadWindupNegative_Test;
};
using ::aos::time::Time;
@@ -36,7 +36,9 @@
voltage_(0.0),
last_voltage_(0.0),
uncapped_voltage_(0.0),
- offset_(0.0) {}
+ offset_(0.0),
+ max_voltage_(12.0),
+ capped_goal_(false) {}
const static int kZeroingMaxVoltage = 5;
@@ -59,6 +61,7 @@
double offset() const { return offset_; }
double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1, 0); }
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
@@ -67,6 +70,9 @@
Correct(Y);
}
+ // Recomputes the power goal for the current controller and position/velocity.
+ void RecalculatePowerGoal();
+
double goal_position() const { return R(0, 0) + kPositionOffset; }
double goal_velocity() const { return R(1, 0); }
void InitializeState(double position) {
@@ -74,15 +80,20 @@
}
void SetGoalPosition(double desired_position, double desired_velocity) {
- LOG(DEBUG, "Goal position: %.2f Goal velocity: %.2f\n", desired_position, desired_velocity);
+ LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position, desired_velocity);
R << desired_position - kPositionOffset, desired_velocity,
- -A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity;
+ (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+ A(1, 1) / A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
+ void set_max_voltage(const double max_voltage) { max_voltage_ = max_voltage; }
+ bool capped_goal() const { return capped_goal_; }
+
+ void CapGoal();
+
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
@@ -92,6 +103,8 @@
double last_voltage_;
double uncapped_voltage_;
double offset_;
+ double max_voltage_;
+ bool capped_goal_;
};
class ShooterMotor
@@ -101,7 +114,7 @@
&control_loops::shooter_queue_group);
// True if the goal was moved to avoid goal windup.
- //bool capped_goal() const { return shooter_.capped_goal(); }
+ bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
@@ -131,8 +144,8 @@
private:
// Friend the test classes for acces to the internal state.
- friend class testing::ShooterTest_NoWindupPositive_Test;
- friend class testing::ShooterTest_NoWindupNegative_Test;
+ friend class testing::ShooterTest_UnloadWindupPositive_Test;
+ friend class testing::ShooterTest_UnloadWindupNegative_Test;
// Enter state STATE_UNLOAD
void Unload() {
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index e89cab9..74cb0ec 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -34,6 +34,11 @@
// Constructs a motor simulation.
ShooterSimulation(double initial_position)
: shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
+ latch_piston_state_(false),
+ latch_delay_count_(0),
+ plunger_latched_(false),
+ brake_piston_state_(true),
+ brake_delay_count_(0),
shooter_queue_group_(
".frc971.control_loops.shooter_queue_group", 0xcbf22ba9,
".frc971.control_loops.shooter_queue_group.goal",
@@ -45,7 +50,7 @@
// 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;
+ constexpr static double kPositionOffset = 0.305054 + 0.0254;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -86,7 +91,15 @@
// the correct range.
if (plunger_latched_) {
position->plunger = true;
+ // Only disengage the spring if we are greater than 0, which is where the
+ // latch will take the load off the pusher.
+ if (GetAbsolutePosition() > 0.0) {
+ shooter_plant_->set_plant_index(1);
+ } else {
+ shooter_plant_->set_plant_index(0);
+ }
} else {
+ shooter_plant_->set_plant_index(0);
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
@@ -157,6 +170,10 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
+ if (GetAbsolutePosition() > 0.01) {
+ EXPECT_GE(last_voltage_, 1)
+ << ": Must preload the gearbox when firing.";
+ }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -191,6 +208,7 @@
shooter_plant_->U << last_voltage_;
shooter_plant_->Update();
}
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {
@@ -204,11 +222,13 @@
latch_delay_count_--;
} else if (latch_piston_state_ && latch_delay_count_ < 0) {
LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
- EXPECT_GE(last_voltage_, 1) << ": Must preload the gearbox when firing.";
if (latch_delay_count_ == -1) {
latch_piston_state_ = false;
- EXPECT_TRUE(brake_piston_state_)
- << ": Must have the brake set when releasing the latch.";
+ if (GetAbsolutePosition() > 0.002) {
+ EXPECT_TRUE(brake_piston_state_) << ": Must have the brake set when "
+ "releasing the latch for "
+ "powerful shots..";
+ }
plunger_latched_ = false;
// TODO(austin): The brake should be set for a number of cycles after
// this as well.
@@ -230,7 +250,7 @@
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
// true latch closed
- int latch_piston_state_;
+ bool latch_piston_state_;
// greater than zero, delaying close. less than zero delaying open
int latch_delay_count_;
@@ -238,7 +258,7 @@
bool plunger_latched_;
// true brake locked
- int brake_piston_state_;
+ bool brake_piston_state_;
// greater than zero, delaying close. less than zero delaying open
int brake_delay_count_;
@@ -264,6 +284,10 @@
ShooterMotor shooter_motor_;
ShooterSimulation shooter_motor_plant_;
+ void Reinitialize(double position) {
+ shooter_motor_plant_.Reinitialize(position);
+ }
+
ShooterTest()
: shooter_queue_group_(
".frc971.control_loops.shooter_queue_group", 0xcbf22ba9,
@@ -335,10 +359,11 @@
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
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();
- for (int i = 0; i < 100; ++i) {
+ for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -349,16 +374,20 @@
bool hit_preparefire = false;
bool hit_fire = false;
- for (int i = 0; i < 100; ++i) {
+ for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- LOG(DEBUG, "MOTORSTATE = %d\n", shooter_motor_.state());
if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
hit_preparefire = true;
}
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
+ if (!hit_fire) {
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_requested(false)
+ .Send();
+ }
hit_fire = true;
}
}
@@ -370,6 +399,211 @@
EXPECT_TRUE(hit_fire);
}
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, FireLong) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+
+ bool hit_preparefire = false;
+ bool hit_fire = false;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
+ hit_preparefire = true;
+ }
+ if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
+ if (!hit_fire) {
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_requested(false)
+ .Send();
+ }
+ 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_.state());
+ EXPECT_TRUE(hit_preparefire);
+ EXPECT_TRUE(hit_fire);
+}
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, MoveGoal) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
+
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(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_.state());
+}
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, Unload) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+}
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, UnloadWindupNegative) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ int kicked_delay = 20;
+ int capped_goal_count = 0;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
+ LOG(DEBUG, "State is UnloadMove\n");
+ --kicked_delay;
+ if (kicked_delay == 0) {
+ shooter_motor_.shooter_.R(0, 0) -= 100;
+ }
+ }
+ if (shooter_motor_.capped_goal()) {
+ ++capped_goal_count;
+ }
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+ EXPECT_LE(1, capped_goal_count);
+ EXPECT_GE(3, capped_goal_count);
+}
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, UnloadWindupPositive) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ int kicked_delay = 20;
+ int capped_goal_count = 0;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
+ LOG(DEBUG, "State is UnloadMove\n");
+ --kicked_delay;
+ if (kicked_delay == 0) {
+ shooter_motor_.shooter_.R(0, 0) += 0.1;
+ }
+ }
+ if (shooter_motor_.capped_goal()) {
+ ++capped_goal_count;
+ }
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+ EXPECT_LE(1, capped_goal_count);
+ EXPECT_GE(3, capped_goal_count);
+}
+
+double HallEffectMiddle(constants::Values::AnglePair pair) {
+ return (pair.lower_angle + pair.upper_angle) / 2.0;
+}
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, StartsOnDistal) {
+ Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ // 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_.state());
+}
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, StartsOnProximal) {
+ Reinitialize(
+ HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 300; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ // 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_.state());
+}
+
+// TODO(austin): Slip the encoder somewhere.
+
// TODO(austin): Test all the timeouts...
} // namespace testing
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index de33fa7..50642b9 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -7,9 +7,9 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
+StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 0.998324052598, 0.0007783475087, 0.000278701304898, -0.181614418697, -0.000138907346386, 0.0302015298419, 0.0, 0.0, 1.0;
+ A << 0.997145287595, 0.00115072867987, 0.000356210952805, -0.322204030364, -0.000199174994385, 0.0402046120149, 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;
@@ -23,23 +23,49 @@
return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
}
+StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00115359397892, 0.000356613321821, 0.0, 0.000172163011452, 0.0403047209622, 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;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 0.996946112601, 10.71141318, 224.213599484;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 121.388812879, 5.06126911425, 0.196946112601;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeSprungShooterPlantCoefficients());
+}
+
StateFeedbackController<3, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 0.998185145251, 11.8167175789, 298.617717297;
+ L << 1.00017216301, 11.0141047888, 223.935057347;
Eigen::Matrix<double, 1, 3> K;
- K << 162.58140285, 6.68264124674, 0.198185145251;
+ K << 122.81439697, 5.05065025388, 0.200172163011;
return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
StateFeedbackPlant<3, 1, 1> MakeShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeShooterPlantCoefficients());
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(2);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeSprungShooterPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeShooterPlantCoefficients());
return StateFeedbackPlant<3, 1, 1>(plants);
}
StateFeedbackLoop<3, 1, 1> MakeShooterLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeShooterController());
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(2);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeSprungShooterController());
+ controllers[1] = new StateFeedbackController<3, 1, 1>(MakeShooterController());
return StateFeedbackLoop<3, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
index f23a38f..968fd04 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -6,6 +6,10 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeSprungShooterController();
+
StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients();
StateFeedbackController<3, 1, 1> MakeShooterController();
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index fce7579..4a0e6f2 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.998324052598, 0.0007783475087, -0.181614418697, -0.000138907346386;
+ A << 0.997145287595, 0.00115072867987, -0.322204030364, -0.000199174994385;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000278701304898, 0.0302015298419;
+ B << 0.000356210952805, 0.0402046120149;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -23,23 +23,49 @@
return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00115359397892, 0.0, 0.000172163011452;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000356613321821, 0.0403047209622;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.896946112601, 1.86767549049;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 743.451871215, -4.17563006819;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawSprungShooterPlantCoefficients());
+}
+
StateFeedbackController<2, 1, 1> MakeRawShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.898185145251, 3.04818975205;
+ L << 0.900172163011, 2.15224193635;
Eigen::Matrix<double, 1, 2> K;
- K << 994.822639009, -5.92927654062;
+ K << 750.532425926, -4.15528738406;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
}
StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawShooterPlantCoefficients());
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(2);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawSprungShooterPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawShooterPlantCoefficients());
return StateFeedbackPlant<2, 1, 1>(plants);
}
StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawShooterController());
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(2);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawSprungShooterController());
+ controllers[1] = new StateFeedbackController<2, 1, 1>(MakeRawShooterController());
return StateFeedbackLoop<2, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
index 4deea48..26504ce 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
@@ -6,6 +6,10 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawSprungShooterController();
+
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients();
StateFeedbackController<2, 1, 1> MakeRawShooterController();
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index c58d7e5..0ef0df5 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -324,6 +324,7 @@
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
if (new_y_) {
+ LOG(INFO, "Got Y. R is (%f, %f, %f)\n", R(0, 0), R(1, 0), R(2, 0));
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {
@@ -344,7 +345,7 @@
}
}
- void controller_index() const { return controller_index_; }
+ int controller_index() const { return controller_index_; }
protected:
::std::vector<StateFeedbackController<number_of_states, number_of_inputs,