Shift into the default when disabled.
Change-Id: Ia33923a9bece64df620c46c74a115c6cd165d191
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 2624483..8a0a7ff 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -36,8 +36,8 @@
wheel_(0.0),
throttle_(0.0),
quickturn_(false),
- left_gear_(Gear::LOW),
- right_gear_(Gear::LOW),
+ left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
+ right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
counter_(0),
dt_config_(dt_config) {
last_position_.Zero();
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index d987268..0bb19dc 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -65,7 +65,7 @@
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
- : is_high_gear_(false),
+ : is_high_gear_(true),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0) {}
@@ -82,7 +82,14 @@
}
}
- if (!data.GetControlBit(ControlBit::kAutonomous)) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ // If we are not enabled, reset the waiting for zero bit.
+ LOG(DEBUG, "Waiting for zero.\n");
+ waiting_for_zero_ = true;
+ is_high_gear_ = true;
+ }
+
+ if (!auto_running_) {
HandleDrivetrain(data);
HandleTeleop(data);
}
@@ -139,13 +146,6 @@
LOG(DEBUG, "Canceling\n");
}
- if (data.PosEdge(ControlBit::kEnabled)) {
- // If we got enabled, wait for everything to zero.
- LOG(INFO, "Waiting for zero.\n");
- waiting_for_zero_ = true;
- is_high_gear_ = true;
- }
-
superstructure_queue.status.FetchLatest();
if (!superstructure_queue.status.get()) {
LOG(ERROR, "Got no superstructure status packet.\n");
@@ -154,7 +154,7 @@
if (superstructure_queue.status.get() &&
superstructure_queue.status->zeroed) {
if (waiting_for_zero_) {
- LOG(INFO, "Zeroed! Starting teleop mode.\n");
+ LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
waiting_for_zero_ = false;
}
} else {