Make aimer code crash less
Change-Id: I15e5d1c5b4cebe8abf327d0656a6496d84f32def
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 5bb1d19..a21c09d 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -9,7 +9,7 @@
using y2024::control_loops::superstructure::Aimer;
// When the turret is at 0 the note will be leaving the robot at PI.
-static constexpr double kTurretZeroOffset = M_PI;
+static constexpr double kTurretZeroOffset = M_PI - 0.22;
Aimer::Aimer(aos::EventLoop *event_loop,
const y2024::Constants *robot_constants)
@@ -29,12 +29,20 @@
frc971::control_loops::aiming::ShotMode shot_mode,
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
*turret_goal) {
+ if (status == nullptr) {
+ return;
+ }
const frc971::control_loops::Pose robot_pose({status->x(), status->y(), 0},
status->theta());
- joystick_state_fetcher_.Fetch();
- CHECK_NOTNULL(joystick_state_fetcher_.get());
+ aos::Alliance alliance = aos::Alliance::kRed;
+ if (!joystick_state_fetcher_.Fetch() && !received_joystick_state_) {
+ received_joystick_state_ = false;
+ } else {
+ received_joystick_state_ = true;
- aos::Alliance alliance = joystick_state_fetcher_->alliance();
+ CHECK_NOTNULL(joystick_state_fetcher_.get());
+ alliance = joystick_state_fetcher_->alliance();
+ }
const frc971::control_loops::Pose red_alliance_goal(
frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
@@ -68,12 +76,11 @@
robot_constants_->common()->turret()->range()),
interpolation_table_.Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.0, kTurretZeroOffset},
+ /*wrap_mode=*/0.15, kTurretZeroOffset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
turret_goal->set_unsafe_goal(current_goal_.position);
- turret_goal->set_goal_velocity(current_goal_.velocity);
}
flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 21d427a..9bec187 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -45,6 +45,8 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
frc971::control_loops::aiming::TurretGoal current_goal_;
+
+ bool received_joystick_state_ = false;
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 0b39ec6..b96bb65 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1401,12 +1401,12 @@
EXPECT_NEAR(
-M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
- M_PI,
+ M_PI - 0.22,
5e-4);
- EXPECT_NEAR(
- -M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position() - M_PI,
- 5e-4);
+ EXPECT_NEAR(-M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() -
+ M_PI - 0.22,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
@@ -1447,12 +1447,12 @@
EXPECT_NEAR(
M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
- M_PI,
+ M_PI - 0.22,
5e-4);
- EXPECT_NEAR(
- M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position() + M_PI,
- 5e-4);
+ EXPECT_NEAR(M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() +
+ M_PI - 0.22,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
superstructure_status_fetcher_->shooter()->aimer()->target_distance());