Merge "Speed up catapult"
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 6577bff..4717f1a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -281,19 +281,12 @@
(turret_intake_state_ == RequestedIntake::kFront
? constants::Values::kTurretFrontIntakePos()
: constants::Values::kTurretBackIntakePos());
- // Turn to the loading position as close to the current position as
- // possible.
+ // Turn to the loading position as close to the middle of the range as
+ // possible. Do the unwraping before we have a ball so we don't have to unwrap
+ // to shoot.
turret_loading_position =
- turret_.estimated_position() +
- aos::math::NormalizeAngle(turret_loading_position -
- turret_.estimated_position());
- // if out of range, reset back to within +/- pi of zero.
- if (turret_loading_position > constants::Values::kTurretRange().upper ||
- turret_loading_position < constants::Values::kTurretRange().lower) {
- turret_loading_position =
- frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
- turret_loading_position, 2.0 * M_PI);
- }
+ frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
+ turret_loading_position, 2.0 * M_PI);
turret_loading_goal_buffer.Finish(
frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index fef165b..86d7878 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -998,7 +998,7 @@
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
- -constants::Values::kTurretBackIntakePos(), 0.001);
+ constants::Values::kTurretBackIntakePos(), 0.001);
// Since the intake beambreak hasn't triggered in a while, it should realize
// the ball was lost.
@@ -1010,7 +1010,7 @@
IntakeState::NO_BALL);
}
-TEST_F(SuperstructureTest, TestTurretWrapsWhenLoading) {
+TEST_F(SuperstructureTest, TestTurretUnWrapsWhenLoading) {
SetEnabled(true);
WaitUntilZeroed();
@@ -1032,21 +1032,22 @@
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
- superstructure_plant_.set_intake_beambreak_front(true);
+ superstructure_plant_.set_intake_beambreak_back(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_FRONT_BALL);
+ IntakeState::INTAKE_BACK_BALL);
RunFor(std::chrono::seconds(3));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
- -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 0.001);
- // it chooses -pi because -pi is closer to -4 than positive pi
+ constants::Values::kTurretBackIntakePos(), 0.001);
+ // It goes to -pi instead of +pi because -pi is closest to the center of the
+ // range at -1.675.
}
// Make sure that the front and back intakes are never switched
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 196ee96..442a156 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -398,6 +398,15 @@
"num_senders": 1
},
{
+ "name": "/logger/camera/decimated",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "logger",
+ "logger": "LOCAL_LOGGER",
+ "frequency": 2,
+ "max_size": 620000,
+ "num_senders": 2
+ },
+ {
"name": "/pi1/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi1",
@@ -521,6 +530,13 @@
"nodes": [
"logger"
]
+ },
+ {
+ "name": "image_decimator",
+ "executable_name": "image_decimator",
+ "nodes": [
+ "logger"
+ ]
}
],
"nodes": [
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index bcd3f6b..9a7ba99 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -166,7 +166,7 @@
"source_node": "pi{{ NUM }}",
"frequency": 2,
"max_size": 620000,
- "num_senders": 18
+ "num_senders": 2
},
{
"name": "/pi{{ NUM }}/camera",