Added landing voltage limit and test.
Change-Id: I0a74fba887c6f791c05ab6fd69c543678fcdb420
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 9b87ff9..43a95d9 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -530,6 +530,17 @@
arm_.set_max_voltage(max_voltage, max_voltage);
intake_.set_max_voltage(max_voltage);
+ if (IsRunning()) {
+ // We don't want lots of negative voltage when we are near the bellypan on
+ // the shoulder...
+ // TODO(austin): Do I want to push negative power into the belly pan at this
+ // point? Maybe just put the goal slightly below the bellypan and call that
+ // good enough.
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ arm_.set_shoulder_asymetric_limits(-2.0, max_voltage);
+ }
+ }
+
// Calculate the loops for a cycle.
arm_.Update(disable);
intake_.Update(disable);
@@ -551,8 +562,7 @@
// Save debug/internal state.
// TODO(austin): Save the voltage errors.
- status->zeroed = (state_ == RUNNING || state_ == LANDING_RUNNING ||
- state_ == SLOW_RUNNING || state_ == LANDING_SLOW_RUNNING);
+ status->zeroed = IsRunning();
status->shoulder.angle = arm_.X_hat(0, 0);
status->shoulder.angular_velocity = arm_.X_hat(1, 0);
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 23491bf..823e130 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -170,6 +170,11 @@
ESTOP = 16,
};
+ bool IsRunning() const {
+ return (state_ == SLOW_RUNNING || state_ == RUNNING ||
+ state_ == LANDING_SLOW_RUNNING || state_ == LANDING_RUNNING);
+ }
+
State state() const { return state_; }
// Returns the value to move the joint to such that it will stay below
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index ce50b4b..3bc41cb 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -137,6 +137,11 @@
double max_angular_acceleration_wrist);
void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
+ void set_shoulder_asymetric_limits(double shoulder_min_voltage,
+ double shoulder_max_voltage) {
+ loop_->set_asymetric_voltage(0, shoulder_min_voltage, shoulder_max_voltage);
+ }
+
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
// Resets the internal state.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index eff5017..1ff958d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -974,7 +974,32 @@
ASSERT_FALSE(collided());
}
+// Make sure that the landing voltage limit works.
+TEST_F(SuperstructureTest, LandingDownVoltageLimit) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.lower);
+ superstructure_plant_.InitializeShoulderPosition(0.0);
+ superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(0.0) // intentionally asking for forward
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+ VerifyNearGoal();
+
+ // If we are near the bottom of the range, we won't have enough power to
+ // compensate for the offset. This means that we fail if we get to the goal.
+ superstructure_plant_.set_power_error(0.0, 3.0, 0.0);
+ RunForTime(Time::InSeconds(2));
+ superstructure_plant_.set_power_error(0.0, 6.0, 0.0);
+ RunForTime(Time::InSeconds(2));
+ EXPECT_LE(0.0, superstructure_queue_.goal->angle_shoulder);
+}
// TODO(austin): Landed to unlanded needs to go fast!
+// TODO(austin): Test that landing is slow below the magic point.
} // namespace testing
} // namespace superstructure