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;
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();