Merge "Bringup main robot"
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index ec59033..3888e9a 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -8,7 +8,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-2-0_cam-24-03_2024-02-07_20-40-34.928600992.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-2-1_cam-24-04_2024-02-07_20-45-22.787382400.json' %}
}
],
"robot": {
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index f717765..38b63d7 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -8,7 +8,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_1970-01-05_17-40-29.245444672.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-07_2024-02-24_19-52-27.338466592.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json' %}
}
],
"robot": {
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
similarity index 93%
rename from y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
index 63f4ea8..b0bdadd 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
@@ -1,6 +1,6 @@
{
"node_name": "orin1",
- "team_number": 971,
+ "team_number": 7971,
"intrinsics": [
646.870789,
0.0,
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
similarity index 93%
rename from y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
index 482158c..d5d2d61 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
@@ -1,6 +1,6 @@
{
"node_name": "orin1",
- "team_number": 971,
+ "team_number": 7971,
"intrinsics": [
644.604858,
0.0,
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json b/y2024/constants/calib_files/calibration_orin-7971-2-0_cam-24-03_2024-02-07_20-40-34.928600992.json
similarity index 80%
rename from y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json
rename to y2024/constants/calib_files/calibration_orin-7971-2-0_cam-24-03_2024-02-07_20-40-34.928600992.json
index febd9ef..32d5db6 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-2-0_cam-24-03_2024-02-07_20-40-34.928600992.json
@@ -1,6 +1,6 @@
{
- "node_name": "orin1",
- "team_number": 971,
+ "node_name": "orin2",
+ "team_number": 7971,
"intrinsics": [
648.13446,
0.0,
@@ -21,5 +21,5 @@
],
"calibration_timestamp": 1707367234928600992,
"camera_id": "24-03",
- "camera_number": 1
+ "camera_number": 0
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json b/y2024/constants/calib_files/calibration_orin-7971-2-1_cam-24-04_2024-02-07_20-45-22.787382400.json
similarity index 80%
rename from y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json
rename to y2024/constants/calib_files/calibration_orin-7971-2-1_cam-24-04_2024-02-07_20-45-22.787382400.json
index a3ea964..4f76b07 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-2-1_cam-24-04_2024-02-07_20-45-22.787382400.json
@@ -1,6 +1,6 @@
{
- "node_name": "orin1",
- "team_number": 971,
+ "node_name": "orin2",
+ "team_number": 7971,
"intrinsics": [
642.80365,
0.0,
@@ -21,5 +21,5 @@
],
"calibration_timestamp": 1707367522787382400,
"camera_id": "24-04",
- "camera_number": 0
+ "camera_number": 1
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_1970-01-05_17-40-29.245444672.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_1970-01-05_17-40-29.245444672.json
new file mode 100755
index 0000000..ffdf16c
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_1970-01-05_17-40-29.245444672.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.644104,
+ 0.0,
+ 755.677979,
+ 0.0,
+ 648.522644,
+ 597.744812,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.25182,
+ 0.063137,
+ 0.000118,
+ 0.000005,
+ -0.006342
+ ],
+ "calibration_timestamp": 409229245444672,
+ "camera_id": "24-06",
+ "camera_number": 0,
+ "reprojection_error": 1.344104
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-07_2024-02-24_19-52-27.338466592.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-07_2024-02-24_19-52-27.338466592.json
new file mode 100755
index 0000000..b691f56
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-07_2024-02-24_19-52-27.338466592.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 647.822815,
+ 0.0,
+ 715.37616,
+ 0.0,
+ 647.799316,
+ 494.638641,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.2423,
+ 0.057169,
+ 0.000302,
+ 0.000016,
+ -0.005638
+ ],
+ "calibration_timestamp": 1708833147338466592,
+ "camera_id": "24-07",
+ "camera_number": 0,
+ "reprojection_error": 1.362672
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json
new file mode 100755
index 0000000..b5a848b
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 645.963562,
+ 0.0,
+ 751.21698,
+ 0.0,
+ 645.34906,
+ 605.204102,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.248733,
+ 0.06221,
+ -0.000901,
+ 0.000128,
+ -0.006595
+ ],
+ "calibration_timestamp": 1708820514420797344,
+ "camera_id": "24-08",
+ "camera_number": 0,
+ "reprojection_error": 1.591953
+}
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json
new file mode 100755
index 0000000..0912038
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 646.04834,
+ 0.0,
+ 703.327576,
+ 0.0,
+ 645.444458,
+ 527.86261,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.251594,
+ 0.064935,
+ 0.000479,
+ 0.000036,
+ -0.007207
+ ],
+ "calibration_timestamp": 1708821845975708672,
+ "camera_id": "24-10",
+ "camera_number": 0,
+ "reprojection_error": 1.523209
+}
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json
new file mode 100755
index 0000000..9ef782e
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.360168,
+ 0.0,
+ 729.818665,
+ 0.0,
+ 648.210327,
+ 641.988037,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.255473,
+ 0.068444,
+ 0.000028,
+ -0.000078,
+ -0.008004
+ ],
+ "calibration_timestamp": 409227793683328,
+ "camera_id": "24-05",
+ "camera_number": 1,
+ "reprojection_error": 1.058851
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-09_2024-02-24_16-10-16.872521280.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-09_2024-02-24_16-10-16.872521280.json
new file mode 100755
index 0000000..9eed9bf
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-09_2024-02-24_16-10-16.872521280.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.187805,
+ 0.0,
+ 736.903137,
+ 0.0,
+ 648.028687,
+ 557.169861,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.265564,
+ 0.078084,
+ -0.000231,
+ 0.000386,
+ -0.010425
+ ],
+ "calibration_timestamp": 1708819816872521280,
+ "camera_id": "24-09",
+ "camera_number": 1,
+ "reprojection_error": 1.881098
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json
new file mode 100755
index 0000000..4957b75
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 649.866699,
+ 0.0,
+ 709.355713,
+ 0.0,
+ 648.893066,
+ 576.101868,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.248092,
+ 0.060938,
+ 0.000313,
+ 0.00009,
+ -0.006163
+ ],
+ "calibration_timestamp": 1708821846986729504,
+ "camera_id": "24-11",
+ "camera_number": 1,
+ "reprojection_error": 1.450069
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json
new file mode 100755
index 0000000..0fda16d
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json
@@ -0,0 +1,26 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 647.19928,
+ 0.0,
+ 690.698181,
+ 0.0,
+ 646.449158,
+ 530.162842,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.249799,
+ 0.062593,
+ 0.00003,
+ 0.000366,
+ -0.006532
+ ],
+ "calibration_timestamp": 1708833159488095264,
+ "camera_id": "24-12",
+ "camera_number": 1,
+ "reprojection_error": 1.23409
+}
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 980efba..5bb1d19 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -8,6 +8,9 @@
using frc971::control_loops::aiming::ShotMode;
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;
+
Aimer::Aimer(aos::EventLoop *event_loop,
const y2024::Constants *robot_constants)
: event_loop_(event_loop),
@@ -65,7 +68,7 @@
robot_constants_->common()->turret()->range()),
interpolation_table_.Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.0, /*turret_zero_offset*/ 0.0},
+ /*wrap_mode=*/0.0, kTurretZeroOffset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index ce5b6a9..56980d5 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -99,6 +99,7 @@
if (state_ != SuperstructureState::READY &&
state_ != SuperstructureState::FIRING) {
state_ = SuperstructureState::READY;
+ catapult_requested_ = true;
}
}
@@ -137,7 +138,7 @@
// Checks if the extend is close enough to the retracted position to be
// considered ready to accept note from the transfer rollers.
- const bool extend_ready_for_transfer = PositionNear(
+ const bool extend_at_retracted = PositionNear(
extend_.position(), extend_set_points->retracted(), kExtendThreshold);
// If true, the turret should be moved to the position to avoid collision with
@@ -171,7 +172,7 @@
case SuperstructureState::IDLE:
if (unsafe_goal != nullptr &&
unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
- extend_ready_for_transfer) {
+ extend_at_retracted) {
state_ = SuperstructureState::INTAKING;
}
extend_goal = ExtendStatus::RETRACTED;
@@ -282,7 +283,7 @@
case NoteGoal::NONE:
extend_goal = ExtendStatus::RETRACTED;
move_turret_to_standby = true;
- if (extend_ready_for_transfer) {
+ if (extend_at_retracted) {
state_ = SuperstructureState::LOADED;
}
break;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 853979c..e0b9846 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -630,6 +630,10 @@
RunFor(chrono::seconds(10));
VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
}
// Tests that loops can reach a goal.
@@ -703,6 +707,9 @@
RunFor(chrono::seconds(15));
VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
}
// Makes sure that the voltage on a motor is properly pulled back after
@@ -753,6 +760,9 @@
RunFor(chrono::seconds(20));
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
@@ -797,6 +807,9 @@
RunFor(chrono::seconds(10));
VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
}
// Tests that the loop zeroes when run for a while without a goal.
@@ -853,6 +866,8 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
WaitUntilZeroed();
{
@@ -890,20 +905,8 @@
VerifyNearGoal();
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_intake_goal(IntakeGoal::INTAKE);
- goal_builder.add_note_goal(NoteGoal::NONE);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(5));
-
- VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
simulated_robot_constants_->common()
@@ -916,6 +919,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -926,6 +930,9 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
}
@@ -969,6 +976,8 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
simulated_robot_constants_->common()->turret_loading_position(),
0.01);
@@ -1005,6 +1014,9 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
simulated_robot_constants_->common()->turret_loading_position(),
0.01);
@@ -1043,11 +1055,15 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
ExtendStatus::RETRACTED);
+
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::TRANSFERING_TO_EXTEND);
@@ -1060,6 +1076,9 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
@@ -1101,6 +1120,9 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADING_CATAPULT);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
@@ -1117,6 +1139,9 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::LOADED);
@@ -1132,9 +1157,6 @@
simulated_robot_constants_->common()->altitude_loading_position(),
0.01);
- EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
- CatapultState::LOADED);
-
// Fire. Start by triggering a motion and then firing all in 1 go.
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -1180,22 +1202,13 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::FIRING);
+
// Make sure it stays at firing for a bit.
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::FIRING);
- // Wheel should spin free again.
- superstructure_plant_.set_catapult_beambreak(false);
-
- RunFor(chrono::seconds(5));
-
- // And we should be back to ready.
- CHECK(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
- CatapultState::READY);
-
- VerifyNearGoal();
-
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
kTurretGoal, 0.001);
@@ -1273,6 +1286,9 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::LOADED);
}
@@ -1342,13 +1358,18 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
EXPECT_NEAR(
-M_PI_2,
- superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+ superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
+ M_PI,
5e-4);
- EXPECT_NEAR(-M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position(),
- 5e-4);
+ EXPECT_NEAR(
+ -M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() - M_PI,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
@@ -1383,13 +1404,18 @@
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
EXPECT_NEAR(
M_PI_2,
- superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+ superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
+ M_PI,
5e-4);
- EXPECT_NEAR(M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position(),
- 5e-4);
+ EXPECT_NEAR(
+ M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() + M_PI,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
superstructure_status_fetcher_->shooter()->aimer()->target_distance());
@@ -1494,6 +1520,9 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::FIRING);
+
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::SCORING_IN_AMP);
EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
diff --git a/y2024/vision/rename_calibration_file.sh b/y2024/vision/rename_calibration_file.sh
new file mode 100755
index 0000000..dcdf2f2
--- /dev/null
+++ b/y2024/vision/rename_calibration_file.sh
@@ -0,0 +1,86 @@
+#!/bin/bash
+
+# Helper script to rename the camera calibration file when moving to new robot
+
+# grep isn't happy with set
+# set -e
+
+
+usage_and_exit () {
+ echo
+ echo "Usage:"
+ echo "$0 ORIG_FILENAME NEW_TEAM_NUMBER NEW_ORIN_NUMBER NEW_CAMERA_NUMBER"
+ echo
+ exit 2
+}
+
+if [[ $# -ne 4 ]]; then
+ echo "ERROR: Requires 4 parameters"
+ usage_and_exit
+fi
+
+ORIG_FILENAME=$1
+NEW_TEAM_NUMBER=$2
+NEW_ORIN_NUMBER=$3
+NEW_CAMERA_NUMBER=$4
+
+if [[ ! -x ${ORIG_FILENAME} ]]; then
+ echo "${ORIG_FILENAME} does not exist"
+ usage_and_exit
+fi
+
+check_971=`echo "${NEW_TEAM_NUMBER}" | grep "971"`
+if [[ ${check_971} == "" ]]; then
+ echo "NEW_TEAM_NUMBER (${NEW_TEAM_NUMBER}) does not contain '971'"
+ usage_and_exit
+fi
+
+if [[ ${NEW_ORIN_NUMBER} != 1 && ${NEW_ORIN_NUMBER} != 2 ]]; then
+ echo "NEW_ORIN_NUMBER (${NEW_ORIN_NUMBER}) must be either 1 or 2"
+ usage_and_exit
+fi
+
+if [[ ${NEW_CAMERA_NUMBER} != 0 && ${NEW_CAMERA_NUMBER} != 1 ]]; then
+ echo "NEW_CAMERA_NUMBER (${NEW_CAMERA_NUMBER}) must be either 0 or 1"
+ usage_and_exit
+fi
+
+# Extract parts of the filename, based on just the basename
+# This assumes filenames of the form:
+# calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
+IFS='_' read -r -a name_parts <<< `basename "${ORIG_FILENAME}"`
+
+echo "For ${ORIG_FILENAME}:"
+for element in "${name_parts[@]}"
+do
+ echo "$element"
+done
+
+# Rename file based on this new info (be sure to handle paths properly)
+NEW_FILENAME=`dirname ${ORIG_FILENAME}`"/${name_parts[0]}_orin-${NEW_TEAM_NUMBER}-${NEW_ORIN_NUMBER}-${NEW_CAMERA_NUMBER}_${name_parts[2]}_${name_parts[3]}_${name_parts[4]}"
+
+echo
+echo "For camera id: ${name_parts[2]}"
+echo "Renaming from:"
+echo "${ORIG_FILENAME} to: "
+echo "${NEW_FILENAME}"
+echo
+echo "and changing from "
+echo "${name_parts[1]} to: "
+echo "orin-${NEW_TEAM_NUMBER}-${NEW_ORIN_NUMBER}-${NEW_CAMERA_NUMBER}"
+echo
+
+mv ${ORIG_FILENAME} ${NEW_FILENAME}
+
+
+echo "REPLACING ORIN_NUMBER"
+sed -i s/orin./orin${NEW_ORIN_NUMBER}/ ${NEW_FILENAME}
+
+echo "Replacing TEAM NUMBER"
+sed -i s/\"team_number\"\:\ [1-9]*\,/\"team_number\"\:\ ${NEW_TEAM_NUMBER},/ ${NEW_FILENAME}
+
+echo "REPLACING CAMERA_NUMBER"
+sed -i s/\"camera_number\"\:\ [0-9]/\"camera_number\"\:\ ${NEW_CAMERA_NUMBER}/ ${NEW_FILENAME}
+
+
+