Tests that zeroing while disabled works.
- Fixes a bug where the profile didn't move when in RUNNING or SLOW_RUNNING
and disabled.
- Tested that we can zero when disabled.
Change-Id: I9bf67dd4019cc6cf487ef24ac97e213ec2fb70a7
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index c4ef468..cdbceb4 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -304,17 +304,30 @@
case SLOW_RUNNING:
case RUNNING:
- // TODO(austin): Exit SLOW_RUNNING if we are not collided.
+ if (disable) {
+ // TODO(austin): Enter SLOW_RUNNING if we are collided.
+
+ // If we are disabled, reset the profile to the current position.
+ intake_.ForceGoal(intake_.angle());
+ arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ } else {
+ if (state_ == SLOW_RUNNING) {
+ // TODO(austin): Exit SLOW_RUNNING if we are not collided.
+ LOG(ERROR, "Need to transition on non-collided, not all the time.\n");
+ state_ = RUNNING;
+ }
+ }
+
if (unsafe_goal) {
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
+ unsafe_goal->max_angular_acceleration_shoulder,
+ unsafe_goal->max_angular_velocity_wrist,
+ unsafe_goal->max_angular_acceleration_wrist);
intake_.AdjustProfile(unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_intake);
+ unsafe_goal->max_angular_acceleration_intake);
arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist);
+ unsafe_goal->angle_wrist);
intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
}