Merge "Added more clear comments to superstructure"
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index cdbceb4..bf4d3b5 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -119,6 +119,10 @@
// LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up,
// leveling the shooter, and then moving back down.
+ if (arm_.error() || intake_.error()) {
+ state_ = ESTOP;
+ }
+
switch (state_) {
case UNINITIALIZED:
// Wait in the uninitialized state until both the arm and intake are
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 1350a15..a860f09 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -16,6 +16,8 @@
namespace superstructure {
namespace testing {
class SuperstructureTest_DisabledGoalTest_Test;
+class SuperstructureTest_ArmZeroingErrorTest_Test;
+class SuperstructureTest_IntakeZeroingErrorTest_Test;
} // namespace testing
class Superstructure
@@ -107,6 +109,8 @@
private:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
+ friend class testing::SuperstructureTest_ArmZeroingErrorTest_Test;
+ friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
Intake intake_;
Arm arm_;
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index a8953de..0798824 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -54,6 +54,11 @@
void Intake::Correct(PotAndIndexPosition position) {
estimator_.UpdateEstimate(position);
+ if (estimator_.error()) {
+ LOG(ERROR, "zeroing error with intake_estimator\n");
+ return;
+ }
+
if (!initialized_) {
if (estimator_.offset_ready()) {
UpdateIntakeOffset(estimator_.offset());
@@ -214,6 +219,16 @@
shoulder_estimator_.UpdateEstimate(position_shoulder);
wrist_estimator_.UpdateEstimate(position_wrist);
+ // Handle zeroing errors
+ if (shoulder_estimator_.error()) {
+ LOG(ERROR, "zeroing error with shoulder_estimator\n");
+ return;
+ }
+ if (wrist_estimator_.error()) {
+ LOG(ERROR, "zeroing error with wrist_estimator\n");
+ return;
+ }
+
if (!initialized_) {
if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
UpdateShoulderOffset(shoulder_estimator_.offset());
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index d20b345..ce50b4b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -24,6 +24,8 @@
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
bool zeroed() const { return zeroed_; }
+ // Returns whether an error has occured
+ bool error() const { return estimator_.error(); }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position);
@@ -71,6 +73,10 @@
const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimator_.TriggerError(); }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -96,6 +102,7 @@
bool zeroed_ = false;
};
+
class Arm {
public:
Arm();
@@ -104,6 +111,10 @@
bool zeroed() const { return shoulder_zeroed_ && wrist_zeroed_; }
bool shoulder_zeroed() const { return shoulder_zeroed_; }
bool wrist_zeroed() const { return wrist_zeroed_; }
+ // Returns whether an error has occured
+ bool error() const {
+ return shoulder_estimator_.error() || wrist_estimator_.error();
+ }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -159,6 +170,10 @@
const Eigen::Matrix<double, 6, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 6d8c18f..625ebf3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -130,8 +130,11 @@
}
double shoulder_angle() const { return arm_plant_->X(0, 0); }
+ double shoulder_angular_velocity() const { return arm_plant_->X(1, 0); }
double wrist_angle() const { return arm_plant_->X(2, 0); }
+ double wrist_angular_velocity() const { return arm_plant_->X(3, 0); }
double intake_angle() const { return intake_plant_->X(0, 0); }
+ double intake_angular_velocity() const { return intake_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
@@ -242,6 +245,42 @@
}
}
+ // Runs iterations while watching the average acceleration per cycle and
+ // making sure it doesn't exceed the provided bounds.
+ void RunForTimeLimitedAccel(const Time &run_for, double shoulder_accel,
+ double intake_accel, double wrist_accel) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ const auto loop_start_time = Time::Now();
+ double begin_shoulder_velocity =
+ superstructure_plant_.shoulder_angular_velocity();
+ double begin_intake_velocity =
+ superstructure_plant_.intake_angular_velocity();
+ double begin_wrist_velocity =
+ superstructure_plant_.wrist_angular_velocity();
+ RunIteration(true);
+ const double loop_time = (Time::Now() - loop_start_time).ToSeconds();
+ const double shoulder_acceleration =
+ (superstructure_plant_.shoulder_angular_velocity() -
+ begin_shoulder_velocity) /
+ loop_time;
+ const double intake_acceleration =
+ (superstructure_plant_.intake_angular_velocity() -
+ begin_intake_velocity) /
+ loop_time;
+ const double wrist_acceleration =
+ (superstructure_plant_.wrist_angular_velocity() -
+ begin_wrist_velocity) /
+ loop_time;
+ EXPECT_GE(shoulder_accel, shoulder_acceleration);
+ EXPECT_LE(-shoulder_accel, shoulder_acceleration);
+ EXPECT_GE(intake_accel, intake_acceleration);
+ EXPECT_LE(-intake_accel, intake_acceleration);
+ EXPECT_GE(wrist_accel, wrist_acceleration);
+ EXPECT_LE(-wrist_accel, wrist_acceleration);
+ }
+ }
+
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointed to
// shared memory that is no longer valid.
@@ -514,6 +553,143 @@
VerifyNearGoal();
}
+// Tests that the zeroing errors in the arm are caught
+TEST_F(SuperstructureTest, ArmZeroingErrorTest) {
+ RunIteration();
+ EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
+ superstructure_.arm_.TriggerEstimatorError();
+ RunIteration();
+
+ EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
+}
+
+// Tests that the zeroing errors in the intake are caught
+TEST_F(SuperstructureTest, IntakeZeroingErrorTest) {
+ RunIteration();
+ EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
+ superstructure_.intake_.TriggerEstimatorError();
+ RunIteration();
+
+ EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
+}
+
+// Tests that the loop respects shoulder acceleration limits while moving.
+TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(1.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(4));
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+
+ VerifyNearGoal();
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(1.5)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(1)
+ .max_angular_acceleration_intake(1)
+ .max_angular_velocity_shoulder(1)
+ .max_angular_acceleration_shoulder(1)
+ .max_angular_velocity_wrist(1)
+ .max_angular_acceleration_wrist(1)
+ .Send());
+
+ // TODO(austin): We should be able to hold the acceleration profile tighter
+ // than this, but we somehow can't. I'm not super worried though about 5%
+ // error...
+ RunForTimeLimitedAccel(Time::InSeconds(4), 1.05, 1.05, 1.05);
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop respects intake acceleration limits while moving.
+TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(1.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(4));
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+
+ VerifyNearGoal();
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.5)
+ .angle_shoulder(1.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(1)
+ .max_angular_acceleration_intake(1)
+ .max_angular_velocity_shoulder(1)
+ .max_angular_acceleration_shoulder(1)
+ .max_angular_velocity_wrist(1)
+ .max_angular_acceleration_wrist(1)
+ .Send());
+
+ // TODO(austin): We should be able to hold the acceleration profile tighter
+ // than this, but we somehow can't. I'm not super worried though about 5%
+ // error...
+ RunForTimeLimitedAccel(Time::InSeconds(4), 1.05, 1.05, 1.05);
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop respects wrist acceleration limits while moving.
+TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(1.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(4));
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+
+ VerifyNearGoal();
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(1.0)
+ .angle_wrist(0.5)
+ .max_angular_velocity_intake(1)
+ .max_angular_acceleration_intake(1)
+ .max_angular_velocity_shoulder(1)
+ .max_angular_acceleration_shoulder(1)
+ .max_angular_velocity_wrist(1)
+ .max_angular_acceleration_wrist(1)
+ .Send());
+
+ // TODO(austin): We should be able to hold the acceleration profile tighter
+ // than this, but we somehow can't. I'm not super worried though about 5%
+ // error...
+ RunForTimeLimitedAccel(Time::InSeconds(4), 1.05, 1.05, 1.05);
+
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops