Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
new file mode 100644
index 0000000..c1a734e
--- /dev/null
+++ b/y2015/control_loops/claw/claw.cc
@@ -0,0 +1,311 @@
+#include "y2015/control_loops/claw/claw.h"
+
+#include <algorithm>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw_motor_plant.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+namespace frc971 {
+namespace control_loops {
+
+using ::aos::time::Time;
+
+constexpr double kZeroingVoltage = 4.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),
+ last_piston_edge_(Time::Now()),
+ claw_loop_(new ClawCappedStateFeedbackLoop(MakeClawLoop())),
+ claw_estimator_(constants::GetValues().claw.zeroing),
+ profile_(::aos::controls::kLoopFrequency) {}
+
+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 *unsafe_goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ control_loops::ClawQueue::Status *status) {
+ const auto &values = constants::GetValues();
+
+ 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(DEBUG, "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();
+ }
+
+ // Clear the current profile state if we are zeroing.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << claw_goal_, claw_goal_velocity;
+ profile_.MoveCurrentState(current);
+ }
+ break;
+
+ case RUNNING:
+ LOG(DEBUG, "Running!\n");
+
+ // Update state_.
+ UpdateZeroingState();
+ if (unsafe_goal) {
+ // Pick a set of sane defaults if none are specified.
+ if (unsafe_goal->max_velocity != 0.0) {
+ profile_.set_maximum_velocity(unsafe_goal->max_velocity);
+ } else {
+ profile_.set_maximum_velocity(2.5);
+ }
+ if (unsafe_goal->max_acceleration != 0.0) {
+ profile_.set_maximum_acceleration(unsafe_goal->max_acceleration);
+ } else {
+ profile_.set_maximum_acceleration(4.0);
+ }
+
+ const double unfiltered_goal = ::std::max(
+ ::std::min(unsafe_goal->angle, values.claw.wrist.upper_limit),
+ values.claw.wrist.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unfiltered_goal, unsafe_goal->angular_velocity);
+ claw_goal_ = goal_state(0, 0);
+ claw_goal_velocity = goal_state(1, 0);
+ }
+
+ 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);
+ }
+ }
+
+ // 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 (state_ != RUNNING) {
+ output->intake_voltage = 0.0;
+ output->rollers_closed = false;
+ } else {
+ 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_;
+ zeroing::PopulateEstimatorState(claw_estimator_, &status->zeroing_state);
+
+ status->angle = claw_loop_->X_hat(0, 0);
+ status->angular_velocity = claw_loop_->X_hat(1, 0);
+
+ if (output) {
+ status->intake = output->intake_voltage;
+ } else {
+ status->intake = 0;
+ }
+ status->goal_angle = claw_goal_;
+ status->goal_velocity = claw_goal_velocity;
+
+ 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
+} // namespace frc971
diff --git a/y2015/control_loops/claw/claw.gyp b/y2015/control_loops/claw/claw.gyp
new file mode 100644
index 0000000..780f065
--- /dev/null
+++ b/y2015/control_loops/claw/claw.gyp
@@ -0,0 +1,89 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'replay_claw',
+ 'type': 'executable',
+ 'variables': {
+ 'no_rsync': 1,
+ },
+ 'sources': [
+ 'replay_claw.cc',
+ ],
+ 'dependencies': [
+ 'claw_queue',
+ '<(AOS)/common/controls/controls.gyp:replay_control_loop',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'claw_queue',
+ 'type': 'static_library',
+ 'sources': ['claw.q'],
+ 'variables': {
+ 'header_path': 'y2015/control_loops/claw',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'claw_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'claw.cc',
+ 'claw_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/y2015/y2015.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',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
+ ],
+ },
+ {
+ 'target_name': 'claw_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'claw_lib',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ ],
+ },
+ {
+ 'target_name': 'claw',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_lib',
+ ],
+ },
+ ],
+}
diff --git a/y2015/control_loops/claw/claw.h b/y2015/control_loops/claw/claw.h
new file mode 100644
index 0000000..02e5398
--- /dev/null
+++ b/y2015/control_loops/claw/claw.h
@@ -0,0 +1,116 @@
+#ifndef Y2015_CONTROL_LOOPS_CLAW_H_
+#define Y2015_CONTROL_LOOPS_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2015/control_loops/claw/claw.q.h"
+#include "y2015/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);
+
+ protected:
+ virtual void RunIteration(const control_loops::ClawQueue::Goal *goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ 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<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;
+
+ ::aos::util::TrapezoidProfile profile_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_CLAW_H_
diff --git a/y2015/control_loops/claw/claw.q b/y2015/control_loops/claw/claw.q
new file mode 100644
index 0000000..8da1c20
--- /dev/null
+++ b/y2015/control_loops/claw/claw.q
@@ -0,0 +1,81 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ClawQueue {
+ implements aos.control_loops.ControlLoop;
+
+ // All angles are in radians with 0 sticking straight out the front. Rotating
+ // up and into the robot is positive. Positive output voltage moves in the
+ // direction of positive encoder values.
+
+ message Goal {
+ // Angle of wrist joint.
+ double angle;
+ // Angular velocity of wrist.
+ float angular_velocity;
+ // Maximum profile velocity, or 0 for the default.
+ float max_velocity;
+ // Maximum profile acceleration, or 0 for the default.
+ float max_acceleration;
+ // Voltage of intake rollers. Positive means sucking in, negative means
+ // spitting out.
+ double intake;
+
+ // true to signal the rollers to close.
+ bool rollers_closed;
+ };
+
+ message Position {
+ PotAndIndexPosition joint;
+ };
+
+ message Output {
+ // Voltage for intake motors. Positive is sucking in, negative is spitting
+ // out.
+ double intake_voltage;
+ // Voltage for claw motor.
+ double voltage;
+
+ // true to signal the rollers to close.
+ bool rollers_closed;
+ };
+
+ message Status {
+ // Is claw zeroed?
+ bool zeroed;
+ // Did the claw estop?
+ bool estopped;
+ // The internal state of the claw state machine.
+ uint32_t state;
+ EstimatorState zeroing_state;
+
+ // Estimated angle of wrist joint.
+ double angle;
+ // Estimated angular velocity of wrist.
+ float angular_velocity;
+
+ // Goal angle of wrist joint.
+ double goal_angle;
+ float goal_velocity;
+ // Voltage of intake rollers. Positive means sucking in, negative means
+ // spitting out.
+ double intake;
+
+ // True iff there has been enough time since we actuated the rollers outward
+ // that they should be there.
+ bool rollers_open;
+ // True iff there has been enough time since we actuated the rollers closed
+ // that they should be there.
+ bool rollers_closed;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ClawQueue claw_queue;
+
diff --git a/y2015/control_loops/claw/claw_lib_test.cc b/y2015/control_loops/claw/claw_lib_test.cc
new file mode 100644
index 0000000..10fcc07
--- /dev/null
+++ b/y2015/control_loops/claw/claw_lib_test.cc
@@ -0,0 +1,347 @@
+#include <math.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2015/control_loops/claw/claw.q.h"
+#include "y2015/control_loops/claw/claw.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "y2015/constants.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the claw and sends out queue messages with the
+// position.
+class ClawSimulation {
+ public:
+ // Constructs a claw simulation.
+ ClawSimulation()
+ : claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeClawPlant())),
+ pot_and_encoder_(constants::GetValues().claw.zeroing.index_difference),
+ claw_queue_(".frc971.control_loops.claw_queue", 0x9d7452fb,
+ ".frc971.control_loops.claw_queue.goal",
+ ".frc971.control_loops.claw_queue.position",
+ ".frc971.control_loops.claw_queue.output",
+ ".frc971.control_loops.claw_queue.status") {
+ 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 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.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ClawQueue::Position> position =
+ claw_queue_.position.MakeMessage();
+ pot_and_encoder_.GetSensorValues(&position->joint);
+ position.Send();
+ }
+
+ // Simulates for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(claw_queue_.output.FetchLatest());
+
+ // Feed voltages into physics simulation.
+ claw_plant_->mutable_U() << claw_queue_.output->voltage;
+ claw_plant_->Update();
+
+ const double wrist_angle = claw_plant_->Y(0, 0);
+
+ // TODO(danielp): Sanity checks.
+
+ pot_and_encoder_.MoveTo(wrist_angle);
+ }
+
+ private:
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> claw_plant_;
+ PositionSensorSimulator pot_and_encoder_;
+
+ ClawQueue claw_queue_;
+};
+
+class ClawTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ ClawTest()
+ : claw_queue_(".frc971.control_loops.claw_queue", 0x9d7452fb,
+ ".frc971.control_loops.claw_queue.goal",
+ ".frc971.control_loops.claw_queue.position",
+ ".frc971.control_loops.claw_queue.output",
+ ".frc971.control_loops.claw_queue.status"),
+ claw_(&claw_queue_),
+ claw_plant_() {
+ set_team_id(kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ claw_queue_.goal.FetchLatest();
+ claw_queue_.status.FetchLatest();
+ 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(bool enabled = true) {
+ SendMessages(enabled);
+
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // 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.
+ ClawQueue claw_queue_;
+
+ // Create a control loop and simulation.
+ Claw claw_;
+ ClawSimulation claw_plant_;
+};
+
+// Tests that the loop does nothing when the goal is our lower limit.
+TEST_F(ClawTest, DoesNothing) {
+ const auto &values = constants::GetValues();
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.lower_limit)
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+
+ // We should not have moved.
+ 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::InSeconds(6));
+
+ 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()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+
+ VerifyNearGoal();
+}
+
+// Tests that it doesn't try to move past the physical range of the mechanism.
+TEST_F(ClawTest, RespectsRange) {
+ const auto &values = constants::GetValues();
+ // Upper limit
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.upper_hard_limit + 5.0)
+ .Send());
+
+ RunForTime(Time::InSeconds(7));
+
+ claw_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.claw.wrist.upper_limit,
+ claw_queue_.status->angle,
+ 2.0 * M_PI / 180.0);
+
+ // Lower limit.
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.lower_hard_limit - 5.0)
+ .Send());
+
+ RunForTime(Time::InMS(6000));
+
+ claw_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.claw.wrist.lower_limit,
+ claw_queue_.status->angle,
+ 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::InSeconds(6));
+
+ 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::InSeconds(15));
+
+ VerifyNearGoal();
+}
+
+
+// Tests that not having a goal doesn't break anything.
+TEST_F(ClawTest, NoGoal) {
+ RunForTime(Time::InMS(50));
+}
+
+// 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::InSeconds(6));
+ 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::InSeconds(6));
+ 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.10);
+
+ 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.10);
+
+ RunIteration();
+
+ EXPECT_EQ(claw_.claw_loop_->U(), claw_.claw_loop_->U_uncapped());
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/claw/claw_main.cc b/y2015/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..d1b869c
--- /dev/null
+++ b/y2015/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "y2015/control_loops/claw/claw.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::Claw claw;
+ claw.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/control_loops/claw/claw_motor_plant.cc b/y2015/control_loops/claw/claw_motor_plant.cc
new file mode 100644
index 0000000..5f5c206
--- /dev/null
+++ b/y2015/control_loops/claw/claw_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "y2015/control_loops/claw/claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00482455476758, 0.0, 0.930652495326;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 6.97110924671e-05, 0.0275544125308;
+ 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> MakeClawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.998251427366, 26.9874526231;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 74.4310031124, 4.72251126222;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0, -0.00518405612386, 0.0, 1.07451492907;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeClawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeClawPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeClawPlantCoefficients()));
+ return StateFeedbackPlant<2, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeClawLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeClawController()));
+ return StateFeedbackLoop<2, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/claw/claw_motor_plant.h b/y2015/control_loops/claw/claw_motor_plant.h
new file mode 100644
index 0000000..6cb9cdb
--- /dev/null
+++ b/y2015/control_loops/claw/claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+#define Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/claw/replay_claw.cc b/y2015/control_loops/claw/replay_claw.cc
new file mode 100644
index 0000000..50673e4
--- /dev/null
+++ b/y2015/control_loops/claw/replay_claw.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2015/control_loops/claw/claw.q.h"
+
+// Reads one or more log files and sends out all the queue messages (in the
+// correct order and at the correct time) to feed a "live" claw process.
+
+int main(int argc, char **argv) {
+ if (argc <= 1) {
+ fprintf(stderr, "Need at least one file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::InitNRT();
+
+ ::aos::controls::ControlLoopReplayer<::frc971::control_loops::ClawQueue>
+ replayer(&::frc971::control_loops::claw_queue, "claw");
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
+
+ ::aos::Cleanup();
+}