Tune auto to make 5 balls for real
Change-Id: Iebfcb29259a27f2cf6c6fcbc794c022972228212
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult_plotter.ts b/y2022/control_loops/superstructure/catapult_plotter.ts
index 6c66340..5736426 100644
--- a/y2022/control_loops/superstructure/catapult_plotter.ts
+++ b/y2022/control_loops/superstructure/catapult_plotter.ts
@@ -14,6 +14,7 @@
const goal = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Goal');
const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
// Robot Enabled/Disabled and Mode
@@ -34,6 +35,7 @@
positionPlot.addMessageLine(status, ['catapult', 'position']).setColor(GREEN).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'velocity']).setColor(PINK).setPointSize(1.0);
positionPlot.addMessageLine(status, ['catapult', 'calculated_velocity']).setColor(BROWN).setPointSize(1.0);
+ positionPlot.addMessageLine(position, ['catapult', 'pot']).setColor(WHITE).setPointSize(1.0);
positionPlot.addMessageLine(status, ['catapult', 'estimator_state', 'position']).setColor(CYAN).setPointSize(1.0);
const voltagePlot =
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 6daea37..0e71656 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -386,11 +386,6 @@
reseating_in_catapult_ = true;
break;
}
- // If we started firing and the flippers closed a bit, estop to prevent
- // damage
- if (fire_ && !flippers_open_) {
- catapult_.Estop();
- }
// If the turret reached the aiming goal and the catapult is safe to move
// up, fire!
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 6c19052..9288da1 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -755,7 +755,7 @@
SendRobotVelocity(3.0);
- constexpr double kTurretGoal = 3.0;
+ constexpr double kTurretGoal = 2.0;
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -1015,7 +1015,7 @@
SetEnabled(true);
WaitUntilZeroed();
- constexpr double kTurretGoal = -4.0;
+ constexpr double kTurretGoal = -6.0;
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -1033,20 +1033,20 @@
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
- superstructure_plant_.set_intake_beambreak_back(true);
+ superstructure_plant_.set_intake_beambreak_front(true);
RunFor(dt() * 2);
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
- IntakeState::INTAKE_BACK_BALL);
+ IntakeState::INTAKE_FRONT_BALL);
RunFor(std::chrono::seconds(3));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
- -constants::Values::kTurretBackIntakePos(), 0.001);
+ -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 0.001);
// it chooses -pi because -pi is closer to -4 than positive pi
}