fixed starting auto mode on a real field
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e755f3d..e78e5f9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -160,6 +160,7 @@
// Decay the offset quickly because this gyro is great.
const double offset =
(right - left - gyro * constants::GetValues().turn_width) / 2.0;
+ // TODO(brians): filtered_offset_ = offset first time around.
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
control_loop_driving_ = control_loop_driving;
@@ -706,7 +707,7 @@
dt_closedloop.Update(output == NULL);
}
- // set the output status of the controll loop state
+ // set the output status of the control loop state
if (status) {
bool done = false;
if (goal) {
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 822d9f9..9fc11dc 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -215,6 +215,26 @@
::frc971::autonomous::autonomous.MakeWithBuilder()
.run_auto(false)
.Send();
+ } else if (!data.GetControlBit(ControlBit::kEnabled)) {
+ {
+ auto goal = drivetrain.goal.MakeMessage();
+ goal->Zero();
+ goal->control_loop_driving = false;
+ goal->left_goal = goal->right_goal = 0;
+ goal->left_velocity_goal = goal->right_velocity_goal = 0;
+ if (!goal.Send()) {
+ LOG(WARNING, "sending 0 drivetrain goal failed\n");
+ }
+ }
+ {
+ // TODO(brians): Make sure this doesn't make it unbrake and not move
+ // or something weird.
+ auto goal = control_loops::shooter_queue_group.goal.MakeMessage();
+ goal->Zero();
+ if (!goal.Send()) {
+ LOG(WARNING, "sending 0 shooter goal failed\n");
+ }
+ }
}
} else {
HandleTeleop(data);
@@ -274,6 +294,8 @@
.control_loop_driving(is_control_loop_driving)
.left_goal(left_goal)
.right_goal(right_goal)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}