Calibrate 2nd robot sensors
Also use different turret ranges for each robot.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I101531d63a59e20afe328bc8b0576fc2ef79c401
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 4717f1a..454e147 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -34,8 +34,8 @@
event_loop->MakeFetcher<CANPosition>("/superstructure")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- ball_color_fetcher_(event_loop->MakeFetcher<y2022::vision::BallColor>(
- "/superstructure")),
+ ball_color_fetcher_(
+ event_loop->MakeFetcher<y2022::vision::BallColor>("/superstructure")),
aimer_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -284,9 +284,8 @@
// 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 =
- frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
- turret_loading_position, 2.0 * M_PI);
+ turret_loading_position = frc971::zeroing::Wrap(
+ values_->turret_range.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 86d7878..cbc71d4 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -158,7 +158,7 @@
PositionSensorSimulator(
values->turret.subsystem_params.zeroing_constants
.one_revolution_distance),
- values->turret, constants::Values::kTurretRange(),
+ values->turret, values->turret_range,
values->turret.subsystem_params.zeroing_constants
.measured_absolute_position,
dt_),
@@ -178,7 +178,7 @@
intake_front_.InitializePosition(
constants::Values::kIntakeRange().middle());
intake_back_.InitializePosition(constants::Values::kIntakeRange().middle());
- turret_.InitializePosition(constants::Values::kTurretRange().middle());
+ turret_.InitializePosition(values->turret_range.middle());
catapult_.InitializePosition(constants::Values::kCatapultRange().middle());
climber_.InitializePosition(constants::Values::kClimberRange().middle());
@@ -566,7 +566,7 @@
superstructure_plant_.intake_back()->InitializePosition(
constants::Values::kIntakeRange().middle());
superstructure_plant_.turret()->InitializePosition(
- constants::Values::kTurretRange().middle());
+ values_->turret_range.middle());
superstructure_plant_.climber()->InitializePosition(
constants::Values::kClimberRange().middle());
WaitUntilZeroed();
@@ -584,7 +584,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().lower,
+ *builder.fbb(), values_->turret_range.lower,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -671,7 +671,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().lower,
+ *builder.fbb(), values_->turret_range.lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 5fe8a2c..6320cb7 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -8,8 +8,8 @@
namespace turret {
using frc971::control_loops::Pose;
-using frc971::control_loops::aiming::ShotConfig;
using frc971::control_loops::aiming::RobotState;
+using frc971::control_loops::aiming::ShotConfig;
namespace {
// If the turret is at zero, then it will be at this angle at which the shot
@@ -55,7 +55,7 @@
// Use the previous shot distance to estimate the speed-over-ground of the
// ball.
current_goal_ = frc971::control_loops::aiming::AimerGoal(
- ShotConfig{goal, shot_mode, constants_->kTurretRange(),
+ ShotConfig{goal, shot_mode, constants_->turret_range,
constants_->shot_velocity_interpolation_table
.Get(current_goal_.target_distance)
.shot_speed_over_ground,