Moved wrist loop to a subfolder.
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
new file mode 100644
index 0000000..4e116c7
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -0,0 +1,233 @@
+#include "frc971/control_loops/wrist/wrist.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
+ : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
+ loop_(new WristStateFeedbackLoop(MakeWristLoop(), this)),
+ state_(UNINITIALIZED),
+ error_count_(0),
+ zero_offset_(0.0) {
+}
+
+bool WristMotor::FetchConstants() {
+ if (!constants::wrist_lower_limit(&wrist_lower_limit_)) {
+ LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
+ return false;
+ }
+ if (!constants::wrist_upper_limit(&wrist_upper_limit_)) {
+ LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
+ return false;
+ }
+ if (!constants::wrist_hall_effect_start_angle(
+ &wrist_hall_effect_start_angle_)) {
+ LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
+ return false;
+ }
+ if (!constants::wrist_zeroing_speed(
+ &wrist_zeroing_speed_)) {
+ LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
+ return false;
+ }
+
+ return true;
+}
+
+double WristMotor::ClipGoal(double goal) const {
+ return std::min(wrist_upper_limit_,
+ std::max(wrist_lower_limit_, goal));
+}
+
+const double kMaxZeroingVoltage = 5.0;
+
+void WristMotor::WristStateFeedbackLoop::CapU() {
+ if (wrist_motor_->state_ == READY) {
+ if (Y(0, 0) >= wrist_motor_->wrist_upper_limit_) {
+ U(0, 0) = std::min(0.0, U(0, 0));
+ }
+ if (Y(0, 0) <= wrist_motor_->wrist_lower_limit_) {
+ U(0, 0) = std::max(0.0, U(0, 0));
+ }
+ }
+
+ 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.
+void WristMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * /*status*/) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ // Cache the constants to avoid error handling down below.
+ if (!FetchConstants()) {
+ return;
+ }
+
+ // Uninitialize the bot if too many cycles pass without an encoder.
+ if (position == NULL) {
+ LOG(WARNING, "no new pos given\n");
+ error_count_++;
+ } else {
+ error_count_ = 0;
+ }
+ if (error_count_ >= 4) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ state_ = UNINITIALIZED;
+ }
+
+ // Compute the absolute position of the wrist.
+ double absolute_position;
+ if (position) {
+ absolute_position =
+ position->pos + wrist_hall_effect_start_angle_;
+ if (state_ == READY) {
+ absolute_position -= zero_offset_;
+ }
+ loop_->Y << absolute_position;
+ if (!position->hall_effect) {
+ last_off_position_ = position->pos;
+ }
+ } else {
+ // Dead recon for now.
+ absolute_position = loop_->X_hat(0, 0);
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ if (position) {
+ // Reset the zeroing goal.
+ zeroing_position_ = absolute_position;
+ // Clear the observer state.
+ loop_->X_hat << absolute_position, 0.0;
+ // Set the goal to here to make it so it doesn't move when disabled.
+ loop_->R = loop_->X_hat;
+ // Only progress if we are enabled.
+ if (::aos::robot_state->enabled) {
+ if (position->hall_effect) {
+ state_ = MOVING_OFF;
+ } else {
+ state_ = ZEROING;
+ }
+ }
+ }
+ break;
+ case MOVING_OFF:
+ // Move off the hall effect sensor.
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && !position->hall_effect) {
+ // We are now off the sensor. Time to zero now.
+ state_ = ZEROING;
+ } else {
+ // Slowly creep off the sensor.
+ zeroing_position_ -= wrist_zeroing_speed_ / 100;
+ loop_->R << zeroing_position_, -wrist_zeroing_speed_;
+ break;
+ }
+ case ZEROING:
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && position->hall_effect) {
+ state_ = READY;
+ // Verify that the calibration number is between the last off position
+ // and the current on position. If this is not true, move off and try
+ // again.
+ if (position->calibration <= last_off_position_ ||
+ position->calibration > position->pos) {
+ LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
+ LOG(ERROR,
+ "Last off position was %f, current is %f, calibration is %f\n",
+ last_off_position_, position->pos, position->calibration);
+ state_ = MOVING_OFF;
+ } else {
+ // Save the zero, and then offset the observer to deal with the
+ // phantom step change.
+ const double old_zero_offset = zero_offset_;
+ zero_offset_ = position->calibration;
+ loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
+ loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ }
+ } else {
+ // Slowly creep towards the sensor.
+ zeroing_position_ += wrist_zeroing_speed_ / 100;
+ loop_->R << zeroing_position_, wrist_zeroing_speed_;
+ }
+ break;
+
+ case READY:
+ {
+ const double limited_goal = ClipGoal(goal->goal);
+ loop_->R << limited_goal, 0.0;
+ break;
+ }
+
+ case ESTOP:
+ LOG(WARNING, "have already given up\n");
+ return;
+ }
+
+ // 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,
+ position->hall_effect ? "true" : "false");
+ }
+
+ if (output) {
+ output->voltage = loop_->U(0, 0);
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
new file mode 100644
index 0000000..7c16577
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -0,0 +1,70 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'wrist_loop',
+ 'type': 'static_library',
+ 'sources': ['wrist_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/wrist',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'wrist_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'wrist.cc',
+ 'wrist_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'wrist_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'wrist_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrist_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'wrist_loop',
+ 'wrist_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrist',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'wrist_lib',
+ 'wrist_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
new file mode 100644
index 0000000..250f536
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.h
@@ -0,0 +1,105 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+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:
+ explicit WristMotor(
+ control_loops::WristLoop *my_wrist = &control_loops::wrist);
+
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // 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();
+
+ // 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;
+
+ // 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<WristStateFeedbackLoop> loop_;
+
+ // Enum to store the state of the internal zeroing state machine.
+ enum State {
+ UNINITIALIZED,
+ MOVING_OFF,
+ ZEROING,
+ READY,
+ ESTOP
+ };
+
+ // Internal state for zeroing.
+ State state_;
+
+ // Missed position packet count.
+ int error_count_;
+ // Offset from the raw encoder value to the absolute angle.
+ double zero_offset_;
+ // Position that gets incremented when zeroing the wrist to slowly move it to
+ // the hall effect sensor.
+ double zeroing_position_;
+ // Last position at which the hall effect sensor was off.
+ double last_off_position_;
+
+ // Local cache of the wrist geometry constants.
+ double wrist_lower_limit_;
+ double wrist_upper_limit_;
+ double wrist_hall_effect_start_angle_;
+ double wrist_zeroing_speed_;
+
+ DISALLOW_COPY_AND_ASSIGN(WristMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
new file mode 100644
index 0000000..8e852c1
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -0,0 +1,338 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/wrist/wrist.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the wrist and sends out queue messages containing the
+// position.
+class WristMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // wrist, which will be treated as 0 by the encoder.
+ WristMotorSimulation(double initial_position)
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeWristPlant())),
+ my_wrist_loop_(".frc971.control_loops.wrist",
+ 0x1a7b7094, ".frc971.control_loops.wrist.goal",
+ ".frc971.control_loops.wrist.position",
+ ".frc971.control_loops.wrist.output",
+ ".frc971.control_loops.wrist.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ wrist_plant_->X(0, 0) = initial_position_;
+ wrist_plant_->X(1, 0) = 0.0;
+ wrist_plant_->Y = wrist_plant_->C * wrist_plant_->X;
+ last_position_ = wrist_plant_->Y(0, 0);
+ calibration_value_ = 0.0;
+ }
+
+ // Returns the absolute angle of the wrist.
+ double GetAbsolutePosition() const {
+ return wrist_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the wrist.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ double wrist_hall_effect_start_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_start_angle(
+ &wrist_hall_effect_start_angle));
+ double wrist_hall_effect_stop_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_stop_angle(
+ &wrist_hall_effect_stop_angle));
+
+ ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
+ my_wrist_loop_.position.MakeMessage();
+ position->pos = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position >= wrist_hall_effect_start_angle &&
+ abs_position <= wrist_hall_effect_stop_angle) {
+ position->hall_effect = true;
+ } else {
+ position->hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ if ((last_position_ < wrist_hall_effect_start_angle ||
+ last_position_ > wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
+ calibration_value_ =
+ wrist_hall_effect_start_angle - initial_position_;
+ }
+
+ position->calibration = calibration_value_;
+ position.Send();
+ }
+
+ // Simulates the wrist moving for one timestep.
+ void Simulate() {
+ last_position_ = wrist_plant_->Y(0, 0);
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ wrist_plant_->U << my_wrist_loop_.output->voltage;
+ wrist_plant_->Update();
+
+ // Assert that we are in the right physical range.
+ double wrist_upper_physical_limit;
+ ASSERT_TRUE(constants::wrist_upper_physical_limit(
+ &wrist_upper_physical_limit));
+ double wrist_lower_physical_limit;
+ ASSERT_TRUE(constants::wrist_lower_physical_limit(
+ &wrist_lower_physical_limit));
+
+ EXPECT_GE(wrist_upper_physical_limit,
+ wrist_plant_->Y(0, 0));
+ EXPECT_LE(wrist_lower_physical_limit,
+ wrist_plant_->Y(0, 0));
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ private:
+ WristLoop my_wrist_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+};
+
+class WristTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ WristLoop my_wrist_loop_;
+
+ // Create a loop and simulation plant.
+ WristMotor wrist_motor_;
+ WristMotorSimulation wrist_motor_plant_;
+
+ WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
+ 0x1a7b7094, ".frc971.control_loops.wrist.goal",
+ ".frc971.control_loops.wrist.position",
+ ".frc971.control_loops.wrist.output",
+ ".frc971.control_loops.wrist.status"),
+ wrist_motor_(&my_wrist_loop_),
+ wrist_motor_plant_(0.5) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_wrist_loop_.goal.FetchLatest();
+ my_wrist_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_wrist_loop_.goal->goal,
+ wrist_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~WristTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(WristTest, ZerosCorrectly) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_F(WristTest, ZerosStartingOn) {
+ wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
+
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(WristTest, HandleMissingPosition) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ wrist_motor_plant_.SendPositionMessage();
+ }
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(WristTest, RezeroWithMissingPos) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ // After 3 seconds, simulate the encoder going missing.
+ // This should trigger a re-zero. To make sure it works, change the goal as
+ // well.
+ if (i < 300 || i > 400) {
+ wrist_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ }
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 430) {
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(WristTest, DisableGoesUninitialized) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // 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);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ }
+ 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
diff --git a/frc971/control_loops/wrist/wrist_main.cc b/frc971/control_loops/wrist/wrist_main.cc
new file mode 100644
index 0000000..8585180
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/wrist/wrist.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::WristMotor wrist;
+ wrist.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/wrist/wrist_motor.q b/frc971/control_loops/wrist/wrist_motor.q
new file mode 100644
index 0000000..3dbeb53
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor.q
@@ -0,0 +1,21 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group WristLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Position {
+ double pos;
+ bool hall_effect;
+ // The exact pos when hall_effect last changed
+ double calibration;
+ };
+
+ queue aos.control_loops.Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group WristLoop wrist;
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.cc b/frc971/control_loops/wrist/wrist_motor_plant.cc
new file mode 100644
index 0000000..3998cae
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeWristPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00876530955899, 0.0, 0.763669024671;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000423500644841, 0.0810618735867;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeWristLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.66366902467, 58.1140316091;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 31.5808145893, 0.867171288023;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeWristPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.h b/frc971/control_loops/wrist/wrist_motor_plant.h
new file mode 100644
index 0000000..f2b3ac9
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeWristPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeWristLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_