Copy back a lot of the 2014 code.
Change-Id: I552292d8bd7bce4409e02d254bef06a9cc009568
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
new file mode 100644
index 0000000..b2ac562
--- /dev/null
+++ b/y2014/control_loops/claw/claw.cc
@@ -0,0 +1,994 @@
+#include "y2014/control_loops/claw/claw.h"
+
+#include <algorithm>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/commonmath.h"
+
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/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 {
+
+static const double kZeroingVoltage = 4.0;
+static const double kMaxVoltage = 12.0;
+const double kRezeroThreshold = 0.07;
+
+ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> &&loop)
+ : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
+ uncapped_average_voltage_(0.0),
+ is_zeroing_(true),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
+ kMaxVoltage, kMaxVoltage).finished()),
+ U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() <<
+ kZeroingVoltage, kZeroingVoltage,
+ kZeroingVoltage, kZeroingVoltage).finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+}
+
+// Caps the voltage prioritizing reducing velocity error over reducing
+// positional error.
+// Uses the polytope libararies which we used to just use for the drivetrain.
+// Uses a region representing the maximum voltage and then transforms it such
+// that the points represent different amounts of positional error and
+// constrains the region such that, if at all possible, it will maintain its
+// current efforts to reduce velocity error.
+void ClawLimitedLoop::CapU() {
+ const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
+
+ double u_top = U(1, 0);
+ double u_bottom = U(0, 0);
+
+ uncapped_average_voltage_ = (u_top + u_bottom) / 2;
+
+ double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
+
+ if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
+ LOG_MATRIX(DEBUG, "U at start", U());
+ // H * U <= k
+ // U = UPos + UVel
+ // H * (UPos + UVel) <= k
+ // H * UPos <= k - H * UVel
+
+ // Now, we can do a coordinate transformation and say the following.
+
+ // UPos = position_K * position_error
+ // (H * position_K) * position_error <= k - H * UVel
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 1),
+ K(1, 0), K(1, 1);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 2), K(0, 3),
+ K(1, 2), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(1, 0);
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(2, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
+
+ const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = position_error;
+
+ // This line was at 45 degrees but is now at some angle steeper than the
+ // straight one between the points.
+ Eigen::Matrix<double, 1, 2> angle_45;
+ // If the top claw is above its soft upper limit, make the line actually
+ // 45 degrees to avoid smashing it into the limit in an attempt to fix the
+ // separation error faster than the bottom position one.
+ if (X_hat(0, 0) + X_hat(1, 0) >
+ constants::GetValues().claw.upper_claw.upper_limit) {
+ angle_45 << 1, 1;
+ } else {
+ // Fixing separation error half as fast as positional error works well
+ // because it means they both close evenly.
+ angle_45 << ::std::sqrt(3), 1;
+ }
+ Eigen::Matrix<double, 1, 2> L45_quadrant;
+ L45_quadrant << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+ const auto L45 = L45_quadrant.cwiseProduct(angle_45);
+ const double w45 = 0;
+
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
+
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, position_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ mutable_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
+
+ {
+ const auto values = constants::GetValues().claw;
+ if (top_known_) {
+ if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit && U(1, 0) > 0) {
+ LOG(WARNING, "upper claw too high and moving up\n");
+ mutable_U(1, 0) = 0;
+ } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
+ U(1, 0) < 0) {
+ LOG(WARNING, "upper claw too low and moving down\n");
+ mutable_U(1, 0) = 0;
+ }
+ }
+ if (bottom_known_) {
+ if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
+ LOG(WARNING, "lower claw too high and moving up\n");
+ mutable_U(0, 0) = 0;
+ } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
+ LOG(WARNING, "lower claw too low and moving down\n");
+ mutable_U(0, 0) = 0;
+ }
+ }
+ }
+ }
+}
+
+ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
+ ClawMotor *motor)
+ : offset_(0.0),
+ name_(name),
+ motor_(motor),
+ zeroing_state_(UNKNOWN_POSITION),
+ posedge_value_(0.0),
+ negedge_value_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0) {}
+
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition &claw) {
+ front_.Update(claw.front);
+ calibration_.Update(claw.calibration);
+ back_.Update(claw.back);
+
+ bool any_sensor_triggered = any_triggered();
+ if (any_sensor_triggered && any_triggered_last_) {
+ // We are still on the hall effect and nothing has changed.
+ min_hall_effect_on_angle_ =
+ ::std::min(min_hall_effect_on_angle_, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(max_hall_effect_on_angle_, claw.position);
+ } else if (!any_sensor_triggered && !any_triggered_last_) {
+ // We are still off the hall effect and nothing has changed.
+ min_hall_effect_off_angle_ =
+ ::std::min(min_hall_effect_off_angle_, claw.position);
+ max_hall_effect_off_angle_ =
+ ::std::max(max_hall_effect_off_angle_, claw.position);
+ } else if (any_sensor_triggered && !any_triggered_last_) {
+ // Saw a posedge on the hall effect. Reset the limits.
+ min_hall_effect_on_angle_ = ::std::min(claw.posedge_value, claw.position);
+ max_hall_effect_on_angle_ = ::std::max(claw.posedge_value, claw.position);
+ } else if (!any_sensor_triggered && any_triggered_last_) {
+ // Saw a negedge on the hall effect. Reset the limits.
+ min_hall_effect_off_angle_ = ::std::min(claw.negedge_value, claw.position);
+ max_hall_effect_off_angle_ = ::std::max(claw.negedge_value, claw.position);
+ }
+
+ posedge_value_ = claw.posedge_value;
+ negedge_value_ = claw.negedge_value;
+ last_encoder_ = encoder_;
+ if (front().value() || calibration().value() || back().value()) {
+ last_on_encoder_ = encoder_;
+ } else {
+ last_off_encoder_ = encoder_;
+ }
+ encoder_ = claw.position;
+ any_triggered_last_ = any_sensor_triggered;
+}
+
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
+ set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+
+ front_.Reset(claw.front);
+ calibration_.Reset(claw.calibration);
+ back_.Reset(claw.back);
+ // close up the min and max edge positions as they are no longer valid and
+ // will be expanded in future iterations
+ min_hall_effect_on_angle_ = claw.position;
+ max_hall_effect_on_angle_ = claw.position;
+ min_hall_effect_off_angle_ = claw.position;
+ max_hall_effect_off_angle_ = claw.position;
+ any_triggered_last_ = any_triggered();
+}
+
+bool TopZeroedStateFeedbackLoop::SetCalibrationOnEdge(
+ const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+}
+
+void TopZeroedStateFeedbackLoop::HandleCalibrationError(
+ const constants::Values::Claws::Claw &claw_values) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ const double calibration_error =
+ ComputeCalibrationChange(edge_encoder, edge_angle);
+ LOG(INFO, "Top calibration error is %f\n", calibration_error);
+ if (::std::abs(calibration_error) > kRezeroThreshold) {
+ LOG(WARNING, "rezeroing top\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ }
+ }
+}
+
+
+void BottomZeroedStateFeedbackLoop::HandleCalibrationError(
+ const constants::Values::Claws::Claw &claw_values) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ const double calibration_error =
+ ComputeCalibrationChange(edge_encoder, edge_angle);
+ LOG(INFO, "Bottom calibration error is %f\n", calibration_error);
+ if (::std::abs(calibration_error) > kRezeroThreshold) {
+ LOG(WARNING, "rezeroing bottom\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ }
+ }
+}
+
+bool BottomZeroedStateFeedbackLoop::SetCalibrationOnEdge(
+ const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+}
+
+ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
+ : aos::controls::ControlLoop<control_loops::ClawGroup>(my_claw),
+ has_top_claw_goal_(false),
+ top_claw_goal_(0.0),
+ top_claw_(this),
+ has_bottom_claw_goal_(false),
+ bottom_claw_goal_(0.0),
+ bottom_claw_(this),
+ claw_(MakeClawLoop()),
+ was_enabled_(false),
+ doing_calibration_fine_tune_(false),
+ capped_goal_(false),
+ mode_(UNKNOWN_LOCATION) {}
+
+const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+
+bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
+ this_sensor.value() && !this_sensor.last_value()) {
+ posedge_filter_ = &this_sensor;
+ } else if (posedge_filter_ == &this_sensor &&
+ !this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() &&
+ !sensorB.posedge_count_changed() && this_sensor.value()) {
+ posedge_filter_ = nullptr;
+ return true;
+ } else if (posedge_filter_ == &this_sensor) {
+ posedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
+ !this_sensor.value() && this_sensor.last_value()) {
+ negedge_filter_ = &this_sensor;
+ } else if (negedge_filter_ == &this_sensor &&
+ !this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() &&
+ !sensorB.negedge_count_changed() && !this_sensor.value()) {
+ negedge_filter_ = nullptr;
+ return true;
+ } else if (negedge_filter_ == &this_sensor) {
+ negedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
+ const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
+ double *edge_angle, const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
+ const char *hall_effect_name) {
+ bool found_edge = false;
+
+ if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
+ LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
+ } else {
+ const double average_last_encoder =
+ (min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
+ if (posedge_value_ < average_last_encoder) {
+ *edge_angle = angles.upper_decreasing_angle;
+ LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, posedge_value_,
+ average_last_encoder);
+ } else {
+ *edge_angle = angles.lower_angle;
+ LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, posedge_value_,
+ average_last_encoder);
+ }
+ *edge_encoder = posedge_value_;
+ found_edge = true;
+ }
+ }
+
+ if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
+ } else {
+ const double average_last_encoder =
+ (min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
+ if (negedge_value_ > average_last_encoder) {
+ *edge_angle = angles.upper_angle;
+ LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, negedge_value_,
+ average_last_encoder);
+ } else {
+ *edge_angle = angles.lower_decreasing_angle;
+ LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle, negedge_value_,
+ average_last_encoder);
+ }
+ *edge_encoder = negedge_value_;
+ found_edge = true;
+ }
+ }
+
+ return found_edge;
+}
+
+bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
+ const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
+ double *edge_angle) {
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
+ calibration_, back_, "front")) {
+ return true;
+ }
+ if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
+ calibration_, front_, back_, "calibration")) {
+ return true;
+ }
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
+ calibration_, front_, "back")) {
+ return true;
+ }
+ return false;
+}
+
+void TopZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
+ double edge_angle) {
+ double old_offset = offset_;
+ offset_ = edge_angle - edge_encoder;
+ const double doffset = offset_ - old_offset;
+ motor_->ChangeTopOffset(doffset);
+}
+
+double TopZeroedStateFeedbackLoop::ComputeCalibrationChange(double edge_encoder,
+ double edge_angle) {
+ const double offset = edge_angle - edge_encoder;
+ const double doffset = offset - offset_;
+ return doffset;
+}
+
+void BottomZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
+ double edge_angle) {
+ double old_offset = offset_;
+ offset_ = edge_angle - edge_encoder;
+ const double doffset = offset_ - old_offset;
+ motor_->ChangeBottomOffset(doffset);
+}
+
+double BottomZeroedStateFeedbackLoop::ComputeCalibrationChange(
+ double edge_encoder, double edge_angle) {
+ const double offset = edge_angle - edge_encoder;
+ const double doffset = offset - offset_;
+ return doffset;
+}
+
+void ClawMotor::ChangeTopOffset(double doffset) {
+ claw_.ChangeTopOffset(doffset);
+ if (has_top_claw_goal_) {
+ top_claw_goal_ += doffset;
+ }
+}
+
+void ClawMotor::ChangeBottomOffset(double doffset) {
+ claw_.ChangeBottomOffset(doffset);
+ if (has_bottom_claw_goal_) {
+ bottom_claw_goal_ += doffset;
+ }
+}
+
+void ClawLimitedLoop::ChangeTopOffset(double doffset) {
+ mutable_Y()(1, 0) += doffset;
+ mutable_X_hat()(1, 0) += doffset;
+ LOG(INFO, "Changing top offset by %f\n", doffset);
+}
+void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
+ mutable_Y()(0, 0) += doffset;
+ mutable_X_hat()(0, 0) += doffset;
+ mutable_X_hat()(1, 0) -= doffset;
+ LOG(INFO, "Changing bottom offset by %f\n", doffset);
+}
+
+void LimitClawGoal(double *bottom_goal, double *top_goal,
+ const frc971::constants::Values &values) {
+ // first update position based on angle limit
+ const double separation = *top_goal - *bottom_goal;
+ if (separation > values.claw.soft_max_separation) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
+ *bottom_goal += dsep;
+ *top_goal -= dsep;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+ if (separation < values.claw.soft_min_separation) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
+ *bottom_goal += dsep;
+ *top_goal -= dsep;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+
+ // now move both goals in unison
+ if (*bottom_goal < values.claw.lower_claw.lower_limit) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ *top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
+ *bottom_goal = values.claw.lower_claw.lower_limit;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+ if (*bottom_goal > values.claw.lower_claw.upper_limit) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ *top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
+ *bottom_goal = values.claw.lower_claw.upper_limit;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+
+ if (*top_goal < values.claw.upper_claw.lower_limit) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ *bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
+ *top_goal = values.claw.upper_claw.lower_limit;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+ if (*top_goal > values.claw.upper_claw.upper_limit) {
+ LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ *bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
+ *top_goal = values.claw.upper_claw.upper_limit;
+ LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ }
+}
+
+bool ClawMotor::is_ready() const {
+ return (
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (((::aos::joystick_state.get() == NULL)
+ ? true
+ : ::aos::joystick_state->autonomous) &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))));
+}
+
+bool ClawMotor::is_zeroing() const { return !is_ready(); }
+
+// Positive angle is up, and positive power is up.
+void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
+ const control_loops::ClawGroup::Position *position,
+ control_loops::ClawGroup::Output *output,
+ control_loops::ClawGroup::Status *status) {
+ constexpr double dt = 0.01;
+
+ // 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;
+ output->tusk_voltage = 0;
+ }
+
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
+ }
+
+ if (WasReset()) {
+ top_claw_.Reset(position->top);
+ bottom_claw_.Reset(position->bottom);
+ }
+
+ const frc971::constants::Values &values = constants::GetValues();
+
+ if (position) {
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << position->bottom.position + bottom_claw_.offset(),
+ position->top.position + top_claw_.offset();
+ claw_.Correct(Y);
+
+ top_claw_.SetPositionValues(position->top);
+ bottom_claw_.SetPositionValues(position->bottom);
+
+ if (!has_top_claw_goal_) {
+ has_top_claw_goal_ = true;
+ top_claw_goal_ = top_claw_.absolute_position();
+ initial_separation_ =
+ top_claw_.absolute_position() - bottom_claw_.absolute_position();
+ }
+ if (!has_bottom_claw_goal_) {
+ has_bottom_claw_goal_ = true;
+ bottom_claw_goal_ = bottom_claw_.absolute_position();
+ initial_separation_ =
+ top_claw_.absolute_position() - bottom_claw_.absolute_position();
+ }
+ LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
+ }
+
+ bool autonomous, enabled;
+ if (::aos::joystick_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::joystick_state->autonomous;
+ enabled = ::aos::joystick_state->enabled;
+ }
+
+ double bottom_claw_velocity_ = 0.0;
+ double top_claw_velocity_ = 0.0;
+
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
+ // Ready to use the claw.
+ // Limit the goals here.
+ bottom_claw_goal_ = goal->bottom_angle;
+ top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
+ has_bottom_claw_goal_ = true;
+ has_top_claw_goal_ = true;
+ doing_calibration_fine_tune_ = false;
+ mode_ = READY;
+
+ bottom_claw_.HandleCalibrationError(values.claw.lower_claw);
+ top_claw_.HandleCalibrationError(values.claw.upper_claw);
+ } else if (top_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ // Time to fine tune the zero.
+ // Limit the goals here.
+ if (!enabled) {
+ // If we are disabled, start the fine tune process over again.
+ doing_calibration_fine_tune_ = false;
+ }
+ if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
+ // always get the bottom claw to calibrated first
+ LOG(DEBUG, "Calibrating the bottom of the claw\n");
+ if (!doing_calibration_fine_tune_) {
+ if (::std::abs(bottom_absolute_position() -
+ values.claw.start_fine_tune_pos) <
+ values.claw.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
+ LOG(DEBUG, "Ready to fine tune the bottom\n");
+ mode_ = FINE_TUNE_BOTTOM;
+ } else {
+ // send bottom to zeroing start
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ LOG(DEBUG, "Going to the start position for the bottom\n");
+ mode_ = PREP_FINE_TUNE_BOTTOM;
+ }
+ } else {
+ mode_ = FINE_TUNE_BOTTOM;
+ bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
+ if (top_claw_.front_or_back_triggered() ||
+ bottom_claw_.front_or_back_triggered()) {
+ // We shouldn't hit a limit, but if we do, go back to the zeroing
+ // point and try again.
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ LOG(DEBUG, "Found a limit, starting over.\n");
+ mode_ = PREP_FINE_TUNE_BOTTOM;
+ }
+
+ if (position && bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(), bottom_claw_.front(),
+ bottom_claw_.back())) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.claw.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else if (bottom_claw_.calibration().last_value()) {
+ LOG(DEBUG, "Aborting bottom fine tune because sensor triggered\n");
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_.set_zeroing_state(
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ } else {
+ LOG(DEBUG, "Fine tuning\n");
+ }
+ }
+ // now set the top claw to track
+
+ top_claw_goal_ = bottom_claw_goal_ + values.claw.claw_zeroing_separation;
+ } else {
+ // bottom claw must be calibrated, start on the top
+ if (!doing_calibration_fine_tune_) {
+ if (::std::abs(top_absolute_position() -
+ values.claw.start_fine_tune_pos) <
+ values.claw.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
+ LOG(DEBUG, "Ready to fine tune the top\n");
+ mode_ = FINE_TUNE_TOP;
+ } else {
+ // send top to zeroing start
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ LOG(DEBUG, "Going to the start position for the top\n");
+ mode_ = PREP_FINE_TUNE_TOP;
+ }
+ } else {
+ mode_ = FINE_TUNE_TOP;
+ top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
+ if (top_claw_.front_or_back_triggered() ||
+ bottom_claw_.front_or_back_triggered()) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ LOG(DEBUG, "Found a limit, starting over.\n");
+ mode_ = PREP_FINE_TUNE_TOP;
+ }
+
+ if (position &&
+ top_claw_.SawFilteredPosedge(top_claw_.calibration(),
+ top_claw_.front(), top_claw_.back())) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ } else if (top_claw_.calibration().last_value()) {
+ LOG(DEBUG, "Aborting top fine tune because sensor triggered\n");
+ doing_calibration_fine_tune_ = false;
+ top_claw_.set_zeroing_state(
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ }
+ }
+ // now set the bottom claw to track
+ bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
+ }
+ } else {
+ doing_calibration_fine_tune_ = false;
+ if (!was_enabled_ && enabled) {
+ if (position) {
+ top_claw_goal_ = position->top.position + top_claw_.offset();
+ bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
+ initial_separation_ =
+ position->top.position - position->bottom.position;
+ } else {
+ has_top_claw_goal_ = false;
+ has_bottom_claw_goal_ = false;
+ }
+ }
+
+ if ((bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
+ bottom_claw_.front().value() || top_claw_.front().value()) &&
+ !top_claw_.back().value() && !bottom_claw_.back().value()) {
+ if (enabled) {
+ // Time to slowly move back up to find any position to narrow down the
+ // zero.
+ top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_off_speed;
+ LOG(DEBUG, "Bottom is known.\n");
+ }
+ } else {
+ // We don't know where either claw is. Slowly start moving down to find
+ // any hall effect.
+ if (enabled) {
+ top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ -values.claw.claw_zeroing_off_speed;
+ LOG(DEBUG, "Both are unknown.\n");
+ }
+ }
+
+ if (position) {
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo
+ // low.
+ // :)
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
+ }
+ mode_ = UNKNOWN_LOCATION;
+ }
+
+ // Limit the goals if both claws have been (mostly) found.
+ if (mode_ != UNKNOWN_LOCATION) {
+ LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
+ }
+
+ claw_.set_positions_known(
+ top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION,
+ bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ if (has_top_claw_goal_ && has_bottom_claw_goal_) {
+ claw_.mutable_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
+ bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
+ LOG_MATRIX(DEBUG, "actual goal", claw_.R());
+
+ // Only cap power when one of the halves of the claw is moving slowly and
+ // could wind up.
+ claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
+ mode_ == FINE_TUNE_BOTTOM);
+ claw_.Update(output == nullptr);
+ } else {
+ claw_.Update(true);
+ }
+
+ capped_goal_ = false;
+ switch (mode_) {
+ case READY:
+ case PREP_FINE_TUNE_TOP:
+ case PREP_FINE_TUNE_BOTTOM:
+ break;
+ case FINE_TUNE_BOTTOM:
+ case FINE_TUNE_TOP:
+ case UNKNOWN_LOCATION: {
+ if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
+ double dx_bot = (claw_.U_uncapped(0, 0) -
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx_top = (claw_.U_uncapped(1, 0) -
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::max(dx_top, dx_bot);
+ bottom_claw_goal_ -= dx;
+ top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
+ claw_.R(3, 0);
+ claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ capped_goal_ = true;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx,
+ claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
+ (claw_.uncapped_average_voltage() -
+ values.claw.max_zeroing_voltage));
+ } else if (claw_.uncapped_average_voltage() <
+ -values.claw.max_zeroing_voltage) {
+ double dx_bot = (claw_.U_uncapped(0, 0) +
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx_top = (claw_.U_uncapped(1, 0) +
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::min(dx_top, dx_bot);
+ bottom_claw_goal_ -= dx;
+ top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
+ claw_.R(3, 0);
+ claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ capped_goal_ = true;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ }
+ } break;
+ }
+
+ if (output) {
+ if (goal) {
+ //setup the intake
+ output->intake_voltage =
+ (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
+ : goal->intake;
+ output->tusk_voltage = goal->centering;
+ output->tusk_voltage =
+ (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ ? -12.0
+ : goal->centering;
+ }
+ output->top_claw_voltage = claw_.U(1, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
+
+ if (output->top_claw_voltage > kMaxVoltage) {
+ output->top_claw_voltage = kMaxVoltage;
+ } else if (output->top_claw_voltage < -kMaxVoltage) {
+ output->top_claw_voltage = -kMaxVoltage;
+ }
+
+ if (output->bottom_claw_voltage > kMaxVoltage) {
+ output->bottom_claw_voltage = kMaxVoltage;
+ } else if (output->bottom_claw_voltage < -kMaxVoltage) {
+ output->bottom_claw_voltage = -kMaxVoltage;
+ }
+ }
+
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
+
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
+
+ status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+
+ was_enabled_ = enabled;
+}
+
+} // namespace control_loops
+} // namespace frc971
+
diff --git a/y2014/control_loops/claw/claw.gyp b/y2014/control_loops/claw/claw.gyp
new file mode 100644
index 0000000..40315ff
--- /dev/null
+++ b/y2014/control_loops/claw/claw.gyp
@@ -0,0 +1,99 @@
+{
+ '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_loop',
+ 'type': 'static_library',
+ 'sources': ['claw.q'],
+ 'variables': {
+ 'header_path': 'y2014/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_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/y2014/y2014.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:polytope',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
+ ],
+ 'export_dependent_settings': [
+ 'claw_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:polytope',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ ],
+ },
+ {
+ 'target_name': 'claw_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'claw_loop',
+ 'claw_lib',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ ],
+ },
+ {
+ 'target_name': 'claw_calibration',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_calibration.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/y2014/y2014.gyp:constants',
+ ],
+ },
+ {
+ 'target_name': 'claw',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_lib',
+ ],
+ },
+ ],
+}
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
new file mode 100644
index 0000000..c04a8fe
--- /dev/null
+++ b/y2014/control_loops/claw/claw.h
@@ -0,0 +1,267 @@
+#ifndef Y2014_CONTROL_LOOPS_CLAW_CLAW_H_
+#define Y2014_CONTROL_LOOPS_CLAW_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/controls/polytope.h"
+#include "y2014/constants.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "frc971/control_loops/hall_effect_tracker.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class WindupClawTest;
+};
+
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
+class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> &&loop);
+ virtual void CapU();
+
+ void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
+ void set_positions_known(bool top_known, bool bottom_known) {
+ top_known_ = top_known;
+ bottom_known_ = bottom_known;
+ }
+
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+
+ double uncapped_average_voltage() const { return uncapped_average_voltage_; }
+
+ private:
+ double uncapped_average_voltage_;
+ bool is_zeroing_;
+
+ bool top_known_ = false, bottom_known_ = false;
+
+ const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
+};
+
+class ClawMotor;
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+// It does not have any zeroing logic in it, only logic to deal with a delta U
+// controller.
+class ZeroedStateFeedbackLoop {
+ public:
+ ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor);
+
+ const static int kZeroingMaxVoltage = 5;
+
+ enum JointZeroingState {
+ // We don't know where the joint is at all.
+ UNKNOWN_POSITION,
+ // We have an approximate position for where the claw is using.
+ APPROXIMATE_CALIBRATION,
+ // We observed the calibration edge while disabled. This is good enough for
+ // autonomous mode.
+ DISABLED_CALIBRATION,
+ // Ready for use during teleop.
+ CALIBRATED
+ };
+
+ void set_zeroing_state(JointZeroingState zeroing_state) {
+ zeroing_state_ = zeroing_state;
+ }
+ JointZeroingState zeroing_state() const { return zeroing_state_; }
+
+ void SetPositionValues(const HalfClawPosition &claw);
+
+ void Reset(const HalfClawPosition &claw);
+
+ double absolute_position() const { return encoder() + offset(); }
+
+ const HallEffectTracker &front() const { return front_; }
+ const HallEffectTracker &calibration() const { return calibration_; }
+ const HallEffectTracker &back() const { return back_; }
+
+ bool any_hall_effect_changed() const {
+ return front().either_count_changed() ||
+ calibration().either_count_changed() ||
+ back().either_count_changed();
+ }
+ bool front_or_back_triggered() const {
+ return front().value() || back().value();
+ }
+ bool any_triggered() const {
+ return calibration().value() || front().value() || back().value();
+ }
+
+ double encoder() const { return encoder_; }
+ double last_encoder() const { return last_encoder_; }
+
+ double offset() const { return offset_; }
+
+ // Returns true if an edge was detected in the last cycle and then sets the
+ // edge_position to the absolute position of the edge.
+ bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
+ double *edge_encoder, double *edge_angle);
+
+ bool SawFilteredPosedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+ bool SawFilteredNegedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+#undef COUNT_SETTER_GETTER
+
+ protected:
+ // The accumulated voltage to apply to the motor.
+ double offset_;
+ const char *name_;
+
+ ClawMotor *motor_;
+
+ HallEffectTracker front_, calibration_, back_;
+
+ JointZeroingState zeroing_state_;
+ double posedge_value_;
+ double negedge_value_;
+ double min_hall_effect_on_angle_;
+ double max_hall_effect_on_angle_;
+ double min_hall_effect_off_angle_;
+ double max_hall_effect_off_angle_;
+ double encoder_;
+ double last_encoder_;
+ double last_on_encoder_;
+ double last_off_encoder_;
+ bool any_triggered_last_;
+
+ const HallEffectTracker* posedge_filter_ = nullptr;
+ const HallEffectTracker* negedge_filter_ = nullptr;
+
+ private:
+ // Does the edges of 1 sensor for GetPositionOfEdge.
+ bool DoGetPositionOfEdge(const constants::Values::Claws::AnglePair &angles,
+ double *edge_encoder, double *edge_angle,
+ const HallEffectTracker &sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB,
+ const char *hall_effect_name);
+};
+
+class TopZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ TopZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("top", motor) {}
+ // Sets the calibration offset given the absolute angle and the corresponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state);
+ double ComputeCalibrationChange(double edge_encoder, double edge_angle);
+ void HandleCalibrationError(
+ const constants::Values::Claws::Claw &claw_values);
+};
+
+class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ BottomZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("bottom", motor) {}
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
+ JointZeroingState zeroing_state);
+ double ComputeCalibrationChange(double edge_encoder, double edge_angle);
+ void HandleCalibrationError(
+ const constants::Values::Claws::Claw &claw_values);
+};
+
+class ClawMotor : public aos::controls::ControlLoop<control_loops::ClawGroup> {
+ public:
+ explicit ClawMotor(control_loops::ClawGroup *my_claw =
+ &control_loops::claw_queue_group);
+
+ // True if the state machine is ready.
+ bool capped_goal() const { return capped_goal_; }
+
+ double uncapped_average_voltage() const {
+ return claw_.uncapped_average_voltage();
+ }
+
+ // True if the claw is zeroing.
+ bool is_zeroing() const;
+
+ // True if the state machine is ready.
+ bool is_ready() const;
+
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+
+ enum CalibrationMode {
+ READY,
+ PREP_FINE_TUNE_TOP,
+ FINE_TUNE_TOP,
+ PREP_FINE_TUNE_BOTTOM,
+ FINE_TUNE_BOTTOM,
+ UNKNOWN_LOCATION
+ };
+
+ CalibrationMode mode() const { return mode_; }
+
+ protected:
+ virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
+ const control_loops::ClawGroup::Position *position,
+ control_loops::ClawGroup::Output *output,
+ control_loops::ClawGroup::Status *status);
+
+ double top_absolute_position() const {
+ return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
+ }
+ double bottom_absolute_position() const { return claw_.X_hat(0, 0); }
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::WindupClawTest;
+
+ // The zeroed joint to use.
+ bool has_top_claw_goal_;
+ double top_claw_goal_;
+ TopZeroedStateFeedbackLoop top_claw_;
+
+ bool has_bottom_claw_goal_;
+ double bottom_claw_goal_;
+ BottomZeroedStateFeedbackLoop bottom_claw_;
+
+ // The claw loop.
+ ClawLimitedLoop claw_;
+
+ bool was_enabled_;
+ bool doing_calibration_fine_tune_;
+
+ // The initial separation when disabled. Used as the safe separation
+ // distance.
+ double initial_separation_;
+
+ bool capped_goal_;
+ CalibrationMode mode_;
+
+ DISALLOW_COPY_AND_ASSIGN(ClawMotor);
+};
+
+// Modifies the bottom and top goal such that they are within the limits and
+// their separation isn't too much or little.
+void LimitClawGoal(double *bottom_goal, double *top_goal,
+ const frc971::constants::Values &values);
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_CLAW_CLAW_H_
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
new file mode 100644
index 0000000..860854b
--- /dev/null
+++ b/y2014/control_loops/claw/claw.q
@@ -0,0 +1,82 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+struct HalfClawPosition {
+ // The current position of this half of the claw.
+ double position;
+
+ // The hall effect sensor at the front limit.
+ HallEffectStruct front;
+ // The hall effect sensor in the middle to use for real calibration.
+ HallEffectStruct calibration;
+ // The hall effect at the back limit.
+ HallEffectStruct back;
+
+ // The encoder value at the last posedge of any of the claw hall effect
+ // sensors (front, calibration, or back).
+ double posedge_value;
+ // The encoder value at the last negedge of any of the claw hall effect
+ // sensors (front, calibration, or back).
+ double negedge_value;
+};
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+queue_group ClawGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The angle of the bottom claw.
+ double bottom_angle;
+ // How much higher the top claw is.
+ double separation_angle;
+ // top claw intake roller
+ double intake;
+ // bottom claw tusk centering
+ double centering;
+ };
+
+ message Position {
+ // All the top claw information.
+ HalfClawPosition top;
+ // All the bottom claw information.
+ HalfClawPosition bottom;
+ };
+
+ message Output {
+ double intake_voltage;
+ double top_claw_voltage;
+ double bottom_claw_voltage;
+ double tusk_voltage;
+ };
+
+ message Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ bool done_with_ball;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ClawGroup claw_queue_group;
+
+struct ClawPositionToLog {
+ double top;
+ double bottom;
+};
diff --git a/y2014/control_loops/claw/claw_calibration.cc b/y2014/control_loops/claw/claw_calibration.cc
new file mode 100644
index 0000000..de91d91
--- /dev/null
+++ b/y2014/control_loops/claw/claw_calibration.cc
@@ -0,0 +1,310 @@
+#include "y2014/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/control_loops.q.h"
+
+#include "aos/linux_code/init.h"
+#include "y2014/constants.h"
+
+namespace frc971 {
+
+typedef constants::Values::Claws Claws;
+
+class Sensor {
+ public:
+ Sensor(const double start_position,
+ const HallEffectStruct &initial_hall_effect)
+ : start_position_(start_position),
+ last_hall_effect_(initial_hall_effect),
+ last_posedge_count_(initial_hall_effect.posedge_count),
+ last_negedge_count_(initial_hall_effect.negedge_count) {
+ last_on_min_position_ = start_position;
+ last_on_max_position_ = start_position;
+ last_off_min_position_ = start_position;
+ last_off_max_position_ = start_position;
+ }
+
+ bool DoGetPositionOfEdge(
+ const control_loops::HalfClawPosition &claw_position,
+ const HallEffectStruct &hall_effect, Claws::AnglePair *limits) {
+ bool print = false;
+
+ if (hall_effect.posedge_count != last_posedge_count_) {
+ const double avg_off_position = (last_off_min_position_ + last_off_max_position_) / 2.0;
+ if (claw_position.posedge_value < avg_off_position) {
+ printf("Posedge upper current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->upper_decreasing_angle = claw_position.posedge_value - start_position_;
+ } else {
+ printf("Posedge lower current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->lower_angle =
+ claw_position.posedge_value - start_position_;
+ }
+ print = true;
+ }
+ if (hall_effect.negedge_count != last_negedge_count_) {
+ const double avg_on_position = (last_on_min_position_ + last_on_max_position_) / 2.0;
+ if (claw_position.negedge_value > avg_on_position) {
+ printf("Negedge upper current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->upper_angle =
+ claw_position.negedge_value - start_position_;
+ } else {
+ printf("Negedge lower current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->lower_decreasing_angle = claw_position.negedge_value - start_position_;
+ }
+ print = true;
+ }
+
+ if (hall_effect.current) {
+ if (!last_hall_effect_.current) {
+ last_on_min_position_ = last_on_max_position_ = claw_position.position;
+ } else {
+ last_on_min_position_ =
+ ::std::min(claw_position.position, last_on_min_position_);
+ last_on_max_position_ =
+ ::std::max(claw_position.position, last_on_max_position_);
+ }
+ } else {
+ if (last_hall_effect_.current) {
+ last_off_min_position_ = last_off_max_position_ = claw_position.position;
+ } else {
+ last_off_min_position_ =
+ ::std::min(claw_position.position, last_off_min_position_);
+ last_off_max_position_ =
+ ::std::max(claw_position.position, last_off_max_position_);
+ }
+ }
+
+ last_hall_effect_ = hall_effect;
+ last_posedge_count_ = hall_effect.posedge_count;
+ last_negedge_count_ = hall_effect.negedge_count;
+
+ return print;
+ }
+
+ private:
+ const double start_position_;
+ HallEffectStruct last_hall_effect_;
+ int32_t last_posedge_count_;
+ int32_t last_negedge_count_;
+ double last_on_min_position_;
+ double last_off_min_position_;
+ double last_on_max_position_;
+ double last_off_max_position_;
+};
+
+class ClawSensors {
+ public:
+ ClawSensors(const double start_position,
+ const control_loops::HalfClawPosition &initial_claw_position)
+ : start_position_(start_position),
+ front_(start_position, initial_claw_position.front),
+ calibration_(start_position, initial_claw_position.calibration),
+ back_(start_position, initial_claw_position.back) {}
+
+ bool GetPositionOfEdge(const control_loops::HalfClawPosition &claw_position,
+ Claws::Claw *claw) {
+
+ bool print = false;
+ if (front_.DoGetPositionOfEdge(claw_position,
+ claw_position.front, &claw->front)) {
+ print = true;
+ } else if (calibration_.DoGetPositionOfEdge(claw_position,
+ claw_position.calibration,
+ &claw->calibration)) {
+ print = true;
+ } else if (back_.DoGetPositionOfEdge(claw_position,
+ claw_position.back, &claw->back)) {
+ print = true;
+ }
+
+ double position = claw_position.position - start_position_;
+
+ if (position > claw->upper_limit) {
+ claw->upper_hard_limit = claw->upper_limit = position;
+ print = true;
+ }
+ if (position < claw->lower_limit) {
+ claw->lower_hard_limit = claw->lower_limit = position;
+ print = true;
+ }
+ return print;
+ }
+
+ private:
+ const double start_position_;
+ Sensor front_;
+ Sensor calibration_;
+ Sensor back_;
+};
+
+int Main() {
+ control_loops::claw_queue_group.position.FetchNextBlocking();
+
+ const double top_start_position =
+ control_loops::claw_queue_group.position->top.position;
+ const double bottom_start_position =
+ control_loops::claw_queue_group.position->bottom.position;
+
+ ClawSensors top(top_start_position,
+ control_loops::claw_queue_group.position->top);
+ ClawSensors bottom(bottom_start_position,
+ control_loops::claw_queue_group.position->bottom);
+
+ Claws limits;
+
+ limits.claw_zeroing_off_speed = 0.5;
+ limits.claw_zeroing_speed = 0.1;
+ limits.claw_zeroing_separation = 0.1;
+
+ // claw separation that would be considered a collision
+ limits.claw_min_separation = 0.0;
+ limits.claw_max_separation = 0.0;
+
+ // We should never get closer/farther than these.
+ limits.soft_min_separation = 0.0;
+ limits.soft_max_separation = 0.0;
+
+ limits.upper_claw.lower_hard_limit = 0.0;
+ limits.upper_claw.upper_hard_limit = 0.0;
+ limits.upper_claw.lower_limit = 0.0;
+ limits.upper_claw.upper_limit = 0.0;
+ limits.upper_claw.front.lower_angle = 0.0;
+ limits.upper_claw.front.upper_angle = 0.0;
+ limits.upper_claw.front.lower_decreasing_angle = 0.0;
+ limits.upper_claw.front.upper_decreasing_angle = 0.0;
+ limits.upper_claw.calibration.lower_angle = 0.0;
+ limits.upper_claw.calibration.upper_angle = 0.0;
+ limits.upper_claw.calibration.lower_decreasing_angle = 0.0;
+ limits.upper_claw.calibration.upper_decreasing_angle = 0.0;
+ limits.upper_claw.back.lower_angle = 0.0;
+ limits.upper_claw.back.upper_angle = 0.0;
+ limits.upper_claw.back.lower_decreasing_angle = 0.0;
+ limits.upper_claw.back.upper_decreasing_angle = 0.0;
+
+ limits.lower_claw.lower_hard_limit = 0.0;
+ limits.lower_claw.upper_hard_limit = 0.0;
+ limits.lower_claw.lower_limit = 0.0;
+ limits.lower_claw.upper_limit = 0.0;
+ limits.lower_claw.front.lower_angle = 0.0;
+ limits.lower_claw.front.upper_angle = 0.0;
+ limits.lower_claw.front.lower_decreasing_angle = 0.0;
+ limits.lower_claw.front.upper_decreasing_angle = 0.0;
+ limits.lower_claw.calibration.lower_angle = 0.0;
+ limits.lower_claw.calibration.upper_angle = 0.0;
+ limits.lower_claw.calibration.lower_decreasing_angle = 0.0;
+ limits.lower_claw.calibration.upper_decreasing_angle = 0.0;
+ limits.lower_claw.back.lower_angle = 0.0;
+ limits.lower_claw.back.upper_angle = 0.0;
+ limits.lower_claw.back.lower_decreasing_angle = 0.0;
+ limits.lower_claw.back.upper_decreasing_angle = 0.0;
+
+ limits.claw_unimportant_epsilon = 0.01;
+ limits.start_fine_tune_pos = -0.2;
+ limits.max_zeroing_voltage = 4.0;
+
+ control_loops::ClawGroup::Position last_position =
+ *control_loops::claw_queue_group.position;
+
+ while (true) {
+ control_loops::claw_queue_group.position.FetchNextBlocking();
+ bool print = false;
+ if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
+ print = true;
+ printf("Got an edge on the upper claw\n");
+ }
+ if (bottom.GetPositionOfEdge(
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
+ print = true;
+ printf("Got an edge on the lower claw\n");
+ }
+ const double top_position =
+ control_loops::claw_queue_group.position->top.position -
+ top_start_position;
+ const double bottom_position =
+ control_loops::claw_queue_group.position->bottom.position -
+ bottom_start_position;
+ const double separation = top_position - bottom_position;
+ if (separation > limits.claw_max_separation) {
+ limits.soft_max_separation = limits.claw_max_separation = separation;
+ print = true;
+ }
+ if (separation < limits.claw_min_separation) {
+ limits.soft_min_separation = limits.claw_min_separation = separation;
+ print = true;
+ }
+
+ if (print) {
+ printf("{%f,\n", limits.claw_zeroing_off_speed);
+ printf("%f,\n", limits.claw_zeroing_speed);
+ printf("%f,\n", limits.claw_zeroing_separation);
+ printf("%f,\n", limits.claw_min_separation);
+ printf("%f,\n", limits.claw_max_separation);
+ printf("%f,\n", limits.soft_min_separation);
+ printf("%f,\n", limits.soft_max_separation);
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.upper_claw.lower_hard_limit,
+ limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
+ limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
+ limits.upper_claw.front.upper_angle,
+ limits.upper_claw.front.lower_decreasing_angle,
+ limits.upper_claw.front.upper_decreasing_angle,
+ limits.upper_claw.calibration.lower_angle,
+ limits.upper_claw.calibration.upper_angle,
+ limits.upper_claw.calibration.lower_decreasing_angle,
+ limits.upper_claw.calibration.upper_decreasing_angle,
+ limits.upper_claw.back.lower_angle,
+ limits.upper_claw.back.upper_angle,
+ limits.upper_claw.back.lower_decreasing_angle,
+ limits.upper_claw.back.upper_decreasing_angle);
+
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.lower_claw.lower_hard_limit,
+ limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
+ limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
+ limits.lower_claw.front.upper_angle,
+ limits.lower_claw.front.lower_decreasing_angle,
+ limits.lower_claw.front.upper_decreasing_angle,
+ limits.lower_claw.calibration.lower_angle,
+ limits.lower_claw.calibration.upper_angle,
+ limits.lower_claw.calibration.lower_decreasing_angle,
+ limits.lower_claw.calibration.upper_decreasing_angle,
+ limits.lower_claw.back.lower_angle,
+ limits.lower_claw.back.upper_angle,
+ limits.lower_claw.back.lower_decreasing_angle,
+ limits.lower_claw.back.upper_decreasing_angle);
+ printf("%f, // claw_unimportant_epsilon\n",
+ limits.claw_unimportant_epsilon);
+ printf("%f, // start_fine_tune_pos\n", limits.start_fine_tune_pos);
+ printf("%f,\n", limits.max_zeroing_voltage);
+ printf("}\n");
+ }
+
+ last_position = *control_loops::claw_queue_group.position;
+ }
+ return 0;
+}
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ int returnvalue = ::frc971::Main();
+ ::aos::Cleanup();
+ return returnvalue;
+}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
new file mode 100644
index 0000000..b98a1bb
--- /dev/null
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -0,0 +1,634 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "aos/common/network/team_number.h"
+#include "aos/common/controls/control_loop_test.h"
+
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+
+#include "gtest/gtest.h"
+
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+typedef enum {
+ TOP_CLAW = 0,
+ BOTTOM_CLAW,
+
+ CLAW_COUNT
+} ClawType;
+
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ // Override this to define how to set up the environment.
+ virtual void SetUp() { aos::network::OverrideTeamNumber(1); }
+};
+
+::testing::Environment* const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
+// 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_top_position,
+ double initial_bottom_position)
+ : claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())),
+ claw_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.claw_queue_group.goal",
+ ".frc971.control_loops.claw_queue_group.position",
+ ".frc971.control_loops.claw_queue_group.output",
+ ".frc971.control_loops.claw_queue_group.status") {
+ Reinitialize(initial_top_position, initial_bottom_position);
+ }
+
+ void Reinitialize(double initial_top_position,
+ double initial_bottom_position) {
+ LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
+ initial_bottom_position);
+ claw_plant_->mutable_X(0, 0) = initial_bottom_position;
+ claw_plant_->mutable_X(1, 0) = initial_top_position - initial_bottom_position;
+ claw_plant_->mutable_X(2, 0) = 0.0;
+ claw_plant_->mutable_X(3, 0) = 0.0;
+ claw_plant_->mutable_Y() = claw_plant_->C() * claw_plant_->X();
+
+ ReinitializePartial(TOP_CLAW, initial_top_position);
+ ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
+ last_position_.Zero();
+ SetPhysicalSensors(&last_position_);
+ }
+
+ // Returns the absolute angle of the wrist.
+ double GetAbsolutePosition(ClawType type) const {
+ if (type == TOP_CLAW) {
+ return claw_plant_->Y(1, 0);
+ } else {
+ return claw_plant_->Y(0, 0);
+ }
+ }
+
+ // Returns the adjusted angle of the wrist.
+ double GetPosition(ClawType type) const {
+ return GetAbsolutePosition(type) - initial_position_[type];
+ }
+
+ // Makes sure pos is inside range (exclusive)
+ bool CheckRange(double pos, const constants::Values::Claws::AnglePair &pair) {
+ // Note: If the >= and <= signs are used, then the there exists a case
+ // where the wrist starts precisely on the edge and because initial
+ // position and the *edge_value_ are the same, then the comparison
+ // in ZeroedStateFeedbackLoop::DoGetPositionOfEdge will return that
+ // the lower, rather than upper, edge of the hall effect was passed.
+ return (pos > pair.lower_angle && pos < pair.upper_angle);
+ }
+
+ void SetHallEffect(double pos,
+ const constants::Values::Claws::AnglePair &pair,
+ HallEffectStruct *hall_effect) {
+ hall_effect->current = CheckRange(pos, pair);
+ }
+
+ void SetClawHallEffects(double pos,
+ const constants::Values::Claws::Claw &claw,
+ control_loops::HalfClawPosition *half_claw) {
+ SetHallEffect(pos, claw.front, &half_claw->front);
+ SetHallEffect(pos, claw.calibration, &half_claw->calibration);
+ SetHallEffect(pos, claw.back, &half_claw->back);
+ }
+
+ // Sets the values of the physical sensors that can be directly observed
+ // (encoder, hall effect).
+ void SetPhysicalSensors(control_loops::ClawGroup::Position *position) {
+ position->top.position = GetPosition(TOP_CLAW);
+ position->bottom.position = GetPosition(BOTTOM_CLAW);
+
+ double pos[2] = {GetAbsolutePosition(TOP_CLAW),
+ GetAbsolutePosition(BOTTOM_CLAW)};
+ LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n", pos[TOP_CLAW],
+ pos[BOTTOM_CLAW]);
+
+ const frc971::constants::Values& values = constants::GetValues();
+
+ // Signal that each hall effect sensor has been triggered if it is within
+ // the correct range.
+ SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw, &position->top);
+ SetClawHallEffects(pos[BOTTOM_CLAW], values.claw.lower_claw,
+ &position->bottom);
+ }
+
+ void UpdateHallEffect(double angle,
+ double last_angle,
+ double initial_position,
+ double *posedge_value,
+ double *negedge_value,
+ HallEffectStruct *position,
+ const HallEffectStruct &last_position,
+ const constants::Values::Claws::AnglePair &pair,
+ const char *claw_name, const char *hall_effect_name) {
+ if (position->current && !last_position.current) {
+ ++position->posedge_count;
+
+ if (last_angle < pair.lower_angle) {
+ LOG(DEBUG, "%s: Positive lower edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *posedge_value = pair.lower_angle - initial_position;
+ } else {
+ LOG(DEBUG, "%s: Positive upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *posedge_value = pair.upper_angle - initial_position;
+ }
+ }
+ if (!position->current && last_position.current) {
+ ++position->negedge_count;
+
+ if (angle < pair.lower_angle) {
+ LOG(DEBUG, "%s: Negative lower edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *negedge_value = pair.lower_angle - initial_position;
+ } else {
+ LOG(DEBUG, "%s: Negative upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *negedge_value = pair.upper_angle - initial_position;
+ }
+ }
+ }
+
+ void UpdateClawHallEffects(
+ control_loops::HalfClawPosition *position,
+ const control_loops::HalfClawPosition &last_position,
+ const constants::Values::Claws::Claw &claw, double initial_position,
+ const char *claw_name) {
+ UpdateHallEffect(position->position + initial_position,
+ last_position.position + initial_position,
+ initial_position, &position->posedge_value,
+ &position->negedge_value, &position->front,
+ last_position.front, claw.front, claw_name, "front");
+ UpdateHallEffect(position->position + initial_position,
+ last_position.position + initial_position,
+ initial_position, &position->posedge_value,
+ &position->negedge_value, &position->calibration,
+ last_position.calibration, claw.calibration, claw_name,
+ "calibration");
+ UpdateHallEffect(position->position + initial_position,
+ last_position.position + initial_position,
+ initial_position, &position->posedge_value,
+ &position->negedge_value, &position->back,
+ last_position.back, claw.back, claw_name, "back");
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ClawGroup::Position> position =
+ claw_queue_group.position.MakeMessage();
+
+ // Initialize all the counters to their previous values.
+ *position = last_position_;
+
+ SetPhysicalSensors(position.get());
+
+ const frc971::constants::Values& values = constants::GetValues();
+
+ UpdateClawHallEffects(&position->top, last_position_.top,
+ values.claw.upper_claw, initial_position_[TOP_CLAW],
+ "Top");
+ UpdateClawHallEffects(&position->bottom, last_position_.bottom,
+ values.claw.lower_claw,
+ initial_position_[BOTTOM_CLAW], "Bottom");
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ last_position_ = *position;
+ position.Send();
+ }
+
+ // Simulates the claw moving for one timestep.
+ void Simulate() {
+ const frc971::constants::Values& v = constants::GetValues();
+ EXPECT_TRUE(claw_queue_group.output.FetchLatest());
+
+ claw_plant_->mutable_U() << claw_queue_group.output->bottom_claw_voltage,
+ claw_queue_group.output->top_claw_voltage;
+ claw_plant_->Update();
+
+ // Check that the claw is within the limits.
+ EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0, 0));
+ EXPECT_LE(v.claw.upper_claw.lower_hard_limit, claw_plant_->Y(0, 0));
+
+ EXPECT_GE(v.claw.lower_claw.upper_hard_limit, claw_plant_->Y(1, 0));
+ EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1, 0));
+
+ EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ v.claw.claw_max_separation);
+ EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ v.claw.claw_min_separation);
+ }
+ // The whole claw.
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
+
+ private:
+ // Resets the plant so that it starts at initial_position.
+ void ReinitializePartial(ClawType type, double initial_position) {
+ initial_position_[type] = initial_position;
+ }
+
+ ClawGroup claw_queue_group;
+ double initial_position_[CLAW_COUNT];
+
+ control_loops::ClawGroup::Position last_position_;
+};
+
+
+class ClawTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ // 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.
+ ClawGroup claw_queue_group;
+
+ // Create a loop and simulation plant.
+ ClawMotor claw_motor_;
+ ClawMotorSimulation claw_motor_plant_;
+
+ // Minimum amount of acceptable separation between the top and bottom of the
+ // claw.
+ double min_separation_;
+
+ ClawTest()
+ : claw_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.claw_queue_group.goal",
+ ".frc971.control_loops.claw_queue_group.position",
+ ".frc971.control_loops.claw_queue_group.output",
+ ".frc971.control_loops.claw_queue_group.status"),
+ claw_motor_(&claw_queue_group),
+ claw_motor_plant_(0.4, 0.2),
+ min_separation_(constants::GetValues().claw.claw_min_separation) {
+ }
+
+ void VerifyNearGoal() {
+ claw_queue_group.goal.FetchLatest();
+ claw_queue_group.position.FetchLatest();
+ double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
+ double separation =
+ claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
+ EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
+ EXPECT_NEAR(claw_queue_group.goal->separation_angle, separation, 1e-4);
+ EXPECT_LE(min_separation_, separation);
+ }
+};
+
+TEST_F(ClawTest, HandlesNAN) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(::std::nan(""))
+ .separation_angle(::std::nan(""))
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+}
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ClawTest, ZerosCorrectly) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ClawTest, LimitClawGoal) {
+ frc971::constants::Values values;
+ values.claw.claw_min_separation = -2.0;
+ values.claw.claw_max_separation = 1.0;
+ values.claw.soft_min_separation = -2.0;
+ values.claw.soft_max_separation = 1.0;
+ values.claw.upper_claw.lower_limit = -5.0;
+ values.claw.upper_claw.upper_limit = 7.5;
+ values.claw.lower_claw.lower_limit = -5.5;
+ values.claw.lower_claw.upper_limit = 8.0;
+
+ double bottom_goal = 0.0;
+ double top_goal = 0.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(0.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(0.0, top_goal, 1e-4);
+
+ bottom_goal = 0.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-1.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(-3.0, top_goal, 1e-4);
+
+ bottom_goal = 0.0;
+ top_goal = 4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(1.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(2.5, top_goal, 1e-4);
+
+ bottom_goal = -10.0;
+ top_goal = -9.5;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-5.0, top_goal, 1e-4);
+
+ bottom_goal = -20.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-4.5, top_goal, 1e-4);
+
+ bottom_goal = -20.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-4.5, top_goal, 1e-4);
+
+ bottom_goal = -5.0;
+ top_goal = -10.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-3.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(-5.0, top_goal, 1e-4);
+
+ bottom_goal = 10.0;
+ top_goal = 9.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(8.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(7.0, top_goal, 1e-4);
+
+ bottom_goal = 8.0;
+ top_goal = 9.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(6.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(7.5, top_goal, 1e-4);
+}
+
+
+class ZeroingClawTest
+ : public ClawTest,
+ public ::testing::WithParamInterface< ::std::pair<double, double>> {};
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_P(ZeroingClawTest, ParameterizedZero) {
+ claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
+
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 700; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
+ SimulateTimestep(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_P(ZeroingClawTest, HandleMissingPosition) {
+ claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
+
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 700; ++i) {
+ if (i % 23) {
+ claw_motor_plant_.SendPositionMessage();
+ }
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
+ SimulateTimestep(true);
+ }
+ VerifyNearGoal();
+}
+
+INSTANTIATE_TEST_CASE_P(ZeroingClawTest, ZeroingClawTest,
+ ::testing::Values(::std::make_pair(0.04, 0.02),
+ ::std::make_pair(0.2, 0.1),
+ ::std::make_pair(0.3, 0.2),
+ ::std::make_pair(0.4, 0.3),
+ ::std::make_pair(0.5, 0.4),
+ ::std::make_pair(0.6, 0.5),
+ ::std::make_pair(0.7, 0.6),
+ ::std::make_pair(0.8, 0.7),
+ ::std::make_pair(0.9, 0.8),
+ ::std::make_pair(1.0, 0.9),
+ ::std::make_pair(1.1, 1.0),
+ ::std::make_pair(1.15, 1.05),
+ ::std::make_pair(1.05, 0.95),
+ ::std::make_pair(1.2, 1.1),
+ ::std::make_pair(1.3, 1.2),
+ ::std::make_pair(1.4, 1.3),
+ ::std::make_pair(1.5, 1.4),
+ ::std::make_pair(1.6, 1.5),
+ ::std::make_pair(1.7, 1.6),
+ ::std::make_pair(1.8, 1.7),
+ ::std::make_pair(2.015, 2.01)
+));
+
+/*
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(ClawTest, RezeroWithMissingPos) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .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) {
+ claw_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_TRUE(claw_motor_.is_uninitialized());
+ }
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.2)
+ .separation_angle(0.2)
+ .Send();
+ }
+ if (i == 410) {
+ EXPECT_TRUE(claw_motor_.is_zeroing());
+ // TODO(austin): Expose if the top and bototm is zeroing through
+ // functions.
+ // EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
+ }
+
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(ClawTest, DisableGoesUninitialized) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 800; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SimulateTimestep(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(claw_motor_.is_uninitialized());
+ // When disabled, we should be applying 0 power.
+ EXPECT_TRUE(claw_queue_group.output.FetchLatest());
+ EXPECT_EQ(0, claw_queue_group.output->top_claw_voltage);
+ EXPECT_EQ(0, claw_queue_group.output->bottom_claw_voltage);
+ }
+ } else {
+ SimulateTimestep(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // TODO(austin): Expose if the top and bottom is zeroing through
+ // functions.
+ }
+
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+*/
+
+class WindupClawTest : public ClawTest {
+ protected:
+ void TestWindup(ClawMotor::CalibrationMode mode, int start_time, double offset) {
+ int capped_count = 0;
+ const frc971::constants::Values& values = constants::GetValues();
+ bool kicked = false;
+ bool measured = false;
+ for (int i = 0; i < 700; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ if (i >= start_time && mode == claw_motor_.mode() && !kicked) {
+ EXPECT_EQ(mode, claw_motor_.mode());
+ // Move the zeroing position far away and verify that it gets moved
+ // back.
+ claw_motor_.top_claw_goal_ += offset;
+ claw_motor_.bottom_claw_goal_ += offset;
+ kicked = true;
+ } else {
+ if (kicked && !measured) {
+ measured = true;
+ EXPECT_EQ(mode, claw_motor_.mode());
+
+ Eigen::Matrix<double, 4, 1> R;
+ R << claw_motor_.bottom_claw_goal_,
+ claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
+ 0.0;
+ Eigen::Matrix<double, 2, 1> uncapped_voltage =
+ claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat());
+ // Use a factor of 1.8 because so long as it isn't actually running
+ // away, the CapU function will deal with getting the actual output
+ // down.
+ EXPECT_LT(::std::abs(uncapped_voltage(0, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
+ EXPECT_LT(::std::abs(uncapped_voltage(1, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
+ }
+ }
+ if (claw_motor_.mode() == mode) {
+ if (claw_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(values.claw.max_zeroing_voltage,
+ ::std::abs(claw_motor_.uncapped_average_voltage()));
+ } else {
+ ASSERT_GT(values.claw.max_zeroing_voltage,
+ ::std::abs(claw_motor_.uncapped_average_voltage()));
+ }
+ }
+
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_TRUE(kicked);
+ EXPECT_TRUE(measured);
+ EXPECT_LE(1, capped_count);
+ EXPECT_GE(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(WindupClawTest, NoWindupPositive) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+
+ TestWindup(ClawMotor::UNKNOWN_LOCATION, 100, 971.0);
+
+ 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(WindupClawTest, NoWindupNegative) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+
+ TestWindup(ClawMotor::UNKNOWN_LOCATION, 100, -971.0);
+
+ 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(WindupClawTest, NoWindupNegativeFineTune) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .separation_angle(0.2)
+ .Send();
+
+ TestWindup(ClawMotor::FINE_TUNE_BOTTOM, 200, -971.0);
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..e573d6f
--- /dev/null
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "y2014/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/y2014/control_loops/claw/claw_motor_plant.cc b/y2014/control_loops/claw/claw_motor_plant.cc
new file mode 100644
index 0000000..acba8d0
--- /dev/null
+++ b/y2014/control_loops/claw/claw_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.0, 0.00909331035298, 0.0, 0.0, 1.0, -6.04514323962e-05, 0.00903285892058, 0.0, 0.0, 0.824315642255, 0.0, 0.0, 0.0, -0.0112975266368, 0.813018115618;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000352527133889, 0.0, -0.000352527133889, 0.000376031064118, 0.0683072794628, 0.0, -0.0683072794628, 0.0726998350615;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 1, 1, 0, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeClawController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.72431564225, 2.70472958703e-16, -1.72431564225, 1.71301811562, 65.9456997026, 1.03478906105e-14, -65.9456997026, 64.4642687194;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 106.138028875, 0.0, 4.20012492658, 0.0, 99.5038407367, 99.7251230882, 3.77683310096, 3.78980738032;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, 0.0, -0.0110313451387, 0.0, 0.0, 1.0, -7.89348747778e-05, -0.0111102800135, 0.0, 0.0, 1.21312753118, 0.0, 0.0, 0.0, 0.016857361889, 1.22998489307;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeClawPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeClawPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClawPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeClawLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeClawController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/claw/claw_motor_plant.h b/y2014/control_loops/claw/claw_motor_plant.h
new file mode 100644
index 0000000..c7de33a
--- /dev/null
+++ b/y2014/control_loops/claw/claw_motor_plant.h
@@ -0,0 +1,22 @@
+#ifndef Y2014_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+#define Y2014_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+static constexpr double kClawMomentOfInertiaRatio = 0.933333;
+
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeClawController();
+
+StateFeedbackPlant<4, 2, 2> MakeClawPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/y2014/control_loops/claw/replay_claw.cc b/y2014/control_loops/claw/replay_claw.cc
new file mode 100644
index 0000000..70d881c
--- /dev/null
+++ b/y2014/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 "y2014/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();
+}