Renamed everything to claw.
- Renamed all the wrists calls to claw.
- Added a top and bottom wrist controller.
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.cc b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
new file mode 100644
index 0000000..a0eb131
--- /dev/null
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeBottomClawController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.81581823335, 78.6334534274, 142.868137351;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 92.6004807973, 4.38063492858, 1.11581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeBottomClawPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeBottomClawPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeBottomClawController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.h b/frc971/control_loops/claw/bottom_claw_motor_plant.h
new file mode 100644
index 0000000..fc905ca
--- /dev/null
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeBottomClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
new file mode 100644
index 0000000..55cbcc5
--- /dev/null
+++ b/frc971/control_loops/claw/claw.cc
@@ -0,0 +1,113 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+// Zeroing plan.
+// There are 2 types of zeros. Enabled and disabled ones.
+// Disabled ones are only valid during auto mode, and can be used to speed up
+// the enabled zero process. We need to re-zero during teleop in case the auto
+// zero was poor and causes us to miss all our shots.
+//
+// We need to be able to zero manually while disabled by moving the joint over
+// the zeros.
+// Zero on the down edge when disabled (gravity in the direction of motion)
+//
+// When enabled, zero on the up edge (gravity opposing the direction of motion)
+// The enabled sequence needs to work as follows. We can crash the claw if we
+// bring them too close to each other or too far from each other. The only safe
+// thing to do is to move them in unison.
+//
+// Start by moving them both towards the front of the bot to either find either
+// the middle hall effect on either jaw, or the front hall effect on the bottom
+// jaw. Any edge that isn't the desired edge will provide an approximate edge
+// location that can be used for the fine tuning step.
+// Once an edge is found on the front claw, move back the other way with both
+// claws until an edge is found for the other claw.
+// Now that we have an approximate zero, we can robustify the limits to keep
+// both claws safe. Then, we can move both claws to a position that is the
+// correct side of the zero and go zero.
+
+// Valid region plan.
+// Difference between the arms has a range, and the values of each arm has a range.
+// If a claw runs up against a static limit, don't let the goal change outside
+// the limit.
+// If a claw runs up against a movable limit, move both claws outwards to get
+// out of the condition.
+
+namespace frc971 {
+namespace control_loops {
+
+ClawMotor::ClawMotor(control_loops::ClawLoop *my_claw)
+ : aos::control_loops::ControlLoop<control_loops::ClawLoop>(my_claw),
+ zeroed_joint_(MakeTopClawLoop()) {
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
+
+ config_data.lower_limit = GetValues().claw_lower_limit;
+ config_data.upper_limit = GetValues().claw_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().claw_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().claw_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().claw_zeroing_speed;
+
+ config_data.max_zeroing_voltage = 5.0;
+ config_data.deadband_voltage = 0.0;
+
+ zeroed_joint_.set_config_data(config_data);
+ }
+}
+
+// Positive angle is up, and positive power is up.
+void ClawMotor::RunIteration(const control_loops::ClawLoop::Goal *goal,
+ const control_loops::ClawLoop::Position *position,
+ control_loops::ClawLoop::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->top_claw_voltage = 0;
+ output->bottom_claw_voltage = 0;
+ output->intake_voltage = 0;
+ }
+
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->top_position;
+ transformed_position.hall_effects[0] = position->top_calibration_hall_effect;
+ transformed_position.hall_effect_positions[0] = position->top_posedge_value;
+ }
+
+ const double voltage =
+ zeroed_joint_.Update(transformed_position_ptr, output != NULL,
+ goal->bottom_angle + goal->seperation_angle, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
+ position->top_calibration_hall_effect ? "true" : "false",
+ zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->top_claw_voltage = voltage;
+ }
+ status->done = ::std::abs(zeroed_joint_.absolute_position() -
+ goal->bottom_angle - goal->seperation_angle) < 0.004;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
new file mode 100644
index 0000000..c541dad
--- /dev/null
+++ b/frc971/control_loops/claw/claw.gyp
@@ -0,0 +1,68 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'claw_loops',
+ 'type': 'static_library',
+ 'sources': ['claw.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/claw',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'claw_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'claw.cc',
+ 'top_claw_motor_plant.cc',
+ 'bottom_claw_motor_plant.cc',
+ 'unaugmented_top_claw_motor_plant.cc',
+ 'unaugmented_bottom_claw_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'claw_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'claw_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'claw_loops',
+ 'claw_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_lib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
new file mode 100644
index 0000000..e057c3e
--- /dev/null
+++ b/frc971/control_loops/claw/claw.h
@@ -0,0 +1,63 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+#define FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class ClawTest_NoWindupPositive_Test;
+class ClawTest_NoWindupNegative_Test;
+};
+
+class ClawMotor
+ : public aos::control_loops::ControlLoop<control_loops::ClawLoop> {
+ public:
+ explicit ClawMotor(
+ control_loops::ClawLoop *my_claw = &control_loops::claw);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the claw is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the claw is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ClawLoop::Goal *goal,
+ const control_loops::ClawLoop::Position *position,
+ control_loops::ClawLoop::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ClawTest_NoWindupPositive_Test;
+ friend class testing::ClawTest_NoWindupNegative_Test;
+
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(ClawMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
new file mode 100644
index 0000000..8852fd3
--- /dev/null
+++ b/frc971/control_loops/claw/claw.q
@@ -0,0 +1,57 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+// All angles here are 0 horizontal, positive up.
+queue_group ClawLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The angle of the bottom wrist.
+ double bottom_angle;
+ // How much higher the top wrist is.
+ double seperation_angle;
+ bool intake;
+ };
+ message Position {
+ double top_position;
+
+ bool top_front_hall_effect;
+ int32_t top_front_hall_effect_posedge_count;
+ int32_t top_front_hall_effect_negedge_count;
+ bool top_calibration_hall_effect;
+ int32_t top_calibration_hall_effect_posedge_count;
+ int32_t top_calibration_hall_effect_negedge_count;
+ bool top_back_hall_effect;
+ int32_t top_back_hall_effect_posedge_count;
+ int32_t top_back_hall_effect_negedge_count;
+ double top_posedge_value;
+ double top_negedge_value;
+
+ double bottom_position;
+ bool bottom_front_hall_effect;
+ int32_t bottom_front_hall_effect_posedge_count;
+ int32_t bottom_front_hall_effect_negedge_count;
+ bool bottom_calibration_hall_effect;
+ int32_t bottom_calibration_hall_effect_posedge_count;
+ int32_t bottom_calibration_hall_effect_negedge_count;
+ bool bottom_back_hall_effect;
+ int32_t bottom_back_hall_effect_posedge_count;
+ int32_t bottom_back_hall_effect_negedge_count;
+ double bottom_posedge_value;
+ double bottom_negedge_value;
+ };
+
+ message Output {
+ double intake_voltage;
+ double top_claw_voltage;
+ double bottom_claw_voltage;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group ClawLoop claw;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
new file mode 100644
index 0000000..e881ef2
--- /dev/null
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -0,0 +1,334 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/claw.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 ClawMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // wrist, which will be treated as 0 by the encoder.
+ ClawMotorSimulation(double initial_position)
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawClawPlant())),
+ my_wrist_loop_(".frc971.control_loops.claw",
+ 0x1a7b7094, ".frc971.control_loops.claw.goal",
+ ".frc971.control_loops.claw.position",
+ ".frc971.control_loops.claw.output",
+ ".frc971.control_loops.claw.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;
+ last_voltage_ = 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();
+
+ ::aos::ScopedMessagePtr<control_loops::ClawLoop::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 >= constants::GetValues().wrist_hall_effect_start_angle &&
+ abs_position <= constants::GetValues().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_ <
+ constants::GetValues().wrist_hall_effect_start_angle ||
+ last_position_ >
+ constants::GetValues().wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
+ calibration_value_ =
+ constants::GetValues().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 << last_voltage_;
+ wrist_plant_->Update();
+
+ EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
+ wrist_plant_->Y(0, 0));
+ EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
+ wrist_plant_->Y(0, 0));
+ last_voltage_ = my_wrist_loop_.output->voltage;
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ private:
+ ClawLoop my_wrist_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+ double last_voltage_;
+};
+
+class ClawTest : 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.
+ ClawLoop my_wrist_loop_;
+
+ // Create a loop and simulation plant.
+ ClawMotor wrist_motor_;
+ ClawMotorSimulation wrist_motor_plant_;
+
+ ClawTest() : 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 ~ClawTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ClawTest, 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(ClawTest, 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(ClawTest, 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(ClawTest, 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_TRUE(wrist_motor_.is_uninitialized());
+ }
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 410) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ 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(ClawTest, 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_TRUE(wrist_motor_.is_uninitialized());
+ // 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_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ 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(ClawTest, NoWindupNegative) {
+ int capped_count = 0;
+ 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_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ } else if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position,
+ wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // 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.
+ ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(ClawTest, NoWindupPositive) {
+ int capped_count = 0;
+ 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_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ } else {
+ if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // 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_.zeroed_joint_.U_uncapped()));
+ } else {
+ EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_main.cc b/frc971/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..3b7c181
--- /dev/null
+++ b/frc971/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ClawMotor claw;
+ claw.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.cc b/frc971/control_loops/claw/top_claw_motor_plant.cc
new file mode 100644
index 0000000..113ff77
--- /dev/null
+++ b/frc971/control_loops/claw/top_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeTopClawController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.81581823335, 78.6334534274, 142.868137351;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 92.6004807973, 4.38063492858, 1.11581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeTopClawPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeTopClawPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeTopClawController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.h b/frc971/control_loops/claw/top_claw_motor_plant.h
new file mode 100644
index 0000000..c74c976
--- /dev/null
+++ b/frc971/control_loops/claw/top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeTopClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
new file mode 100644
index 0000000..cda15c4
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawBottomClawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawBottomClawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawBottomClawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
new file mode 100644
index 0000000..8f59925
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
new file mode 100644
index 0000000..8ab4bbf
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawTopClawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawTopClawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawTopClawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawTopClawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
new file mode 100644
index 0000000..c87d3ca
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawTopClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_