Merge "Add 2022 superstructure replay"
diff --git a/y2022/constants.cc b/y2022/constants.cc
index b00e1e8..9d27780 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 =
@@ -210,15 +217,15 @@
0.0172237531191 - 0.0172237531191;
intake_front->potentiometer_offset =
- 2.79628370453323 - 0.0250288114832881 + 0.577152542437606;
+ 2.79628370453323 - 0.0250288114832881 + 0.577152542437606 + 0.476513825677792;
intake_front->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.26963366701647;
+ .measured_absolute_position = 0.205422145836751;
- intake_back->potentiometer_offset = 3.1409576474047 + 0.278653334013286 +
- 0.00879137908308503 +
- 0.0837134053818833;
+ intake_back->potentiometer_offset =
+ 3.1409576474047 + 0.278653334013286 + 0.00879137908308503 +
+ 0.0837134053818833 + 0.832945730100298 - 0.00759895654985426 - 2.03114758819475;
intake_back->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.15924088639178;
+ .measured_absolute_position = 0.352050723370449;
turret->potentiometer_offset =
-9.99970387166721 + 0.06415943 + 0.073290115367682 -
@@ -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/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index 04b8e8a..1d6f89c 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -61,7 +61,7 @@
static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
// Maximum position of the intake to avoid collisions
- static constexpr double kCollisionZoneIntake = 1.33;
+ static constexpr double kCollisionZoneIntake = 1.30;
// Tolerances for the subsystems
static constexpr double kEpsTurret = 0.05;
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 9642ae0..c59737d 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -239,7 +239,7 @@
back_intake_beambreak_timer_ = timestamp;
}
- // Check if we're either spitting of have lost the ball.
+ // Check if we're either spitting or have lost the ball.
if ((transfer_roller_speed < 0.0 && front_intake_has_ball_) ||
timestamp >
front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
@@ -281,12 +281,19 @@
(turret_intake_state_ == RequestedIntake::kFront
? constants::Values::kTurretFrontIntakePos()
: constants::Values::kTurretBackIntakePos());
- // 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.
+ // Turn to the loading position as close to the current position as
+ // possible.
turret_loading_position =
- frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
- turret_loading_position, 2.0 * M_PI);
+ 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 > values_->turret_range.upper ||
+ turret_loading_position < values_->turret_range.lower) {
+ 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..ff243e2 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>
@@ -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, TestTurretUnWrapsWhenLoading) {
+TEST_F(SuperstructureTest, TestTurretWrapsWhenLoading) {
SetEnabled(true);
WaitUntilZeroed();
@@ -1032,22 +1032,21 @@
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);
- // It goes to -pi instead of +pi because -pi is closest to the center of the
- // range at -1.675.
+ -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 0.001);
+ // it chooses -pi because -pi is closer to -4 than positive pi
}
// Make sure that the front and back intakes are never switched
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,
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 7fb6706..26c7f33 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -64,8 +64,9 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpitFront(3, 3);
-const ButtonLocation kSpitBack(3, 1);
+const ButtonLocation kSpitFront(4, 8);
+const ButtonLocation kSpitBack(4, 7);
+const ButtonLocation kSpit(3, 3);
const ButtonLocation kRedLocalizerReset(4, 14);
const ButtonLocation kBlueLocalizerReset(4, 13);
@@ -232,6 +233,13 @@
constexpr double kIntakePosition = -0.12;
constexpr size_t kIntakeCounterIterations = 25;
+ if (data.PosEdge(kSpit)) {
+ last_front_intake_has_ball_ =
+ superstructure_status_fetcher_->front_intake_has_ball();
+ last_back_intake_has_ball_ =
+ superstructure_status_fetcher_->back_intake_has_ball();
+ }
+
// Extend the intakes and spin the rollers.
// Don't let this happen if there is a ball in the other intake, because
// that would spit this one out.
@@ -249,10 +257,12 @@
intake_back_counter_ = kIntakeCounterIterations;
intake_front_counter_ = 0;
- } else if (data.IsPressed(kSpitFront)) {
+ } else if (data.IsPressed(kSpitFront) ||
+ (data.IsPressed(kSpit) && last_front_intake_has_ball_)) {
transfer_roller_speed = -kTransferRollerSpeed;
intake_front_counter_ = 0;
- } else if (data.IsPressed(kSpitBack)) {
+ } else if (data.IsPressed(kSpitBack) ||
+ (data.IsPressed(kSpit) && last_back_intake_has_ball_)) {
transfer_roller_speed = kTransferRollerSpeed;
intake_back_counter_ = 0;
}
@@ -361,6 +371,9 @@
size_t intake_front_counter_ = 0;
size_t intake_back_counter_ = 0;
+
+ bool last_front_intake_has_ball_ = false;
+ bool last_back_intake_has_ball_ = false;
};
} // namespace joysticks
diff --git a/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json b/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-07_2022-02-16_21-20-00.000000000.json
similarity index 93%
rename from y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
rename to y2022/vision/calib_files/calibration_pi-9971-4_cam-22-07_2022-02-16_21-20-00.000000000.json
index 6dbd3b2..fbc23f9 100755
--- a/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
+++ b/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-07_2022-02-16_21-20-00.000000000.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi7",
+ "node_name": "pi4",
"team_number": 9971,
"intrinsics": [
388.062378,
diff --git a/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-04_2022-01-28_05-26-43.135661745.json
similarity index 93%
rename from y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
rename to y2022/vision/calib_files/calibration_pi-9971-7_cam-22-04_2022-01-28_05-26-43.135661745.json
index 5466224..3a964fc 100755
--- a/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
+++ b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-04_2022-01-28_05-26-43.135661745.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi4",
+ "node_name": "pi7",
"team_number": 9971,
"intrinsics": [
386.619232,
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index f59f2cf..8d1c614 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -100,16 +100,16 @@
if pi_number == "pi1":
camera_yaw = 90.0 * np.pi / 180.0
- T = np.array([-8.25 * 0.0254, 3.25 * 0.0254, 32.0 * 0.0254])
+ T = np.array([-11.0 * 0.0254, 5.5 * 0.0254, 29.5 * 0.0254])
elif pi_number == "pi2":
camera_yaw = 0.0
- T = np.array([-7.5 * 0.0254, -3.5 * 0.0254, 34.0 * 0.0254])
+ T = np.array([-9.5 * 0.0254, -3.5 * 0.0254, 34.5 * 0.0254])
elif pi_number == "pi3":
- camera_yaw = 178.5 * np.pi / 180.0
- T = np.array([-1.0 * 0.0254, 8.5 * 0.0254, 34.25 * 0.0254])
+ camera_yaw = 179.0 * np.pi / 180.0
+ T = np.array([-9.5 * 0.0254, 3.5 * 0.0254, 34.5 * 0.0254])
elif pi_number == "pi4":
camera_yaw = -90.0 * np.pi / 180.0
- T = np.array([-9.0 * 0.0254, -5 * 0.0254, 27.5 * 0.0254])
+ T = np.array([-10.25 * 0.0254, -5.0 * 0.0254, 27.5 * 0.0254])
return compute_extrinsic(camera_pitch, camera_yaw, T, is_turret)
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 442a156..6c33eec 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -483,7 +483,7 @@
},
{
"match": {
- "name": "/camera",
+ "name": "/camera*",
"source_node": "logger"
},
"rename": {
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 8651923..1a5a1aa 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -7,7 +7,8 @@
"frequency": 100,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes" : [
- "imu"
+ "imu",
+ "logger"
],
"destination_nodes": [
{
@@ -18,6 +19,15 @@
"timestamp_logger_nodes": [
"roborio"
]
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
}
]
},
@@ -31,6 +41,15 @@
"max_size": 200
},
{
+ "name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/roborio/aos",
"type": "aos.RobotState",
"source_node": "roborio",