The wrist goal can no longer run away.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index cf3d7e4..c49ca44 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -89,6 +89,7 @@
Eigen::Matrix<double, number_of_states, 1> X_hat;
Eigen::Matrix<double, number_of_states, 1> R;
Eigen::Matrix<double, number_of_inputs, 1> U;
+ Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
Eigen::Matrix<double, number_of_outputs, 1> Y;
@@ -106,6 +107,7 @@
X_hat.setZero();
R.setZero();
U.setZero();
+ U_uncapped.setZero();
U_ff.setZero();
Y.setZero();
}
@@ -137,7 +139,7 @@
U[i] = 0.0;
}
} else {
- U.noalias() = K * (R - X_hat);
+ U = U_uncapped = K * (R - X_hat);
CapU();
}
diff --git a/frc971/control_loops/wrist.cc b/frc971/control_loops/wrist.cc
index dee0438..de62580 100644
--- a/frc971/control_loops/wrist.cc
+++ b/frc971/control_loops/wrist.cc
@@ -18,7 +18,7 @@
WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
: aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- loop_(new StateFeedbackLoop<2, 1, 1>(MakeWristLoop())),
+ loop_(new WristStateFeedbackLoop(MakeWristLoop(), this)),
state_(UNINITIALIZED),
error_count_(0),
zero_offset_(0.0) {
@@ -52,23 +52,22 @@
std::max(horizontal_lower_limit_, goal));
}
-double WristMotor::LimitVoltage(double absolute_position,
- double voltage) const {
- if (state_ == READY) {
- if (absolute_position >= horizontal_upper_limit_) {
- voltage = std::min(0.0, voltage);
+const double kMaxZeroingVoltage = 5.0;
+
+void WristMotor::WristStateFeedbackLoop::CapU() {
+ if (wrist_motor_->state_ == READY) {
+ if (Y(0, 0) >= wrist_motor_->horizontal_upper_limit_) {
+ U(0, 0) = std::min(0.0, U(0, 0));
}
- if (absolute_position <= horizontal_lower_limit_) {
- voltage = std::max(0.0, voltage);
+ if (Y(0, 0) <= wrist_motor_->horizontal_lower_limit_) {
+ U(0, 0) = std::max(0.0, U(0, 0));
}
}
- double limit = (state_ == READY) ? 12.0 : 5.0;
- // TODO(aschuh): Remove this line when we are done testing.
- //limit = std::min(0.3, limit);
- voltage = std::min(limit, voltage);
- voltage = std::max(-limit, voltage);
- return voltage;
+ double limit = (wrist_motor_->state_ == READY) ? 12.0 : kMaxZeroingVoltage;
+
+ U(0, 0) = std::min(limit, U(0, 0));
+ U(0, 0) = std::max(-limit, U(0, 0));
}
// Positive angle is up, and positive power is up.
@@ -197,6 +196,28 @@
// Update the observer.
loop_->Update(position != NULL, output == NULL);
+ // Verify that the zeroing goal hasn't run away.
+ switch (state_) {
+ case UNINITIALIZED:
+ case READY:
+ case ESTOP:
+ // Not zeroing. No worries.
+ break;
+ case MOVING_OFF:
+ case ZEROING:
+ // Check if we have cliped and adjust the goal.
+ if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
+ double dx = (loop_->U_uncapped(0, 0) -
+ kMaxZeroingVoltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ } else if(loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
+ double dx = (loop_->U_uncapped(0, 0) +
+ kMaxZeroingVoltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ }
+ break;
+ }
+
if (position) {
LOG(DEBUG, "pos=%f zero=%f currently %f hall: %s\n",
position->pos, zero_offset_, absolute_position,
@@ -204,7 +225,7 @@
}
if (output) {
- output->voltage = LimitVoltage(absolute_position, loop_->U(0, 0));
+ output->voltage = loop_->U(0, 0);
}
}
diff --git a/frc971/control_loops/wrist.h b/frc971/control_loops/wrist.h
index fe09105..81c3bcb 100644
--- a/frc971/control_loops/wrist.h
+++ b/frc971/control_loops/wrist.h
@@ -14,8 +14,13 @@
namespace testing {
class WristTest_RezeroWithMissingPos_Test;
class WristTest_DisableGoesUninitialized_Test;
+class WristTest_NoWindup_Test;
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
};
+class WristMotor;
+
class WristMotor
: public aos::control_loops::ControlLoop<control_loops::WristLoop> {
public:
@@ -33,6 +38,9 @@
// Friend the test classes for acces to the internal state.
friend class testing::WristTest_RezeroWithMissingPos_Test;
friend class testing::WristTest_DisableGoesUninitialized_Test;
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+ friend class WristStateFeedbackLoop;
// Fetches and locally caches the latest set of constants.
bool FetchConstants();
@@ -40,12 +48,25 @@
// Clips the goal to be inside the limits and returns the clipped goal.
// Requires the constants to have already been fetched.
double ClipGoal(double goal) const;
- // Limits the voltage depending whether the wrist has been zeroed or is out of
- // range to make it safer to use.
- double LimitVoltage(double absolute_position, double voltage) const;
+
+ // This class implements the CapU function correctly given all the extra
+ // information that we know about from the wrist motor.
+ class WristStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ WristStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
+ WristMotor *wrist_motor)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ wrist_motor_(wrist_motor) {
+ }
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+ private:
+ WristMotor *wrist_motor_;
+ };
// The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+ ::std::unique_ptr<WristStateFeedbackLoop> loop_;
// Enum to store the state of the internal zeroing state machine.
enum State {
diff --git a/frc971/control_loops/wrist_lib_test.cc b/frc971/control_loops/wrist_lib_test.cc
index 9f3afb8..36474ae 100644
--- a/frc971/control_loops/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist_lib_test.cc
@@ -244,6 +244,9 @@
// Give the loop a couple cycled to get the message and then verify that
// it is in the correct state.
EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ // When disabled, we should be applying 0 power.
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ EXPECT_EQ(0, my_wrist_loop_.output->voltage);
}
} else {
SendDSPacket(true);
@@ -259,6 +262,75 @@
VerifyNearGoal();
}
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupNegative) {
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroing_position_;
+ wrist_motor_.zeroing_position_ = -100.0;
+ } else if (i == 51) {
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ }
+ if (wrist_motor_.state_ != WristMotor::READY) {
+ if (i == 51) {
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ } else {
+ EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ }
+ }
+
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupPositive) {
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroing_position_;
+ wrist_motor_.zeroing_position_ = 100.0;
+ } else {
+ if (i == 51) {
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ }
+ }
+ if (wrist_motor_.state_ != WristMotor::READY) {
+ if (i == 51) {
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ } else {
+ EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971