Wrist loop test case now runs and fails.
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