Merge changes I2fbcd394,I861317ea,Icba24385
* changes:
Updated intrinsic and extrinsic data for 971-3
Update for pi-7971-3 intrinsic calibration
Update to 7971-2 with new intrinsic parameters
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index a1f19cd..c200b55 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -48,8 +48,8 @@
if (!ddend0.isApprox(ddstart1, 1e-6)) {
AOS_LOG(
ERROR,
- "Splines %d and %d don't line up in the second derivative. [%f, "
- "%f] != [%f, %f]\n",
+ "Splines %d and %d don't line up in the second derivative. [%.7f, "
+ "%.7f] != [%.7f, %.7f]\n",
static_cast<int>(i - 1), static_cast<int>(i), ddend0(0, 0),
ddend0(1, 0), ddstart1(0, 0), ddstart1(1, 0));
}
diff --git a/frc971/input/redundant_joystick_data.cc b/frc971/input/redundant_joystick_data.cc
index 3274856..a25e6c3 100644
--- a/frc971/input/redundant_joystick_data.cc
+++ b/frc971/input/redundant_joystick_data.cc
@@ -9,7 +9,7 @@
RedundantData::RedundantData(const Data &data) : joystick_map_(), data_(data) {
// Start with a naive map.
for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
- joystick_map_.at(i) = i;
+ joystick_map_.at(i) = -1;
}
for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
@@ -32,6 +32,9 @@
};
int RedundantData::MapRedundantJoystick(int joystick) const {
+ if (joystick < 0 || joystick >= static_cast<int>(joystick_map_.size())) {
+ return -1;
+ }
return joystick_map_.at(joystick);
}
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 317ff0c..58c31d0 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -206,6 +206,8 @@
SplineAuto();
} else if (FLAGS_charged_up) {
ChargedUp();
+ } else if (FLAGS_charged_up_cable) {
+ ChargedUpCableSide();
} else {
AOS_LOG(WARNING, "No auto mode selected.");
}
@@ -538,6 +540,9 @@
AOS_LOG(
INFO, "Placed second cube %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(200));
+ Neutral();
}
void AutonomousActor::SendSuperstructureGoal() {