Wrist loop test case now runs and fails.
diff --git a/aos/atom_code/queue-tmpl.h b/aos/atom_code/queue-tmpl.h
index 3a8eea8..530f30f 100644
--- a/aos/atom_code/queue-tmpl.h
+++ b/aos/atom_code/queue-tmpl.h
@@ -179,6 +179,15 @@
}
template <class T>
+void Queue<T>::Clear() {
+ if (queue_ == NULL) {
+ queue_msg_.reset();
+ queue_ = NULL;
+ queue_msg_.set_queue(NULL);
+ }
+}
+
+template <class T>
bool Queue<T>::FetchNext() {
Init();
// TODO(aschuh): Use aos_queue_read_msg_index so that multiple readers
diff --git a/aos/common/control_loop/control_loops.q b/aos/common/control_loop/control_loops.q
index 5ba30ec..823005e 100644
--- a/aos/common/control_loop/control_loops.q
+++ b/aos/common/control_loop/control_loops.q
@@ -20,7 +20,7 @@
};
message Output {
- double pwm;
+ double voltage;
};
message Status {
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 7b38e67..2ae2f0a 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -202,6 +202,10 @@
// take a different amount of time the first cycle.
void Init();
+ // Removes all internal references to shared memory so shared memory can be
+ // restarted safely. This should only be used in testing.
+ void Clear();
+
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it. This removes the message from the queue for all readers.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index d727036..01545ff 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -6,6 +6,7 @@
'dependencies': [
'<(AOS)/build/aos_all.gyp:Atom',
'../control_loops/control_loops.gyp:DriveTrain',
+ '../control_loops/control_loops.gyp:wrist_lib_test',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 8bdc7a0..d0fe261 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -2,6 +2,7 @@
'variables': {
'loop_files': [
'DriveTrain.q',
+ 'wrist_motor.q',
]
},
'targets': [
@@ -23,6 +24,48 @@
'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',
+ 'control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'wrist_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ 'wrist_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'wrist',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'wrist_lib',
+ 'control_loops',
+ ],
+ },
+ {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index a2e46e4..0b22945 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -116,6 +116,42 @@
ans.append("}\n")
return "".join(ans)
+ def DumpLoopHeader(self, loop_name):
+ """Writes out a c++ header declaration which will create a Loop object.
+
+ Args:
+ loop_name: string, the name of the loop. Used to create the name of the
+ function. The function name will be Make<loop_name>Loop().
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return "StateFeedbackLoop<%d, %d, %d> Make%sLoop();\n" % (
+ num_states, num_inputs, num_outputs, loop_name)
+
+ def DumpLoop(self, loop_name):
+ """Writes out a c++ function which will create a Loop object.
+
+ Args:
+ loop_name: string, the name of the loop. Used to create the name of the
+ function and create the plant. The function name will be
+ Make<loop_name>Loop().
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ["StateFeedbackLoop<%d, %d, %d> Make%sLoop() {\n" % (
+ num_states, num_inputs, num_outputs, loop_name)]
+
+ ans.append(self._DumpMatrix("L", self.L))
+ ans.append(self._DumpMatrix("K", self.K))
+
+ ans.append(" return StateFeedbackLoop<%d, %d, %d>"
+ "(Make%sPlant(), L, K);\n" % (num_states, num_inputs,
+ num_outputs, loop_name))
+ ans.append("}\n")
+ return "".join(ans)
+
def main(argv):
wrist = Wrist()
@@ -124,8 +160,8 @@
wrist.Update(numpy.matrix([[12.0]]))
simulated_x.append(wrist.X[0, 0])
- pylab.plot(range(100), simulated_x)
- pylab.show()
+ #pylab.plot(range(100), simulated_x)
+ #pylab.show()
wrist = Wrist()
close_loop_x = []
@@ -137,8 +173,8 @@
wrist.Update(U)
close_loop_x.append(wrist.X[0, 0])
- pylab.plot(range(100), close_loop_x)
- pylab.show()
+ #pylab.plot(range(100), close_loop_x)
+ #pylab.show()
if len(argv) != 3:
print "Expected .cc file name and .h file name"
@@ -150,22 +186,31 @@
"} // namespace control_loops\n");
header_start = ("#ifndef FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n"
- "#define // FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n\n")
+ "#define FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n\n")
header_end = "#endif // FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n";
with open(argv[1], "w") as fd:
+ fd.write("#include \"frc971/control_loops/wrist_motor_plant.h\"\n")
+ fd.write('\n')
+ fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
+ fd.write('\n')
fd.write(namespace_start)
+ fd.write('\n')
fd.write(wrist.DumpPlant("Wrist"))
fd.write('\n')
+ fd.write(wrist.DumpLoop("Wrist"))
+ fd.write('\n')
fd.write(namespace_end)
with open(argv[2], "w") as fd:
fd.write(header_start)
- fd.write(namespace_start)
fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
fd.write('\n')
+ fd.write(namespace_start)
fd.write(wrist.DumpPlantHeader("Wrist"))
fd.write('\n')
+ fd.write(wrist.DumpLoopHeader("Wrist"))
+ fd.write('\n')
fd.write(namespace_end)
fd.write(header_end)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
new file mode 100644
index 0000000..12ddab7
--- /dev/null
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -0,0 +1,158 @@
+#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+
+// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
+
+#include "Eigen/Dense"
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> A;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+
+ Eigen::Matrix<double, number_of_states, 1> X;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+ Eigen::Matrix<double, number_of_inputs, 1> U;
+
+ StateFeedbackPlant(const StateFeedbackPlant &other)
+ : A(other.A),
+ B(other.B),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {
+ X.setZero();
+ Y.setZero();
+ U.setZero();
+ }
+
+ StateFeedbackPlant(
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<double, number_of_outputs, 1> &U_max,
+ const Eigen::Matrix<double, number_of_outputs, 1> &U_min)
+ : A(A),
+ B(B),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max) {
+ X.setZero();
+ Y.setZero();
+ U.setZero();
+ }
+
+ virtual ~StateFeedbackPlant() {}
+
+ // If U is outside the hardware range, limit it before the plant tries to use
+ // it.
+ virtual void CapU() {
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U[i] > U_max[i]) {
+ U[i] = U_max[i];
+ } else if (U[i] < U_min[i]) {
+ U[i] = U_min[i];
+ }
+ }
+ }
+ // Computes the new X and Y given the control input.
+ void Update() {
+ CapU();
+ X = A * X + B * U;
+ Y = C * X + D * U;
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static constexpr int kNumStates = number_of_states;
+ static constexpr int kNumOutputs = number_of_outputs;
+ static constexpr int kNumInputs = number_of_inputs;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackLoop {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+
+ Eigen::Matrix<double, number_of_states, 1> X_hat;
+ Eigen::Matrix<double, number_of_states, 1> R;
+ Eigen::Matrix<double, number_of_inputs, 1> U;
+ Eigen::Matrix<double, number_of_outputs, 1> U_ff;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> plant;
+
+ StateFeedbackLoop(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : L(L),
+ K(K),
+ plant(plant) {
+ X_hat.setZero();
+ R.setZero();
+ U.setZero();
+ U_ff.setZero();
+ Y.setZero();
+ }
+ virtual ~StateFeedbackLoop() {}
+
+ virtual void FeedForward() {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ U_ff[i] = 0.0;
+ }
+ }
+
+ // If U is outside the hardware range, limit it before the plant tries to use
+ // it.
+ virtual void CapU() {
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U[i] > plant.U_max[i]) {
+ U[i] = plant.U_max[i];
+ } else if (U[i] < plant.U_min[i]) {
+ U[i] = plant.U_min[i];
+ }
+ }
+ }
+
+ // update_observer is whether or not to use the values in Y.
+ // stop_motors is whether or not to output all 0s.
+ void Update(bool update_observer, bool stop_motors) {
+ if (stop_motors) {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ U[i] = 0.0;
+ }
+ } else {
+ U.noalias() = K * (R - X_hat);
+ CapU();
+ }
+
+ if (update_observer) {
+ X_hat = (plant.A - L * plant.C) * X_hat + L * Y + plant.B * U;
+ } else {
+ X_hat = plant.A * X_hat + plant.B * U;
+ }
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static constexpr int kNumStates = number_of_states;
+ static constexpr int kNumOutputs = number_of_outputs;
+ static constexpr int kNumInputs = number_of_inputs;
+};
+
+#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
diff --git a/frc971/control_loops/wrist.cc b/frc971/control_loops/wrist.cc
new file mode 100644
index 0000000..9fa0077
--- /dev/null
+++ b/frc971/control_loops/wrist.cc
@@ -0,0 +1,190 @@
+#include "frc971/control_loops/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_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+static const double MIN_POS = -0.80;
+static const double MAX_POS = 1.77;
+static const double positive_deadband_power = 0.15 * 12.0;
+static const double negative_deadband_power = 0.09 * 12.0;
+// Final units is radians/iteration.
+static const double MAX_SPEED = 19300.0/*free RPM*/ * 12.0/*pinions*/ / 60.0 * 14.0 / 50.0 * 14.0 / 48.0 / 60.0/*60 sec/min*/ / 100.0/*100 cycles/sec*/ * (2.0 * M_PI)/*rotations to radians*/;
+// The minimum magnitude for X_hat[1] to be sure of the direction it's moving so
+// that the code can get a zero. X_hat[1] is the angular velocity of the wrist
+// predicted by the observer, and it can be wrong when the wrist is just
+// switching direction and/or it is moving very slowly in/around the victor
+// deadband. Using this threshold instead of just 0 helps avoid falsely zeroing
+// on the back edge of the magnet instead of the front one.
+static const double kX_hatThreshold = 0.2;
+// The maximum amount that the calibration value can be off for it to be
+// accepted.
+// MAX_SPEED / 10 is the amount it would be off if it was read 1ms earlier than
+// the actual encoder position, which is a lot more than it should ever be.
+static const double kMaxCalibrationError = MAX_SPEED / 10;
+
+WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
+ : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
+ loop_(new StateFeedbackLoop<2, 1, 1>(MakeWristLoop())),
+ stop_(false),
+ error_count_(0),
+ zeroed_(false),
+ zero_offset_(0.0) {
+}
+
+// down is positive
+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*/) {
+
+ if (output) {
+ output->voltage = 0;
+ }
+
+ if (stop_) {
+ LOG(WARNING, "have already given up\n");
+ return;
+ }
+
+ if (position == NULL) {
+ LOG(WARNING, "no new pos given\n");
+ error_count_++;
+ } else {
+ error_count_ = 0;
+ }
+
+ double horizontal;
+ if (!constants::horizontal_offset(&horizontal)) {
+ LOG(ERROR, "Failed to fetch the horizontal offset constant.\n");
+ return;
+ }
+
+ static bool good_zeroed = false;
+ static bool first = true;
+ if (error_count_ >= 4) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ first = true;
+ zeroed_ = false;
+ good_zeroed = false;
+ }
+
+ const double limited_goal = std::min(MAX_POS, std::max(MIN_POS, goal->goal));
+
+ static double zero_goal;
+ static bool old_cal = false;
+ static double old_pos = -5000000;
+ bool bad_encoder = false;
+ if (!first) {
+ if (old_cal != position->hall_effect) {
+ LOG(INFO, "old=%s X_hat=%f\n", old_cal ? "true" : "false", loop_->X_hat[1]);
+ // Don't want to miss the first one if it starts really close to the edge
+ // of the magnet so isn't moving very fast when it passes.
+ const double X_hatThreshold = zeroed_ ? kX_hatThreshold : 0;
+ if ((old_cal && loop_->X_hat[1] < -X_hatThreshold) ||
+ (!old_cal && loop_->X_hat[1] > X_hatThreshold)) {
+ LOG(INFO, "zeroing at %f (pos=%f, old_pos=%f) (used to be %f)\n",
+ position->calibration, position->pos, old_pos, zero_offset_);
+ // If ((old_pos - 0.02) <= position->calibration <= position->pos) or
+ // the other way around. Checks if position->calibration is between
+ // old_pos and position->pos if the wrist is moving either way, with a
+ // slight tweak to
+ // old_pos in case it was triggered while the sensor packet was being
+ // sent or something.
+ if (!((old_pos - kMaxCalibrationError <= position->calibration &&
+ position->calibration <= position->pos) ||
+ (old_pos + kMaxCalibrationError >= position->calibration &&
+ position->calibration >= position->pos))) {
+ if (!good_zeroed) {
+ LOG(WARNING, "using encoder pos because "
+ "calibration sensor pos doesn't make sense\n");
+ zero_offset_ = position->pos;
+ } else {
+ LOG(INFO, "already had a good zero, "
+ "so not using inaccurate zero value\n");
+ }
+ } else {
+ good_zeroed = true;
+ zero_offset_ = position->calibration;
+ }
+ zeroed_ = true;
+ } else {
+ LOG(INFO, "hit back edge at %f\n", position->calibration);
+ }
+ old_cal = position->hall_effect;
+ }
+ if (std::abs(position->pos - old_pos) > MAX_SPEED * 1.5) {
+ bad_encoder = true;
+ LOG(WARNING, "encoder value changed by %f which is more than MAX_SPEED(%f)\n",
+ std::abs(position->pos - old_pos), MAX_SPEED);
+ }
+ } // !first
+
+ old_pos = position->pos;
+ const double absolute_position = position->pos - zero_offset_ - horizontal;
+ if (first) {
+ first = false;
+ old_cal = position->hall_effect;
+ zero_goal = absolute_position;
+ }
+
+ loop_->Y << absolute_position;
+ if (!zeroed_) {
+ loop_->R << zero_goal, 0.0;
+ if (aos::robot_state->enabled) {
+ if (!position->hall_effect) {
+ zero_goal += 0.010;
+ } else {
+ zero_goal -= 0.010;
+ }
+ }
+ } else {
+ loop_->R << limited_goal, 0.0;
+ }
+ loop_->Update(!bad_encoder, bad_encoder || output == NULL);
+ double output_voltage = loop_->U[0];
+ //LOG(DEBUG, "fancy math gave %f\n", status->pwm);
+
+ if (output_voltage > 0) {
+ output_voltage += positive_deadband_power;
+ }
+ if (output_voltage < 0) {
+ output_voltage -= negative_deadband_power;
+ }
+
+ if (zeroed_) {
+ if (absolute_position >= MAX_POS) {
+ output_voltage = std::min(0.0, output_voltage);
+ }
+ if (absolute_position <= MIN_POS) {
+ output_voltage = std::max(0.0, output_voltage);
+ }
+ }
+
+ double limit = zeroed_ ? 1.0 : 0.5;
+ //limit = std::min(0.3, limit);
+ output_voltage = std::min(limit, output_voltage);
+ output_voltage = std::max(-limit, output_voltage);
+ LOG(DEBUG, "pos=%f zero=%f horizontal=%f currently %f hall: %s\n",
+ position->pos, zero_offset_, horizontal, absolute_position,
+ position->hall_effect ? "true" : "false");
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist.h b/frc971/control_loops/wrist.h
new file mode 100644
index 0000000..728b46f
--- /dev/null
+++ b/frc971/control_loops/wrist.h
@@ -0,0 +1,38 @@
+#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_motor.q.h"
+#include "frc971/control_loops/wrist_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+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:
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+ bool stop_;
+ int error_count_;
+ bool zeroed_;
+ double zero_offset_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/wrist_lib_test.cc b/frc971/control_loops/wrist_lib_test.cc
new file mode 100644
index 0000000..4a7c4b5
--- /dev/null
+++ b/frc971/control_loops/wrist_lib_test.cc
@@ -0,0 +1,69 @@
+#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_motor.q.h"
+#include "frc971/control_loops/wrist.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+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_;
+
+ WristMotor wrist_motor_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_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_plant_(new StateFeedbackPlant<2, 1, 1>(MakeWristPlant())) {
+ // Flush the robot state queue so we can recreate shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ ::aos::robot_state.MakeWithBuilder().enabled(true)
+ .autonomous(false)
+ .team_id(971).Send();
+ }
+};
+
+
+// Tests that we can send a message to another thread and it blocking receives
+// it at the correct time.
+TEST_F(WristTest, Stable) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.0).Send();
+ for (int i = 0; i < 1000; ++i) {
+ my_wrist_loop_.position.MakeWithBuilder()
+ .pos(wrist_plant_->Y(0, 0))
+ .hall_effect(false)
+ .calibration(0.0).Send();
+
+ wrist_motor_.Iterate();
+
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ wrist_plant_->U << my_wrist_loop_.output->voltage;
+ wrist_plant_->Update();
+ }
+ my_wrist_loop_.position.FetchLatest();
+ EXPECT_FLOAT_EQ(0.0, my_wrist_loop_.position->pos);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist_main.cc b/frc971/control_loops/wrist_main.cc
new file mode 100644
index 0000000..42006bc
--- /dev/null
+++ b/frc971/control_loops/wrist_main.cc
@@ -0,0 +1,5 @@
+#include "frc971/control_loops/wrist.h"
+
+#include "aos/aos_core.h"
+
+AOS_RUN_LOOP(frc971::control_loops::WristMotor);
diff --git a/frc971/control_loops/wrist_motor.q b/frc971/control_loops/wrist_motor.q
new file mode 100644
index 0000000..3dbeb53
--- /dev/null
+++ b/frc971/control_loops/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_motor_plant.cc b/frc971/control_loops/wrist_motor_plant.cc
new file mode 100644
index 0000000..872e875
--- /dev/null
+++ b/frc971/control_loops/wrist_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/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.00688850240086, 0.0, 0.450098411557;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00106724827203, 0.188617057012;
+ 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.35009841156, 23.2478308944;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.12069897779, -2.23821168376;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeWristPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/wrist_motor_plant.h b/frc971/control_loops/wrist_motor_plant.h
new file mode 100644
index 0000000..9595b5f
--- /dev/null
+++ b/frc971/control_loops/wrist_motor_plant.h
@@ -0,0 +1,15 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_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_MOTOR_PLANT_H_