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/constants.cc b/y2022/constants.cc
index b00e1e8..7f9cd25 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -34,7 +34,6 @@
Values r;
- // TODO(Yash): Set constants
// Intake constants.
auto *const intake_front = &r.intake_front;
auto *const intake_back = &r.intake_back;
@@ -62,16 +61,24 @@
intake_front->subsystem_params = intake_params;
intake_back->subsystem_params = intake_params;
- // TODO(Yash): Set constants
// Turret constants.
auto *const turret = &r.turret;
auto *const turret_params = &turret->subsystem_params;
+ auto *turret_range = &r.turret_range;
+
+ *turret_range = ::frc971::constants::Range{
+ .lower_hard = -7.0, // Back Hard
+ .upper_hard = 3.4, // Front Hard
+ .lower = -6.5, // Back Soft
+ .upper = 3.15 // Front Soft
+ };
turret_params->zeroing_voltage = 4.0;
turret_params->operating_voltage = 12.0;
turret_params->zeroing_profile_params = {0.5, 2.0};
turret_params->default_profile_params = {10.0, 20.0};
- turret_params->range = Values::kTurretRange();
+ turret_params->default_profile_params = {15.0, 20.0};
+ turret_params->range = *turret_range;
turret_params->make_integral_loop =
control_loops::superstructure::turret::MakeIntegralTurretLoop;
turret_params->zeroing_constants.average_filter_size =
@@ -237,21 +244,25 @@
break;
case kPracticeTeamNumber:
+ // TODO(milind): calibrate once mounted
climber->potentiometer_offset = 0.0;
- intake_front->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 3.06604378582351;
intake_front->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- intake_back->potentiometer_offset = 0.0;
+ .measured_absolute_position = 0.318042402595181;
+ intake_back->potentiometer_offset = 3.10861174832838;
intake_back->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- turret->potentiometer_offset = 0.0;
+ .measured_absolute_position = 0.140554083520329;
+ turret->potentiometer_offset = -8.14418207451834 + 0.342635491808218;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.0;
- flipper_arm_left->potentiometer_offset = 0.0;
- flipper_arm_right->potentiometer_offset = 0.0;
+ 1.15423161235124;
+ turret_range->upper = 3.0;
+ turret_params->range = *turret_range;
+ flipper_arm_left->potentiometer_offset = -4.39536583413615;
+ flipper_arm_right->potentiometer_offset = 4.36264091401229;
- catapult_params->zeroing_constants.measured_absolute_position = 0.0;
- catapult->potentiometer_offset = 0.0;
+ catapult_params->zeroing_constants.measured_absolute_position =
+ 1.62909518684227;
+ catapult->potentiometer_offset = -1.52951814169821 - 0.0200812009850977;
break;
case kCodingRobotTeamNumber:
diff --git a/y2022/constants.h b/y2022/constants.h
index 4b5351e..f410014 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -44,8 +44,10 @@
// Climber
static constexpr ::frc971::constants::Range kClimberRange() {
- return ::frc971::constants::Range{
- .lower_hard = -0.01, .upper_hard = 0.59, .lower = 0.003, .upper = 0.555};
+ return ::frc971::constants::Range{.lower_hard = -0.01,
+ .upper_hard = 0.59,
+ .lower = 0.003,
+ .upper = 0.555};
}
static constexpr double kClimberPotMetersPerRevolution() {
return 22 * 0.25 * 0.0254;
@@ -109,16 +111,7 @@
// Turret
PotAndAbsEncoderConstants turret;
-
- // TODO (Yash): Constants need to be tuned
- static constexpr ::frc971::constants::Range kTurretRange() {
- return ::frc971::constants::Range{
- .lower_hard = -7.0, // Back Hard
- .upper_hard = 3.4, // Front Hard
- .lower = -6.5, // Back Soft
- .upper = 3.15 // Front Soft
- };
- }
+ frc971::constants::Range turret_range;
static constexpr double kTurretBackIntakePos() { return -M_PI; }
static constexpr double kTurretFrontIntakePos() { return 0; }
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,