Add claw control loop.
It's based pretty heavily on the fridge loop. Currently, all the
tests pass.
Change-Id: Ieb386dfa5c3fe2d34e2d191fa39a44dc77ee6ab6
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index f9abd08..057c323 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -1,24 +1,275 @@
#include "frc971/control_loops/claw/claw.h"
+#include <algorithm>
+
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
namespace frc971 {
namespace control_loops {
+using ::aos::time::Time;
+
+constexpr double kZeroingVoltage = 5.0;
+
+void ClawCappedStateFeedbackLoop::CapU() {
+ mutable_U(0, 0) = ::std::min(mutable_U(0, 0), max_voltage_);
+ mutable_U(0, 0) = ::std::max(mutable_U(0, 0), -max_voltage_);
+}
+
+double ClawCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+ // Compute K matrix to compensate for position errors.
+ double Kp = K(0, 0);
+
+ // Compute how much we need to change R in order to achieve the change in U
+ // that was observed.
+ return -(1.0 / Kp) * (U_uncapped() - U())(0, 0);
+}
+
Claw::Claw(control_loops::ClawQueue *claw)
: aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
- claw_loop_(new StateFeedbackLoop<2, 1, 1>(MakeClawLoop())) {}
+ last_piston_edge_(Time::Now()),
+ claw_loop_(new ClawCappedStateFeedbackLoop(MakeClawLoop())),
+ claw_estimator_(constants::GetValues().claw.zeroing) {}
+
+void Claw::UpdateZeroingState() {
+ if (claw_estimator_.offset_ratio_ready() < 1.0) {
+ state_ = INITIALIZING;
+ } else if (!claw_estimator_.zeroed()) {
+ state_ = ZEROING;
+ } else {
+ state_ = RUNNING;
+ }
+}
+
+void Claw::Correct() {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << claw_position();
+ claw_loop_->Correct(Y);
+}
+
+void Claw::SetClawOffset(double offset) {
+ LOG(INFO, "Changing claw offset from %f to %f.\n",
+ claw_offset_, offset);
+ const double doffset = offset - claw_offset_;
+
+ // Adjust the height. The derivative should not need to be updated since the
+ // speed is not changing.
+ claw_loop_->mutable_X_hat(0, 0) += doffset;
+
+ // Modify claw zeroing goal.
+ claw_goal_ += doffset;
+ // Update the cached offset value to the actual value.
+ claw_offset_ = offset;
+}
+
+double Claw::estimated_claw_position() const {
+ return current_position_.joint.encoder + claw_estimator_.offset();
+}
+
+double Claw::claw_position() const {
+ return current_position_.joint.encoder + claw_offset_;
+}
+
+constexpr double kClawZeroingVelocity = 0.2;
+
+double Claw::claw_zeroing_velocity() {
+ const auto &values = constants::GetValues();
+
+ // Zeroing will work as following. At startup, record the offset of the claw.
+ // Then, start moving the claw towards where the index pulse should be. We
+ // search around it a little, and if we don't find anything, we estop.
+ // Otherwise, we're done.
+
+ const double target_pos = values.claw.zeroing.measured_index_position;
+ // How far away we need to stay from the ends of the range while zeroing.
+ constexpr double zeroing_limit = 0.1375;
+ // Keep the zeroing range within the bounds of the mechanism.
+ const double zeroing_range =
+ ::std::min(target_pos - values.claw.wrist.lower_limit - zeroing_limit,
+ values.claw.zeroing_range);
+
+ if (claw_zeroing_velocity_ == 0) {
+ if (estimated_claw_position() > target_pos) {
+ claw_zeroing_velocity_ = -kClawZeroingVelocity;
+ } else {
+ claw_zeroing_velocity_ = kClawZeroingVelocity;
+ }
+ } else if (claw_zeroing_velocity_ > 0 &&
+ estimated_claw_position() > target_pos + zeroing_range) {
+ claw_zeroing_velocity_ = -kClawZeroingVelocity;
+ } else if (claw_zeroing_velocity_ < 0 &&
+ estimated_claw_position() < target_pos - zeroing_range) {
+ claw_zeroing_velocity_ = kClawZeroingVelocity;
+ }
+
+ return claw_zeroing_velocity_;
+}
void Claw::RunIteration(
- const control_loops::ClawQueue::Goal * /*goal*/,
- const control_loops::ClawQueue::Position * /*position*/,
- control_loops::ClawQueue::Output * /*output*/,
- control_loops::ClawQueue::Status * /*status*/) {
+ const control_loops::ClawQueue::Goal *unsafe_goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ control_loops::ClawQueue::Status *status) {
+ const auto &values = constants::GetValues();
- LOG(DEBUG, "Hi Brian!\n");
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset! Restarting.\n");
+ claw_estimator_.Reset();
+ state_ = UNINITIALIZED;
+ }
+
+ current_position_ = *position;
+
+ // Bool to track if we should turn the motor on or not.
+ bool disable = output == nullptr;
+ double claw_goal_velocity = 0.0;
+
+ claw_estimator_.UpdateEstimate(position->joint);
+
+ if (state_ != UNINITIALIZED) {
+ Correct();
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(INFO, "Uninitialized.\n");
+ // Startup. Assume that we are at the origin.
+ claw_offset_ = -position->joint.encoder;
+ claw_loop_->mutable_X_hat().setZero();
+ Correct();
+ state_ = INITIALIZING;
+ disable = true;
+ break;
+
+ case INITIALIZING:
+ LOG(INFO, "Waiting for accurate initial position.\n");
+ disable = true;
+ // Update state_ to accurately represent the state of the zeroing
+ // estimator.
+ UpdateZeroingState();
+
+ if (state_ != INITIALIZING) {
+ // Set the goals to where we are now.
+ claw_goal_ = claw_position();
+ }
+ break;
+
+ case ZEROING:
+ LOG(INFO, "Zeroing.\n");
+
+ // Update state_.
+ UpdateZeroingState();
+ if (claw_estimator_.zeroed()) {
+ LOG(INFO, "Zeroed!\n");
+ SetClawOffset(claw_estimator_.offset());
+ } else if (!disable) {
+ claw_goal_velocity = claw_zeroing_velocity();
+ claw_goal_ += claw_goal_velocity *
+ ::aos::controls::kLoopFrequency.ToSeconds();
+ }
+ break;
+
+ case RUNNING:
+ LOG(DEBUG, "Running!\n");
+
+ // Update state_.
+ UpdateZeroingState();
+ if (unsafe_goal) {
+ claw_goal_ = unsafe_goal->angle;
+ claw_goal_velocity = unsafe_goal->angular_velocity;
+ }
+
+ if (state_ != RUNNING && state_ != ESTOP) {
+ state_ = UNINITIALIZED;
+ }
+ break;
+
+ case ESTOP:
+ LOG(ERROR, "Estop!\n");
+ disable = true;
+ break;
+ }
+
+ // Make sure goal and position do not exceed the hardware limits if we are
+ // RUNNING.
+ if (state_ == RUNNING) {
+ // Limit goal.
+ claw_goal_ = ::std::min(claw_goal_, values.claw.wrist.upper_limit);
+ claw_goal_ = ::std::max(claw_goal_, values.claw.wrist.lower_limit);
+
+ // Check position.
+ if (claw_position() >= values.claw.wrist.upper_hard_limit ||
+ claw_position() <= values.claw.wrist.lower_hard_limit) {
+ LOG(ERROR, "Claw at %f out of bounds [%f, %f].\n", claw_position(),
+ values.claw.wrist.lower_limit, values.claw.wrist.upper_limit);
+ state_ = ESTOP;
+ }
+ }
+
+ // Set the goals.
+ claw_loop_->mutable_R() << claw_goal_, claw_goal_velocity;
+
+ const double max_voltage = (state_ == RUNNING) ? 12.0 : kZeroingVoltage;
+ claw_loop_->set_max_voltage(max_voltage);
+
+ if (state_ == ESTOP) {
+ disable = true;
+ }
+ claw_loop_->Update(disable);
+
+ if (state_ == INITIALIZING || state_ == ZEROING) {
+ if (claw_loop_->U() != claw_loop_->U_uncapped()) {
+ double deltaR = claw_loop_->UnsaturateOutputGoalChange();
+
+ // Move the claw goal by the amount observed.
+ LOG(WARNING, "Moving claw goal by %f to handle saturation.\n",
+ deltaR);
+ claw_goal_ += deltaR;
+ }
+ }
+
+ if (output) {
+ output->voltage = claw_loop_->U(0, 0);
+ if (unsafe_goal) {
+ output->intake_voltage = unsafe_goal->intake;
+ output->rollers_closed = unsafe_goal->rollers_closed;
+ } else {
+ output->intake_voltage = 0.0;
+ output->rollers_closed = false;
+ }
+ if (output->rollers_closed != last_rollers_closed_) {
+ last_piston_edge_ = Time::Now();
+ }
+ }
+
+ status->zeroed = state_ == RUNNING;
+ status->estopped = state_ == ESTOP;
+ status->state = state_;
+
+ status->angle = claw_loop_->X_hat(0, 0);
+ if (output) {
+ status->intake = output->intake_voltage;
+ } else {
+ status->intake = 0;
+ }
+
+ if (output) {
+ status->rollers_open = !output->rollers_closed &&
+ (Time::Now() - last_piston_edge_ >= values.claw.piston_switch_time);
+ status->rollers_closed = output->rollers_closed &&
+ (Time::Now() - last_piston_edge_ >= values.claw.piston_switch_time);
+ } else {
+ status->rollers_open = false;
+ status->rollers_closed = false;
+ }
+
+ if (output) {
+ last_rollers_closed_ = output->rollers_closed;
+ }
}
} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 31a6933..16001c3 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -28,13 +28,17 @@
'dependencies': [
'claw_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/common.gyp:time',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
],
'export_dependent_settings': [
'claw_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/common.gyp:time',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
],
},
{
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 3d7e647..6abc082 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -4,28 +4,60 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
+#include "frc971/zeroing/zeroing.h"
namespace frc971 {
namespace control_loops {
+namespace testing {
+class ClawTest_DisabledGoal_Test;
+class ClawTest_GoalPositiveWindup_Test;
+class ClawTest_GoalNegativeWindup_Test;
+} // namespace testing
+
+class ClawCappedStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ ClawCappedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> &&loop)
+ : StateFeedbackLoop<2, 1, 1>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+ }
+
+ void CapU() override;
+
+ // Returns the amount to change the position goal in order to no longer
+ // saturate the controller.
+ double UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
class Claw
: public aos::controls::ControlLoop<control_loops::ClawQueue> {
public:
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Estimating the starting location.
+ INITIALIZING = 1,
+ // Moving to find an index pulse.
+ ZEROING = 2,
+ // Normal operation.
+ RUNNING = 3,
+ // Internal error caused the claw to abort.
+ ESTOP = 4,
+ };
+
+ int state() { return state_; }
+
explicit Claw(
control_loops::ClawQueue *claw_queue = &control_loops::claw_queue);
- // Control loop time step.
- // Please figure out how to set dt from a common location
- // Please decide the correct value
- // Please use dt in your implementation so we can change looptimnig
- // and be consistent with legacy
- // And Brian please approve my code review as people are wait on
- // these files to exist and they will be rewritten anyway
- //static constexpr double dt;
-
protected:
virtual void RunIteration(
const control_loops::ClawQueue::Goal *goal,
@@ -34,8 +66,47 @@
control_loops::ClawQueue::Status *status);
private:
+ friend class testing::ClawTest_DisabledGoal_Test;
+ friend class testing::ClawTest_GoalPositiveWindup_Test;
+ friend class testing::ClawTest_GoalNegativeWindup_Test;
+
+ // Sets state_ to the correct state given the current state of the zeroing
+ // estimator.
+ void UpdateZeroingState();
+ void SetClawOffset(double offset);
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ // Getter for the current claw position.
+ double claw_position() const;
+ // Our best guess at the current position of the claw.
+ double estimated_claw_position() const;
+ // Returns the current zeroing velocity for the claw. If the subsystem is too
+ // far away from the center, it will switch directions.
+ double claw_zeroing_velocity();
+
+ State state_ = UNINITIALIZED;
+
+ // The time when we last changed the claw piston state.
+ ::aos::time::Time last_piston_edge_;
+
// The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> claw_loop_;
+ ::std::unique_ptr<ClawCappedStateFeedbackLoop> claw_loop_;
+
+ // Latest position from queue.
+ control_loops::ClawQueue::Position current_position_;
+ // Zeroing estimator for claw.
+ zeroing::ZeroingEstimator claw_estimator_;
+
+ // The goal for the claw.
+ double claw_goal_ = 0.0;
+ // Current velocity to move at while zeroing.
+ double claw_zeroing_velocity_ = 0.0;
+ // Offsets from the encoder position to the absolute position.
+ double claw_offset_ = 0.0;
+
+ // Whether claw was closed last cycle.
+ bool last_rollers_closed_ = false;
};
} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 1ca5746..00c38c4 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -13,6 +13,8 @@
message Goal {
// Angle of wrist joint.
double angle;
+ // Angular velocity of wrist.
+ double angular_velocity;
// Voltage of intake rollers. Positive means sucking in, negative means
// spitting out.
double intake;
@@ -39,8 +41,10 @@
message Status {
// Is claw zeroed?
bool zeroed;
- // Has claw zeroed and reached goal?
- bool done;
+ // Did the claw estop?
+ bool estopped;
+ // The internal state of the claw state machine.
+ uint32_t state;
// Angle of wrist joint.
double angle;
@@ -53,7 +57,7 @@
bool rollers_open;
// True iff there has been enough time since we actuated the rollers closed
// that they should be there.
- bool rollers_close;
+ bool rollers_closed;
};
queue Goal goal;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 9c0553c..f7b987c 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -32,14 +32,31 @@
".frc971.control_loops.claw_queue.position",
".frc971.control_loops.claw_queue.output",
".frc971.control_loops.claw_queue.status") {
- pot_and_encoder_.Initialize(
- constants::GetValues().claw.wrist.lower_limit,
- constants::GetValues().claw.zeroing.index_difference / 3.0);
+ InitializePosition(constants::GetValues().claw.wrist.lower_limit);
+ }
+
+ void InitializePosition(double start_pos) {
+ InitializePosition(start_pos,
+ constants::GetValues().claw.zeroing.measured_index_position);
+ }
+
+ void InitializePosition(double start_pos, double index_pos) {
+ InitializePosition(start_pos,
+ // This gives us a standard deviation of ~9 degrees on the pot noise.
+ constants::GetValues().claw.zeroing.index_difference / 10.0,
+ index_pos);
}
// Do specific initialization for the sensors.
- void SetSensors(double start_pos, double pot_noise_stddev) {
- pot_and_encoder_.Initialize(start_pos, pot_noise_stddev);
+ void InitializePosition(double start_pos,
+ double pot_noise_stddev,
+ double index_pos) {
+ // Update internal state.
+ claw_plant_->mutable_X(0, 0) = start_pos;
+ // Zero velocity.
+ claw_plant_->mutable_X(1, 0) = 0.0;
+
+ pot_and_encoder_.Initialize(start_pos, pot_noise_stddev, index_pos);
}
// Sends a queue message with the position.
@@ -91,11 +108,14 @@
EXPECT_NEAR(claw_queue_.goal->angle,
claw_queue_.status->angle,
10.0);
+
+ EXPECT_TRUE(claw_queue_.status->zeroed);
+ EXPECT_FALSE(claw_queue_.status->estopped);
}
// Runs one iteration of the whole simulation.
- void RunIteration() {
- SendMessages(true);
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
claw_plant_.SendPositionMessage();
claw_.Iterate();
@@ -105,10 +125,10 @@
}
// Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const Time &run_for) {
+ void RunForTime(const Time &run_for, bool enabled = true) {
const auto start_time = Time::Now();
while (Time::Now() < start_time + run_for) {
- RunIteration();
+ RunIteration(enabled);
}
}
@@ -135,6 +155,43 @@
VerifyNearGoal();
}
+// NOTE: Claw zeroing is a little annoying because we only hit one index pulse
+// in our entire range of motion.
+
+// Tests that the loop zeroing works with normal values.
+TEST_F(ClawTest, Zeroes) {
+ // It should zero itself if we run it for awhile.
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ ASSERT_TRUE(claw_queue_.status.FetchLatest());
+ EXPECT_TRUE(claw_queue_.status->zeroed);
+ EXPECT_EQ(Claw::RUNNING, claw_queue_.status->state);
+}
+
+// Tests that claw zeroing fails if the index pulse occurs too close to the end
+// of the range.
+TEST_F(ClawTest, BadIndexPosition) {
+ const auto values = constants::GetValues();
+ claw_plant_.InitializePosition(values.claw.wrist.lower_limit,
+ values.claw.wrist.upper_limit);
+
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ // The zeroing is going to take its sweet time on this one, so we had better
+ // run it for longer.
+ RunForTime(Time::InMS(12000));
+
+ ASSERT_TRUE(claw_queue_.status.FetchLatest());
+ EXPECT_FALSE(claw_queue_.status->zeroed);
+ EXPECT_FALSE(claw_queue_.status->estopped);
+}
+
// Tests that we can reach a reasonable goal.
TEST_F(ClawTest, ReachesGoal) {
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
@@ -157,9 +214,9 @@
RunForTime(Time::InMS(4000));
claw_queue_.status.FetchLatest();
- /*EXPECT_NEAR(values.claw.wrist.upper_limit,
+ EXPECT_NEAR(values.claw.wrist.upper_limit,
claw_queue_.status->angle,
- 2.0 * M_PI / 180.0);*/
+ 2.0 * M_PI / 180.0);
// Lower limit.
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
@@ -169,20 +226,120 @@
RunForTime(Time::InMS(4000));
claw_queue_.status.FetchLatest();
- /*EXPECT_NEAR(values.claw.wrist.lower_limit,
+ EXPECT_NEAR(values.claw.wrist.lower_limit,
claw_queue_.status->angle,
- 2.0 * M_PI / 180.0);*/
+ 2.0 * M_PI / 180.0);
}
+// Tests that starting at the lower hardstop doesn't cause an abort.
+TEST_F(ClawTest, LowerHardstopStartup) {
+ claw_plant_.InitializePosition(
+ constants::GetValues().claw.wrist.lower_hard_limit);
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+ RunForTime(Time::InMS(4000));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstop doesn't cause an abort.
+TEST_F(ClawTest, UpperHardstopStartup) {
+ claw_plant_.InitializePosition(
+ constants::GetValues().claw.wrist.upper_hard_limit);
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+ // Zeroing will take a long time here.
+ RunForTime(Time::InMS(12000));
+
+ VerifyNearGoal();
+}
+
+
// Tests that not having a goal doesn't break anything.
TEST_F(ClawTest, NoGoal) {
- for (int i = 0; i < 10; ++i) {
- claw_plant_.SendPositionMessage();
- claw_.Iterate();
- claw_plant_.Simulate();
+ RunForTime(Time::InMS(50));
+}
- TickTime();
+// Tests that a WPILib reset results in rezeroing.
+TEST_F(ClawTest, WpilibReset) {
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+ VerifyNearGoal();
+
+ SimulateSensorReset();
+ RunForTime(Time::InMS(100));
+ ASSERT_TRUE(claw_queue_.status.FetchLatest());
+ EXPECT_NE(Claw::RUNNING, claw_queue_.status->state);
+
+ // Once again, it's going to take us awhile to rezero since we moved away from
+ // our index pulse.
+ RunForTime(Time::InMS(10000));
+ VerifyNearGoal();
+}
+
+// Tests that internal goals don't change while disabled.
+TEST_F(ClawTest, DisabledGoal) {
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ RunForTime(Time::InMS(100), false);
+ EXPECT_EQ(0.0, claw_.claw_goal_);
+
+ // Now make sure they move correctly.
+ RunForTime(Time::InMS(1000), true);
+ EXPECT_NE(0.0, claw_.claw_goal_);
+}
+
+// Tests that the claw zeroing goals don't wind up too far.
+TEST_F(ClawTest, GoalPositiveWindup) {
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ while (claw_.state() != Claw::ZEROING) {
+ RunIteration();
}
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ const double orig_claw_goal = claw_.claw_goal_;
+ claw_.claw_goal_ += 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(claw_.claw_loop_->U(), claw_.claw_loop_->U_uncapped());
+}
+
+// Tests that the claw zeroing goals don't wind up too far.
+TEST_F(ClawTest, GoalNegativeWindup) {
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ while (claw_.state() != Claw::ZEROING) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_claw_goal = claw_.claw_goal_;
+ claw_.claw_goal_ -= 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_claw_goal, claw_.claw_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(claw_.claw_loop_->U(), claw_.claw_loop_->U_uncapped());
}
} // namespace testing