Sped box grab up significantly and handle disable
Add some more trajectories and cut tolerances to make box grabbing go
faster. Also, go to the nearest node when disabled.
Change-Id: Ic641db69a8c18eb61771ebc352e05ceef0d90084
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 239f15a..2baa14a 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -139,6 +139,12 @@
new_superstructure_goal->intake.roller_voltage = 0.0;
}
+ // If we are disabled, stay at the node closest to where we start. This
+ // should remove long motions when enabled.
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ arm_goal_position_ = superstructure_queue.status->arm.current_node;
+ }
+
bool grab_box = false;
if (data.IsPressed(kArmPickupBoxFromIntake)) {
arm_goal_position_ = arm::NeutralIndex();
@@ -173,7 +179,7 @@
new_superstructure_goal->arm_goal_position = arm_goal_position_;
- if (data.IsPressed(kClawOpen)) {
+ if (data.IsPressed(kClawOpen) || data.PosEdge(kArmPickupBoxFromIntake)) {
new_superstructure_goal->open_claw = true;
} else {
new_superstructure_goal->open_claw = false;