Fixed the shooter constants, a bunch of initialization bugs, and made it work without springs.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 763f005..4789e1d 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -174,7 +174,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_controller_index(1);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -216,16 +216,24 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->plunger && position->latch) {
- // The plunger is back and we are latched, get ready to shoot.
- state_ = STATE_PREPARE_SHOT;
- latch_piston_ = true;
- } else if (position->pusher_distal.current) {
+ if (position->pusher_distal.current) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
state_ = STATE_LOAD_BACKTRACK;
- latch_piston_ = false;
+
+ // The plunger is already back and latched. Don't release it.
+ if (position->plunger && position->latch) {
+ latch_piston_ = true;
+ } else {
+ latch_piston_ = false;
+ }
+ } else if (position->plunger && position->latch) {
+ // The plunger is back and we are latched. We most likely got here
+ // from Initialize, in which case we want to 'load' again anyways to
+ // zero.
+ Load();
+ latch_piston_ = true;
} else {
// Off the sensor, start loading.
Load();
@@ -248,7 +256,7 @@
values.shooter.zeroing_speed);
}
cap_goal = true;
- shooter_.set_max_voltage(12.0);
+ shooter_.set_max_voltage(4.0);
if (position) {
if (!position->pusher_distal.current) {
@@ -289,17 +297,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current;
+ latch_piston_ = position->pusher_distal.current || position->plunger;
- // Check if we are latched and back.
- if (position->plunger && position->latch) {
+ // Check if we are latched and back. Make sure the plunger is all the
+ // way back as well.
+ if (position->plunger && position->latch &&
+ position->pusher_distal.current) {
state_ = STATE_PREPARE_SHOT;
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = Time::Now() + Time::InSeconds(0.5);
+ loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
}
}
if (load_timeout_ < Time::Now()) {
@@ -354,7 +364,7 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = Time::Now() + Time::InSeconds(0.05);
+ shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
@@ -384,8 +394,7 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
apply_some_voltage = true;
state_ = STATE_PREPARE_FIRE;
}
@@ -408,14 +417,13 @@
shooter_loop_disable = true;
if (disabled) {
// If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
break;
}
if (Time::Now() > prepare_fire_end_time_) {
cycles_not_moved_ = 0;
firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
state_ = STATE_FIRE;
latch_piston_ = false;
} else {
@@ -432,7 +440,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
}
}
}
@@ -485,7 +493,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
if (Time::Now() > unload_timeout_) {
@@ -498,11 +506,11 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(6.0);
+ shooter_.set_max_voltage(5.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 322a556..195894b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -107,6 +107,13 @@
bool capped_goal_;
};
+const Time kUnloadTimeout = Time::InSeconds(10);
+const Time kLoadTimeout = Time::InSeconds(10);
+const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+const Time kShotEndTimeout = Time::InSeconds(1.0);
+const Time kPrepareFireEndTime = Time::InMS(40);
+
class ShooterMotor
: public aos::control_loops::ControlLoop<control_loops::ShooterGroup> {
public:
@@ -150,12 +157,12 @@
// Enter state STATE_UNLOAD
void Unload() {
state_ = STATE_UNLOAD;
- unload_timeout_ = Time::Now() + Time::InSeconds(1);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
// Enter state STATE_LOAD
void Load() {
state_ = STATE_LOAD;
- load_timeout_ = Time::Now() + Time::InSeconds(1);
+ load_timeout_ = Time::Now() + kLoadTimeout;
}
control_loops::ShooterGroup::Position last_position_;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 74cb0ec..8aa4c27 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -606,6 +606,9 @@
// TODO(austin): Test all the timeouts...
+// TODO(austin): Test starting latched and with the plunger back.
+// TODO(austin): Verify that we zeroed if we started latched and all the way back.
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 50642b9..546387d 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -9,7 +9,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 0.997145287595, 0.00115072867987, 0.000356210952805, -0.322204030364, -0.000199174994385, 0.0402046120149, 0.0, 0.0, 1.0;
+ A << 0.999391114909, 0.00811316740387, 7.59766686183e-05, -0.113584343654, 0.64780421498, 0.0141730519709, 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;
@@ -25,7 +25,7 @@
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;
+ A << 1.0, 0.00811505488455, 7.59852687598e-05, 0.0, 0.648331305446, 0.0141763492481, 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;
@@ -41,17 +41,17 @@
StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 0.996946112601, 10.71141318, 224.213599484;
+ L << 1.64719532989, 57.0572680832, 636.74290365;
Eigen::Matrix<double, 1, 3> K;
- K << 121.388812879, 5.06126911425, 0.196946112601;
+ K << 450.571849185, 11.8404918938, 0.997195329889;
return StateFeedbackController<3, 1, 1>(L, K, MakeSprungShooterPlantCoefficients());
}
StateFeedbackController<3, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 1.00017216301, 11.0141047888, 223.935057347;
+ L << 1.64833130545, 57.2417604572, 636.668851906;
Eigen::Matrix<double, 1, 3> K;
- K << 122.81439697, 5.05065025388, 0.200172163011;
+ K << 349.173113146, 8.65077618169, 0.848331305446;
return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index 4a0e6f2..42b166f 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.997145287595, 0.00115072867987, -0.322204030364, -0.000199174994385;
+ A << 0.999391114909, 0.00811316740387, -0.113584343654, 0.64780421498;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356210952805, 0.0402046120149;
+ B << 7.59766686183e-05, 0.0141730519709;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00115359397892, 0.0, 0.000172163011452;
+ A << 1.0, 0.00811505488455, 0.0, 0.648331305446;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356613321821, 0.0403047209622;
+ B << 7.59852687598e-05, 0.0141763492481;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -41,17 +41,17 @@
StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.896946112601, 1.86767549049;
+ L << 1.54719532989, 43.9345489758;
Eigen::Matrix<double, 1, 2> K;
- K << 743.451871215, -4.17563006819;
+ K << 2126.06977433, 41.3223370936;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawSprungShooterPlantCoefficients());
}
StateFeedbackController<2, 1, 1> MakeRawShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.900172163011, 2.15224193635;
+ L << 1.54833130545, 44.1155797675;
Eigen::Matrix<double, 1, 2> K;
- K << 750.532425926, -4.15528738406;
+ K << 2133.83569145, 41.3499425476;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
}