Fixed shooter to not have a spring some times.
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 3c411fb..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,11 +31,11 @@
# 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
self.A_continuous = numpy.matrix(
[[0, 1],
@@ -63,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)
@@ -102,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)
@@ -145,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 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;
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index efb4480..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;
@@ -68,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) {
@@ -84,6 +89,11 @@
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).
@@ -93,6 +103,8 @@
double last_voltage_;
double uncapped_voltage_;
double offset_;
+ double max_voltage_;
+ bool capped_goal_;
};
class ShooterMotor
@@ -102,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);
@@ -132,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 98ea5a5..74cb0ec 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -91,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);
}
@@ -200,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) {
@@ -275,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,
@@ -346,6 +359,7 @@
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();
@@ -437,7 +451,7 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
- for (int i = 0; i < 50; ++i) {
+ for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -474,7 +488,120 @@
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
}
-// TODO(austin): Windup.
+// 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...
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,