Merge "Add a three piece auto which takes center notes"
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index 42686a1..7e8a55c 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -31,8 +31,8 @@
"Mode to use. Don't change unless you know what you are doing.");
DEFINE_int32(camera, 0, "Camera number");
DEFINE_int32(mode, 0, "Mode number to use.");
-DEFINE_int32(exposure, 200000, "Exposure number to use.");
-DEFINE_int32(gain, 5, "gain number to use.");
+DEFINE_int32(exposure, 100000, "Exposure number to use.");
+DEFINE_int32(gain, 8, "gain number to use.");
DEFINE_int32(width, 1456, "Image width");
DEFINE_int32(height, 1088, "Image height");
DEFINE_double(rgain, 1.0, "R gain");
@@ -226,8 +226,8 @@
{
auto range = i_sensor_mode->getFrameDurationRange();
- LOG(INFO) << "Min: " << range.min() << ", " << range.max();
- LOG(INFO) << "type " << i_sensor_mode->getSensorModeType().getName();
+ LOG(INFO) << "Frame duration min: " << range.min() << ", " << range.max()
+ << ", type " << i_sensor_mode->getSensorModeType().getName();
}
// Create the capture session using the first device and get the core
@@ -326,21 +326,22 @@
Argus::interface_cast<Argus::IAutoControlSettings>(
i_request->getAutoControlSettings());
CHECK(i_auto_control_settings != nullptr);
- i_auto_control_settings->setAwbMode(Argus::AWB_MODE_OFF);
+ CHECK_EQ(i_auto_control_settings->setAwbMode(Argus::AWB_MODE_OFF),
+ Argus::STATUS_OK);
i_auto_control_settings->setAeLock(false);
Argus::Range<float> isp_digital_gain_range;
isp_digital_gain_range.min() = 1;
isp_digital_gain_range.max() = 1;
- i_auto_control_settings->setIspDigitalGainRange(isp_digital_gain_range);
+ CHECK_EQ(
+ i_auto_control_settings->setIspDigitalGainRange(isp_digital_gain_range),
+ Argus::STATUS_OK);
Argus::IEdgeEnhanceSettings *i_ee_settings =
Argus::interface_cast<Argus::IEdgeEnhanceSettings>(request_);
CHECK(i_ee_settings != nullptr);
- i_ee_settings->setEdgeEnhanceStrength(0);
-
- i_request->enableOutputStream(output_stream_.get());
+ CHECK_EQ(i_ee_settings->setEdgeEnhanceStrength(0), Argus::STATUS_OK);
Argus::ISourceSettings *i_source_settings =
Argus::interface_cast<Argus::ISourceSettings>(
@@ -349,17 +350,22 @@
i_source_settings->setFrameDurationRange(
i_sensor_mode->getFrameDurationRange().min());
- i_source_settings->setSensorMode(sensor_modes[FLAGS_mode]);
+ CHECK_EQ(i_source_settings->setSensorMode(sensor_modes[FLAGS_mode]),
+ Argus::STATUS_OK);
Argus::Range<float> sensor_mode_analog_gain_range;
sensor_mode_analog_gain_range.min() = FLAGS_gain;
sensor_mode_analog_gain_range.max() = FLAGS_gain;
- i_source_settings->setGainRange(sensor_mode_analog_gain_range);
+ CHECK_EQ(i_source_settings->setGainRange(sensor_mode_analog_gain_range),
+ Argus::STATUS_OK);
Argus::Range<uint64_t> limit_exposure_time_range;
limit_exposure_time_range.min() = FLAGS_exposure;
limit_exposure_time_range.max() = FLAGS_exposure;
- i_source_settings->setExposureTimeRange(limit_exposure_time_range);
+ CHECK_EQ(i_source_settings->setExposureTimeRange(limit_exposure_time_range),
+ Argus::STATUS_OK);
+
+ i_request->enableOutputStream(output_stream_.get());
}
void Start() {
diff --git a/frc971/orin/gpu_apriltag.cc b/frc971/orin/gpu_apriltag.cc
index 87d5f49..425e45f 100644
--- a/frc971/orin/gpu_apriltag.cc
+++ b/frc971/orin/gpu_apriltag.cc
@@ -282,12 +282,12 @@
if (!converged) {
VLOG(1) << "Rejecting detection because Undistort failed to coverge";
- // Send rejected corner points to foxglove in red
+ // Send corner points rejected to to lack of convergence in orange
std::vector<cv::Point2f> rejected_corner_points =
MakeCornerVector(gpu_detection);
foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
builder.fbb(), eof, rejected_corner_points,
- std::vector<double>{1.0, 0.0, 0.0, 0.5}));
+ std::vector<double>{1.0, 0.65, 0.0, 0.5}));
rejections_++;
continue;
}
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index d3ddba7..e0ec38c 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -44,6 +44,13 @@
"camera moves.";
std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
+ CHECK(aos::network::ParsePiOrOrin(hostname))
+ << "Failed to parse node type from " << hostname
+ << ". Should be of form orin1-971-1";
+ CHECK(aos::network::ParsePiOrOrinNumber(hostname))
+ << "Failed to parse node number from " << hostname
+ << ". Should be of form orin2-7971-1";
+
std::string camera_name = absl::StrCat(
"/", aos::network::ParsePiOrOrin(hostname).value(),
std::to_string(aos::network::ParsePiOrOrinNumber(hostname).value()),
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 710f80b..a8512c3 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -57,11 +57,11 @@
"turret_constants": {
{% set _ = turret_zero.update(
{
- "measured_absolute_position" : 0.2077
+ "measured_absolute_position" : 1.3715
}
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -6.47164779835404 - 0.0711209027239817 + 1.0576004531907 - 0.343 }}
+ "potentiometer_offset": {{ -6.47164779835404 - 0.0711209027239817 + 1.0576004531907 - 0.343 - 0.05 }}
},
"extend_constants": {
{% set _ = extend_zero.update(
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index da8ed92..755b56a 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -8,16 +8,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-9971-1-0_cam-24-09_2024-02-24_16-10-16.872521280.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-9971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
},
],
"robot": {
diff --git a/y2024/constants/calib_files/calibration_imu-9971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json b/y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
similarity index 63%
rename from y2024/constants/calib_files/calibration_imu-9971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json
rename to y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
index 66ea94a..e5f50e3 100755
--- a/y2024/constants/calib_files/calibration_imu-9971-1-0_cam-24-10_2024-02-24_16-44-05.975708672.json
+++ b/y2024/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
@@ -12,6 +12,26 @@
0.0,
1.0
],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ -0.258819,
+ -0.965926,
+ -0.323293,
+ 1.0,
+ 0.0,
+ -0.0,
+ 0.268249,
+ 0.0,
+ -0.965926,
+ 0.258819,
+ 0.471129,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
"dist_coeffs": [
-0.251594,
0.064935,
diff --git a/y2024/constants/calib_files/calibration_imu-9971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json b/y2024/constants/calib_files/calibration_imu-9971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json
deleted file mode 100755
index 7f8b6ad..0000000
--- a/y2024/constants/calib_files/calibration_imu-9971-1-1_cam-24-12_2024-02-24_19-52-39.488095264.json
+++ /dev/null
@@ -1,26 +0,0 @@
-{
- "node_name": "imu",
- "team_number": 9971,
- "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
-}
diff --git a/y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json b/y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..530e88a
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 9971,
+ "intrinsics": [
+ 647.19928,
+ 0.0,
+ 690.698181,
+ 0.0,
+ 646.449158,
+ 530.162842,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.99969,
+ 0.020217,
+ -0.014527,
+ 0.160342,
+ 0.009501,
+ 0.229548,
+ 0.973251,
+ 0.25208,
+ 0.023011,
+ -0.973088,
+ 0.229284,
+ 0.41504,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.249799,
+ 0.062593,
+ 0.00003,
+ 0.000366,
+ -0.006532
+ ],
+ "calibration_timestamp": 1711306369592332476,
+ "camera_id": "24-12",
+ "camera_number": 1,
+ "reprojection_error": 1.23409
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-9971-1-0_cam-24-09_2024-02-24_16-10-16.872521280.json b/y2024/constants/calib_files/calibration_orin-9971-1-0_cam-24-09_2024-02-24_16-10-16.872521280.json
deleted file mode 100755
index 068e3d2..0000000
--- a/y2024/constants/calib_files/calibration_orin-9971-1-0_cam-24-09_2024-02-24_16-10-16.872521280.json
+++ /dev/null
@@ -1,26 +0,0 @@
-{
- "node_name": "orin1",
- "team_number": 9971,
- "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": 0,
- "reprojection_error": 1.881098
-}
diff --git a/y2024/constants/calib_files/calibration_orin-9971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json b/y2024/constants/calib_files/calibration_orin-9971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json
deleted file mode 100755
index c94dcaa..0000000
--- a/y2024/constants/calib_files/calibration_orin-9971-1-1_cam-24-11_2024-02-24_16-44-06.986729504.json
+++ /dev/null
@@ -1,26 +0,0 @@
-{
- "node_name": "orin1",
- "team_number": 9971,
- "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
-}
diff --git a/y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json b/y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..97234c9
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 648.187805,
+ 0.0,
+ 736.903137,
+ 0.0,
+ 648.028687,
+ 557.169861,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.999204,
+ -0.034711,
+ -0.019682,
+ 0.162103,
+ 0.028118,
+ -0.262536,
+ -0.964512,
+ -0.329348,
+ 0.028312,
+ -0.964298,
+ 0.263303,
+ 0.562319,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.265564,
+ 0.078084,
+ -0.000231,
+ 0.000386,
+ -0.010425
+ ],
+ "calibration_timestamp": 1711306369593360533,
+ "camera_id": "24-09",
+ "camera_number": 0,
+ "reprojection_error": 1.881098
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json b/y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..daef89c
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 649.866699,
+ 0.0,
+ 709.355713,
+ 0.0,
+ 648.893066,
+ 576.101868,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.014623,
+ 0.004459,
+ 0.999883,
+ 0.345385,
+ 0.997965,
+ 0.062137,
+ 0.014318,
+ 0.150131,
+ -0.062066,
+ 0.998058,
+ -0.005359,
+ 0.570236,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.248092,
+ 0.060938,
+ 0.000313,
+ 0.00009,
+ -0.006163
+ ],
+ "calibration_timestamp": 1711306369592886702,
+ "camera_id": "24-11",
+ "camera_number": 1,
+ "reprojection_error": 1.450069
+}
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 7002657..b280b2f 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -167,10 +167,12 @@
const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
- kCatapultActivationTurretThreshold);
+ kCatapultActivationTurretThreshold) &&
+ (std::abs(turret_.estimated_velocity()) < 0.2);
const bool altitude_in_range =
(std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationAltitudeThreshold);
+ kCatapultActivationAltitudeThreshold) &&
+ (std::abs(altitude_.estimated_velocity()) < 0.1);
const bool altitude_above_min_angle =
(altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 9e8b3a5..a4e47d7 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1272,7 +1272,7 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(dt());
+ RunFor(10 * dt());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());