Merge "Calibrate 2nd robot sensors"
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 454e147..42453ff 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()) {
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..5bf8c15 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])
+ 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": {