tests run, shooter not implemented, so it doesnt look like it moves when you run the test
diff --git a/aos/linux_code/configuration.cc b/aos/linux_code/configuration.cc
index 6c39a93..a8cc4ae 100644
--- a/aos/linux_code/configuration.cc
+++ b/aos/linux_code/configuration.cc
@@ -23,6 +23,20 @@
// TODO(brians): Don't hard-code this.
const char *const kLinuxNetInterface = "eth0";
const in_addr *DoGetOwnIPAddress() {
+ static const char *kOverrideVariable = "FRC971_IP_OVERRIDE";
+ const char *override_ip = getenv(kOverrideVariable);
+ if (override_ip != NULL) {
+ LOG(INFO, "Override IP is %s\n", override_ip);
+ static in_addr r;
+ if (inet_aton(override_ip, &r) != 0) {
+ return &r;
+ } else {
+ LOG(WARNING, "error parsing %s value '%s'\n", kOverrideVariable, override_ip);
+ }
+ } else {
+ LOG(INFO, "Couldn't get environmental variable.\n");
+ }
+
ifaddrs *addrs;
if (getifaddrs(&addrs) != 0) {
LOG(FATAL, "getifaddrs(%p) failed with %d: %s\n", &addrs,
diff --git a/frc971/constants.cc b/frc971/constants.cc
old mode 100644
new mode 100755
index b17a7cb..e6a7aa5
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -38,12 +38,13 @@
0.47};
const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
0.55};
-const double shooter_lower_limit=0.0;
-const double shooter_upper_limit=0.0;
+const double shooter_lower_physical_limit=0.0;
+const double shooter_upper_physical_limit=0.0;
+const double shooter_voltage=0.0;
const double shooter_hall_effect_start_position=0.0;
const double shooter_zeroing_off_speed=0.0;
const double shooter_zeroing_speed=0.0;
-const double pos=0.0;
+const double position=0.0;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
@@ -54,12 +55,13 @@
kCompDrivetrainEncoderRatio,
kCompLowGearRatio,
kCompHighGearRatio,
- shooter_lower_limit,
- shooter_upper_limit,
+ shooter_voltage,
+ shooter_lower_physical_limit,
+ shooter_upper_physical_limit,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
- pos,
+ position,
kCompLeftDriveShifter,
kCompRightDriveShifter,
true,
@@ -72,12 +74,13 @@
kPracticeDrivetrainEncoderRatio,
kPracticeLowGearRatio,
kPracticeHighGearRatio,
- shooter_lower_limit,
- shooter_upper_limit,
+ shooter_voltage,
+ shooter_lower_physical_limit,
+ shooter_upper_physical_limit,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
- pos,
+ position,
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
diff --git a/frc971/constants.h b/frc971/constants.h
old mode 100644
new mode 100755
index 58825ca..07cf04f
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -33,12 +33,13 @@
double low_gear_ratio;
double high_gear_ratio;
- double shooter_lower_limit;
- double shooter_upper_limit;
+ double shooter_voltage;
+ double shooter_lower_physical_limit;
+ double shooter_upper_physical_limit;
double shooter_hall_effect_start_position;
double shooter_zeroing_off_speed;
double shooter_zeroing_speed;
- double pos;
+ double position;
ShifterHallEffect left_drive, right_drive;
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.cc b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
new file mode 100644
index 0000000..a0eb131
--- /dev/null
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeBottomClawController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.81581823335, 78.6334534274, 142.868137351;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 92.6004807973, 4.38063492858, 1.11581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeBottomClawPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeBottomClawPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeBottomClawController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.h b/frc971/control_loops/claw/bottom_claw_motor_plant.h
new file mode 100644
index 0000000..fc905ca
--- /dev/null
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeBottomClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
new file mode 100755
index 0000000..e3e7e6d
--- /dev/null
+++ b/frc971/control_loops/claw/claw.cc
@@ -0,0 +1,376 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+// Zeroing plan.
+// There are 2 types of zeros. Enabled and disabled ones.
+// Disabled ones are only valid during auto mode, and can be used to speed up
+// the enabled zero process. We need to re-zero during teleop in case the auto
+// zero was poor and causes us to miss all our shots.
+//
+// We need to be able to zero manually while disabled by moving the joint over
+// the zeros.
+// Zero on the down edge when disabled (gravity in the direction of motion)
+//
+// When enabled, zero on the up edge (gravity opposing the direction of motion)
+// The enabled sequence needs to work as follows. We can crash the claw if we
+// bring them too close to each other or too far from each other. The only safe
+// thing to do is to move them in unison.
+//
+// Start by moving them both towards the front of the bot to either find either
+// the middle hall effect on either jaw, or the front hall effect on the bottom
+// jaw. Any edge that isn't the desired edge will provide an approximate edge
+// location that can be used for the fine tuning step.
+// Once an edge is found on the front claw, move back the other way with both
+// claws until an edge is found for the other claw.
+// Now that we have an approximate zero, we can robustify the limits to keep
+// both claws safe. Then, we can move both claws to a position that is the
+// correct side of the zero and go zero.
+
+// Valid region plan.
+// Difference between the arms has a range, and the values of each arm has a range.
+// If a claw runs up against a static limit, don't let the goal change outside
+// the limit.
+// If a claw runs up against a movable limit, move both claws outwards to get
+// out of the condition.
+
+namespace frc971 {
+namespace control_loops {
+
+void ZeroedStateFeedbackLoop::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ }
+
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ U(0, 0) = voltage_ - old_voltage;
+ LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
+ LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+
+ last_voltage_ = voltage_;
+}
+
+ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ has_top_claw_goal_(false),
+ top_claw_goal_(0.0),
+ top_claw_(MakeTopClawLoop()),
+ has_bottom_claw_goal_(false),
+ bottom_claw_goal_(0.0),
+ bottom_claw_(MakeBottomClawLoop()),
+ was_enabled_(false),
+ doing_calibration_fine_tune_(false) {}
+
+const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+
+bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
+ const constants::Values::Claw &claw_values, double *edge_encoder,
+ double *edge_angle) {
+
+ // TODO(austin): Validate that the hall effect edge makes sense.
+ // We must now be on the side of the edge that we expect to be, and the
+ // encoder must have been on either side of the edge before and after.
+
+ if (front_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.front.upper_angle;
+ } else {
+ *edge_angle = claw_values.front.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (front_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.front.upper_angle;
+ } else {
+ *edge_angle = claw_values.front.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ if (calibration_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.calibration.upper_angle;
+ } else {
+ *edge_angle = claw_values.calibration.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (calibration_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.calibration.upper_angle;
+ } else {
+ *edge_angle = claw_values.calibration.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ if (back_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.back.upper_angle;
+ } else {
+ *edge_angle = claw_values.back.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (back_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.back.upper_angle;
+ } else {
+ *edge_angle = claw_values.back.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ return false;
+}
+
+// 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,
+ ::aos::control_loops::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;
+ }
+
+ // TODO(austin): Handle the disabled state and the disabled -> enabled
+ // transition in all of these states.
+ // TODO(austin): Handle zeroing while disabled.
+
+ // TODO(austin): Save all the counters so we know when something actually
+ // happens.
+ // TODO(austin): Helpers to find the position of the claw on an edge.
+
+ // TODO(austin): This may not be necesary because of the ControlLoop class.
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() == nullptr) {
+ return;
+ }
+
+ const frc971::constants::Values &values = constants::GetValues();
+
+ if (position) {
+ top_claw_.SetPositionValues(position->top);
+ bottom_claw_.SetPositionValues(position->bottom);
+
+ if (!has_top_claw_goal_) {
+ has_top_claw_goal_ = true;
+ top_claw_goal_ = position->top.position;
+ }
+ if (!has_bottom_claw_goal_) {
+ has_bottom_claw_goal_ = true;
+ bottom_claw_goal_ = position->bottom.position;
+ }
+ }
+
+ bool autonomous = ::aos::robot_state->autonomous;
+ bool enabled = ::aos::robot_state->enabled;
+
+ if ((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->seperation_angle;
+ } 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 (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
+ // always get the bottom claw to calibrated first
+ if (!doing_calibration_fine_tune_) {
+ if (position->bottom.position > values.start_fine_tune_pos -
+ values.claw_unimportant_epsilon &&
+ position->bottom.position < values.start_fine_tune_pos +
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ } else {
+ // send bottom to zeroing start
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ }
+ } else {
+ if (position->bottom.front_hall_effect ||
+ position->bottom.back_hall_effect ||
+ position->top.front_hall_effect ||
+ position->top.back_hall_effect) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ }
+ if (position->bottom.calibration_hall_effect) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calinrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ }
+ }
+ // now set the top claw to track
+ top_claw_goal_ = bottom_claw_goal_ + goal->seperation_angle;
+ } else {
+ // bottom claw must be calibrated, start on the top
+ if (!doing_calibration_fine_tune_) {
+ if (position->top.position > values.start_fine_tune_pos -
+ values.claw_unimportant_epsilon &&
+ position->top.position < values.start_fine_tune_pos +
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ top_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ } else {
+ // send top to zeroing start
+ top_claw_goal_ = values.start_fine_tune_pos;
+ }
+ } else {
+ if (position->top.front_hall_effect ||
+ position->top.back_hall_effect ||
+ position->top.front_hall_effect ||
+ position->top.back_hall_effect) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.start_fine_tune_pos;
+ }
+ if (position->top.calibration_hall_effect) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calinrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ }
+ }
+ // now set the bottom claw to track
+ bottom_claw_goal_ = top_claw_goal_ - goal->seperation_angle;
+ }
+ } else {
+ if (!was_enabled_ && enabled) {
+ if (position) {
+ top_claw_goal_ = position->top.position;
+ bottom_claw_goal_ = position->bottom.position;
+ } else {
+ has_top_claw_goal_ = false;
+ has_bottom_claw_goal_ = false;
+ }
+ }
+
+ // TODO(austin): Limit the goals here.
+ // Need to prevent windup, limit voltage, deal with windup on only 1 claw,
+ // ...
+ if (top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ }
+ if (bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ }
+
+ if (bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ if (enabled) {
+ // Time to slowly move back up to find any position to narrow down the
+ // zero.
+ top_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ // TODO(austin): Goal velocity too!
+ }
+ } 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_zeroing_off_speed * dt;
+ bottom_claw_goal_ -= values.claw_zeroing_off_speed * dt;
+ // TODO(austin): Goal velocity too!
+ }
+ }
+
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ top_claw_.SetCalibrationOnEdge(
+ values.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
+ }
+
+ // TODO(austin): Handle disabled.
+
+ // TODO(austin): ...
+ if (has_top_claw_goal_ && has_bottom_claw_goal_) {
+ top_claw_.R << top_claw_goal_, 0.0, 0.0;
+ bottom_claw_.R << bottom_claw_goal_, 0.0, 0.0;
+
+ top_claw_.Update(output == nullptr);
+ bottom_claw_.Update(output == nullptr);
+ } else {
+ top_claw_.ZeroPower();
+ bottom_claw_.ZeroPower();
+ }
+
+ if (position) {
+ //LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
+ //position->top_calibration_hall_effect ? "true" : "false",
+ //zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->top_claw_voltage = top_claw_.voltage();
+ output->bottom_claw_voltage = bottom_claw_.voltage();
+ }
+ status->done = false;
+ //::std::abs(zeroed_joint_.absolute_position() - goal->bottom_angle -
+ //goal->seperation_angle) < 0.004;
+
+ was_enabled_ = ::aos::robot_state->enabled;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
new file mode 100644
index 0000000..c541dad
--- /dev/null
+++ b/frc971/control_loops/claw/claw.gyp
@@ -0,0 +1,68 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'claw_loops',
+ 'type': 'static_library',
+ 'sources': ['claw.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/claw',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'claw_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'claw.cc',
+ 'top_claw_motor_plant.cc',
+ 'bottom_claw_motor_plant.cc',
+ 'unaugmented_top_claw_motor_plant.cc',
+ 'unaugmented_bottom_claw_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'claw_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'claw_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'claw_loops',
+ 'claw_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_lib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
new file mode 100755
index 0000000..c045d6f
--- /dev/null
+++ b/frc971/control_loops/claw/claw.h
@@ -0,0 +1,231 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+#define FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class ClawTest_NoWindupPositive_Test;
+class ClawTest_NoWindupNegative_Test;
+};
+
+// 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.
+
+// 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 StateFeedbackLoop<3, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ voltage_(0.0),
+ last_voltage_(0.0),
+ uncapped_voltage_(0.0),
+ offset_(0.0),
+ front_hall_effect_posedge_count_(0.0),
+ previous_front_hall_effect_posedge_count_(0.0),
+ front_hall_effect_negedge_count_(0.0),
+ previous_front_hall_effect_negedge_count_(0.0),
+ calibration_hall_effect_posedge_count_(0.0),
+ previous_calibration_hall_effect_posedge_count_(0.0),
+ calibration_hall_effect_negedge_count_(0.0),
+ previous_calibration_hall_effect_negedge_count_(0.0),
+ back_hall_effect_posedge_count_(0.0),
+ previous_back_hall_effect_posedge_count_(0.0),
+ back_hall_effect_negedge_count_(0.0),
+ previous_back_hall_effect_negedge_count_(0.0),
+ zeroing_state_(UNKNOWN_POSITION),
+ posedge_value_(0.0),
+ negedge_value_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0){}
+
+ const static int kZeroingMaxVoltage = 5;
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
+
+ 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_; }
+
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle) {
+ offset_ = edge_angle - edge_encoder;
+ }
+
+ bool SetCalibrationOnEdge(const constants::Values::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;
+ }
+
+ void SetPositionValues(const HalfClawPosition &claw) {
+ set_front_hall_effect_posedge_count(claw.front_hall_effect_posedge_count);
+ set_front_hall_effect_negedge_count(claw.front_hall_effect_negedge_count);
+ set_calibration_hall_effect_posedge_count(
+ claw.calibration_hall_effect_posedge_count);
+ set_calibration_hall_effect_negedge_count(
+ claw.calibration_hall_effect_negedge_count);
+ set_back_hall_effect_posedge_count(claw.back_hall_effect_posedge_count);
+ set_back_hall_effect_negedge_count(claw.back_hall_effect_negedge_count);
+
+ posedge_value_ = claw.posedge_value;
+ negedge_value_ = claw.negedge_value;
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << claw.position;
+ Correct(Y);
+ }
+
+#define COUNT_SETTER_GETTER(variable) \
+ void set_##variable(int32_t value) { \
+ previous_##variable##_ = variable##_; \
+ variable##_ = value; \
+ } \
+ int32_t variable() const { return variable##_; } \
+ bool variable##_changed() const { \
+ return previous_##variable##_ != variable##_; \
+ }
+
+ // TODO(austin): Pull all this out of the new sub structure.
+ COUNT_SETTER_GETTER(front_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(front_hall_effect_negedge_count);
+ COUNT_SETTER_GETTER(calibration_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(calibration_hall_effect_negedge_count);
+ COUNT_SETTER_GETTER(back_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(back_hall_effect_negedge_count);
+
+ bool any_hall_effect_changed() const {
+ return front_hall_effect_posedge_count_changed() ||
+ front_hall_effect_negedge_count_changed() ||
+ calibration_hall_effect_posedge_count_changed() ||
+ calibration_hall_effect_negedge_count_changed() ||
+ back_hall_effect_posedge_count_changed() ||
+ back_hall_effect_negedge_count_changed();
+ }
+
+ double position() const { return X_hat(0, 0); }
+ double encoder() const { return encoder_; }
+ double last_encoder() const { return last_encoder_; }
+
+ // 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::Claw &claw,
+ double *edge_encoder, double *edge_angle);
+
+#undef COUNT_SETTER_GETTER
+
+ private:
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
+ double offset_;
+
+ double previous_position_;
+
+ int32_t front_hall_effect_posedge_count_;
+ int32_t previous_front_hall_effect_posedge_count_;
+ int32_t front_hall_effect_negedge_count_;
+ int32_t previous_front_hall_effect_negedge_count_;
+ int32_t calibration_hall_effect_posedge_count_;
+ int32_t previous_calibration_hall_effect_posedge_count_;
+ int32_t calibration_hall_effect_negedge_count_;
+ int32_t previous_calibration_hall_effect_negedge_count_;
+ int32_t back_hall_effect_posedge_count_;
+ int32_t previous_back_hall_effect_posedge_count_;
+ int32_t back_hall_effect_negedge_count_;
+ int32_t previous_back_hall_effect_negedge_count_;
+
+ JointZeroingState zeroing_state_;
+ double posedge_value_;
+ double negedge_value_;
+ double encoder_;
+ double last_encoder_;
+};
+
+
+class ClawMotor
+ : public aos::control_loops::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 is_ready() const { return false; }
+
+ protected:
+ virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
+ const control_loops::ClawGroup::Position *position,
+ control_loops::ClawGroup::Output *output,
+ ::aos::control_loops::Status *status);
+
+ double top_absolute_position() const { return top_claw_.position(); }
+ double bottom_absolute_position() const { return bottom_claw_.position(); }
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ClawTest_NoWindupPositive_Test;
+ friend class testing::ClawTest_NoWindupNegative_Test;
+
+ // The zeroed joint to use.
+ bool has_top_claw_goal_;
+ double top_claw_goal_;
+ ZeroedStateFeedbackLoop top_claw_;
+
+ bool has_bottom_claw_goal_;
+ double bottom_claw_goal_;
+ ZeroedStateFeedbackLoop bottom_claw_;
+
+ bool was_enabled_;
+ bool doing_calibration_fine_tune_;
+
+ DISALLOW_COPY_AND_ASSIGN(ClawMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
new file mode 100644
index 0000000..66e3036
--- /dev/null
+++ b/frc971/control_loops/claw/claw.q
@@ -0,0 +1,66 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+struct HalfClawPosition {
+ // The current position of this half of the claw.
+ double position;
+ // The value of the front hall effect sensor.
+ bool front_hall_effect;
+ // The number of positive and negative edges that have been captured on the
+ // front hall effect sensor.
+ int32_t front_hall_effect_posedge_count;
+ int32_t front_hall_effect_negedge_count;
+ // The value of the calibration hall effect sensor.
+ bool calibration_hall_effect;
+ // The number of positive and negative edges that have been captured on the
+ // calibration hall effect sensor.
+ int32_t calibration_hall_effect_posedge_count;
+ int32_t calibration_hall_effect_negedge_count;
+ // The value of the back hall effect sensor.
+ bool back_hall_effect;
+ // The number of positive and negative edges that have been captured on the
+ // back hall effect sensor.
+ int32_t back_hall_effect_posedge_count;
+ int32_t back_hall_effect_negedge_count;
+
+ // 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 horizontal, positive up.
+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 seperation_angle;
+ bool intake;
+ };
+
+ 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;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group ClawGroup claw_queue_group;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
new file mode 100644
index 0000000..30155f5
--- /dev/null
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -0,0 +1,633 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/claw.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+typedef enum {
+ TOP_CLAW = 0,
+ BOTTOM_CLAW,
+
+ CLAW_COUNT
+} ClawType;
+
+// 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)
+ : top_claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant())),
+ bottom_claw_plant_(
+ new StateFeedbackPlant<2, 1, 1>(MakeRawBottomClawPlant())),
+ 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);
+ 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 top_claw_plant_->Y(0, 0);
+ } else {
+ return bottom_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 (inclusive)
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ }
+
+ // 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 the hall effect sensor has been triggered if it is within
+ // the correct range.
+ position->top.front_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.front);
+ position->top.calibration_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.calibration);
+ position->top.back_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.back);
+
+ position->bottom.front_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.front);
+ position->bottom.calibration_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.calibration);
+ position->bottom.back_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.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();
+
+ // Handle the front hall effect.
+ if (position->top.front_hall_effect &&
+ !last_position_.top.front_hall_effect) {
+ ++position->top.front_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.front.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.front_hall_effect &&
+ last_position_.top.front_hall_effect) {
+ ++position->top.front_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.front.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the calibration hall effect.
+ if (position->top.calibration_hall_effect &&
+ !last_position_.top.calibration_hall_effect) {
+ ++position->top.calibration_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.calibration.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.calibration_hall_effect &&
+ last_position_.top.calibration_hall_effect) {
+ ++position->top.calibration_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.calibration.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the back hall effect.
+ if (position->top.back_hall_effect &&
+ !last_position_.top.back_hall_effect) {
+ ++position->top.back_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.back.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.back_hall_effect &&
+ last_position_.top.back_hall_effect) {
+ ++position->top.back_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.back.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Now deal with the bottom part of the claw.
+ // Handle the front hall effect.
+ if (position->bottom.front_hall_effect &&
+ !last_position_.bottom.front_hall_effect) {
+ ++position->bottom.front_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.front.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.front_hall_effect &&
+ last_position_.bottom.front_hall_effect) {
+ ++position->bottom.front_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.front.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the calibration hall effect.
+ if (position->bottom.calibration_hall_effect &&
+ !last_position_.bottom.calibration_hall_effect) {
+ ++position->bottom.calibration_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.calibration.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.calibration_hall_effect &&
+ last_position_.bottom.calibration_hall_effect) {
+ ++position->bottom.calibration_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.calibration.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the back hall effect.
+ if (position->bottom.back_hall_effect &&
+ !last_position_.bottom.back_hall_effect) {
+ ++position->bottom.back_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.back.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.back_hall_effect &&
+ last_position_.bottom.back_hall_effect) {
+ ++position->bottom.back_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.back.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // 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());
+ Simulate(TOP_CLAW, top_claw_plant_.get(), v.upper_claw,
+ claw_queue_group.output->top_claw_voltage);
+ Simulate(BOTTOM_CLAW, bottom_claw_plant_.get(), v.lower_claw,
+ claw_queue_group.output->bottom_claw_voltage);
+ }
+ // Top of the claw, the one with rollers
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> top_claw_plant_;
+ // Bottom of the claw, the one with tusks
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> bottom_claw_plant_;
+
+ private:
+ // Resets the plant so that it starts at initial_position.
+ void ReinitializePartial(ClawType type, double initial_position) {
+ StateFeedbackPlant<2, 1, 1>* plant;
+ if (type == TOP_CLAW) {
+ plant = top_claw_plant_.get();
+ } else {
+ plant = bottom_claw_plant_.get();
+ }
+ initial_position_[type] = initial_position;
+ plant->X(0, 0) = initial_position_[type];
+ plant->X(1, 0) = 0.0;
+ plant->Y = plant->C() * plant->X;
+ last_voltage_[type] = 0.0;
+ }
+
+ void Simulate(ClawType type, StateFeedbackPlant<2, 1, 1>* plant,
+ const constants::Values::Claw &claw, double nl_voltage) {
+ plant->U << last_voltage_[type];
+ plant->Update();
+
+ // check top claw inside limits
+ EXPECT_GE(claw.upper_limit, plant->Y(0, 0));
+ EXPECT_LE(claw.lower_limit, plant->Y(0, 0));
+
+ // TODO(austin): Check that the claws aren't too close to eachother.
+ last_voltage_[type] = nl_voltage;
+ }
+
+ ClawGroup claw_queue_group;
+ double initial_position_[CLAW_COUNT];
+ double last_voltage_[CLAW_COUNT];
+
+ control_loops::ClawGroup::Position last_position_;
+};
+
+
+class ClawTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ ClawGroup claw_queue_group;
+
+ // Create a loop and simulation plant.
+ ClawMotor claw_motor_;
+ ClawMotorSimulation claw_motor_plant_;
+
+ // Minimum amount of acceptable seperation between the top and bottom of the
+ // claw.
+ double min_seperation_;
+
+ 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_seperation_(constants::GetValues().claw_min_seperation) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ claw_queue_group.goal.FetchLatest();
+ claw_queue_group.position.FetchLatest();
+ double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
+ double seperation =
+ claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
+ EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
+ EXPECT_NEAR(claw_queue_group.goal->seperation_angle, seperation, 1e-4);
+ EXPECT_TRUE(min_seperation_ <= seperation);
+ }
+
+ virtual ~ClawTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ClawTest, ZerosCorrectly) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 400; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_F(ClawTest, ZerosStartingOn) {
+ claw_motor_plant_.Reinitialize(100 * M_PI / 180.0, 90 * M_PI / 180.0);
+
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+/*
+// Tests that missing positions are correctly handled.
+TEST_F(ClawTest, HandleMissingPosition) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ claw_motor_plant_.SendPositionMessage();
+ }
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// 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)
+ .seperation_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)
+ .seperation_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();
+ SendDSPacket(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)
+ .seperation_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) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // Give the loop a couple cycled to get the message and then verify that
+ // it is in the correct state.
+ EXPECT_TRUE(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 {
+ SendDSPacket(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();
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(ClawTest, NoWindupNegative) {
+ int capped_count[2] = {0, 0};
+ double saved_zeroing_position[2] = {0, 0};
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(claw_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position[TOP_CLAW] =
+ top_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ saved_zeroing_position[BOTTOM_CLAW] =
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_;
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ } else if (i == 51) {
+ EXPECT_TRUE(claw_motor_.is_zeroing());
+
+ EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
+ top_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ if (!claw_motor_.top().is_ready()) {
+ if (claw_motor_.top().capped_goal()) {
+ ++capped_count[TOP_CLAW];
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+ if (!claw_motor_.bottom().is_ready()) {
+ if (claw_motor_.bottom().capped_goal()) {
+ ++capped_count[BOTTOM_CLAW];
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count[TOP_CLAW]);
+ EXPECT_GT(3, capped_count[BOTTOM_CLAW]);
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(ClawTest, NoWindupPositive) {
+ int capped_count[2] = {0, 0};
+ double saved_zeroing_position[2] = {0, 0};
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position[TOP_CLAW] =
+ top_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ saved_zeroing_position[BOTTOM_CLAW] =
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ } else {
+ if (i == 51) {
+ EXPECT_TRUE(claw_motor_.top().is_zeroing());
+ EXPECT_TRUE(claw_motor_.bottom().is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
+ claw_motor_.top().zeroing_position_, 0.4);
+ EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
+ claw_motor_.bottom().zeroing_position_, 0.4);
+ }
+ }
+ if (!top_claw_motor_.is_ready()) {
+ if (top_claw_motor_.capped_goal()) {
+ ++capped_count[TOP_CLAW];
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(claw_motor_.top().zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(claw_motor_.top().zeroed_joint_.U_uncapped()));
+ }
+ }
+ if (!bottom_claw_motor_.is_ready()) {
+ if (bottom_claw_motor_.capped_goal()) {
+ ++capped_count[BOTTOM_CLAW];
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(claw_motor_.bottom().zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(claw_motor_.bottom().zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count[TOP_CLAW]);
+ EXPECT_GT(3, capped_count[BOTTOM_CLAW]);
+}
+*/
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_main.cc b/frc971/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..3b7c181
--- /dev/null
+++ b/frc971/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ClawMotor claw;
+ claw.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.cc b/frc971/control_loops/claw/top_claw_motor_plant.cc
new file mode 100644
index 0000000..113ff77
--- /dev/null
+++ b/frc971/control_loops/claw/top_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeTopClawController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.81581823335, 78.6334534274, 142.868137351;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 92.6004807973, 4.38063492858, 1.11581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeTopClawPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeTopClawPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeTopClawController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.h b/frc971/control_loops/claw/top_claw_motor_plant.h
new file mode 100644
index 0000000..c74c976
--- /dev/null
+++ b/frc971/control_loops/claw/top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeTopClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
new file mode 100644
index 0000000..cda15c4
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawBottomClawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawBottomClawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawBottomClawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
new file mode 100644
index 0000000..8f59925
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
new file mode 100644
index 0000000..8ab4bbf
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawTopClawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawTopClawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawTopClawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawTopClawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
new file mode 100644
index 0000000..c87d3ca
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawTopClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index b548365..802827a 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -20,8 +20,8 @@
using ::frc971::constants::GetValues;
ZeroedJoint<1>::ConfigurationData config_data;
- config_data.lower_limit = GetValues().shooter_lower_limit;
- config_data.upper_limit = GetValues().shooter_upper_limit;
+ config_data.lower_limit = GetValues().shooter_lower_physical_limit;
+ config_data.upper_limit = GetValues().shooter_upper_physical_limit;
//config_data.hall_effect_start_position[0] =
// GetValues().shooter_hall_effect_start_position;
config_data.zeroing_off_speed = GetValues().shooter_zeroing_off_speed;
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index a6615bf..0bcc97a 100644
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -22,7 +22,7 @@
: public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
public:
explicit ShooterMotor(
- control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter_queue_group);
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return zeroed_joint_.capped_goal(); }
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
old mode 100644
new mode 100755
index 7a487bc..cf99c30
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -13,18 +13,51 @@
bool locked; //Disc brake locked
};
message Goal {
- // The energy to load to in joules.
- double energy;
+ // encoder ticks of shot energy.
+ double shot_power;
double goal;
// Shoots as soon as this is true.
- bool shoot;
+ bool shot_requested;
+ bool unload_requested;
};
message Position {
- bool back_hall_effect;
+ // back on the plunger
+ bool plunger_back_hall_effect;
+ // truely back on the pusher
+ bool pusher_distal_hall_effect;
+ // warning that we are back on the pusher
+ bool pusher_proximal_hall_effect;
+ // the latch is closed
+ bool latch_hall_effect;
+
+ // count of positive edges
+ int64_t plunger_back_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t plunger_back_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t pusher_distal_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t pusher_distal_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t pusher_proximal_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t pusher_proximal_hall_effect_negedge_count;
+ // count of positive edges
+ int64_t latch_hall_effect_posedge_count;
+ // count of negative edges
+ int64_t latch_hall_effect_negedge_count;
+
// In meters, out is positive.
double position;
double back_calibration;
+
+ // last positive edge
+ double posedge_value;
+ // last negative edge
+ double negedge_value;
};
+ // I don't think this is needed, but it is here
+ // so I won't delete it yet.
message Status {
// Whether it's ready to shoot right now.
bool ready;
@@ -35,10 +68,18 @@
bool done;
};
+ message Output {
+ // desired motor voltage
+ double voltage;
+ // true: close latch, false: open latch
+ double latch_piston;
+ // true: brake engaded, false: brake release
+ double brake_piston;
+ };
queue Goal goal;
queue Position position;
queue Output output;
queue Status status;
};
-queue_group ShooterLoop shooter;
+queue_group ShooterLoop shooter_queue_group;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index a111917..37027f8 100644
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -44,20 +44,60 @@
// Returns the absolute angle of the shooter.
double GetAbsolutePosition() const {
- return 0.0;
+ return shooter_plant_->Y(0,0);
}
// Returns the adjusted angle of the shooter.
double GetPosition() const {
- return 0.0;
+ return shooter_plant_->Y(0, 0);
}
// Sends out the position queue messages.
void SendPositionMessage() {
+ const double current_pos = GetPosition();
+
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = current_pos;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position >= constants::GetValues().shooter_lower_physical_limit &&
+ abs_position <= constants::GetValues().shooter_upper_physical_limit) {
+ //position->plunger_back_hall_effect = true;
+ } else {
+ //position->plunger_back_hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ if ((last_position_ <
+ constants::GetValues().shooter_lower_physical_limit ||
+ last_position_ >
+ constants::GetValues().shooter_lower_physical_limit)/* &&
+ position->hall_effect*/) {
+ calibration_value_ =
+ constants::GetValues().shooter_hall_effect_start_position -
+ initial_position_;
+ }
+
+ position->back_calibration = calibration_value_;
+ position.Send();
}
// Simulates the shooter moving for one timestep.
void Simulate() {
+ last_position_ = shooter_plant_->Y(0, 0);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << last_voltage_;
+ shooter_plant_->Update();
+
+ EXPECT_GE(constants::GetValues().shooter_upper_physical_limit,
+ shooter_plant_->Y(0, 0));
+ EXPECT_LE(constants::GetValues().shooter_lower_physical_limit,
+ shooter_plant_->Y(0, 0));
+ last_voltage_ = my_shooter_loop_.output->voltage;
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
@@ -115,23 +155,26 @@
}
};
-// Tests that the shooter zeros correctly and goes to a position.
-//TEST_F(ShooterTest, ZerosCorrectly) {
-// my_shooter.goal.MakeWithBuilder().goal(0.1).Send();
-// for (int i = 0; i < 400; ++i) {
-// shooter_motor_plant_.SendPositionMessage();
-// shooter_motor_.Iterate();
-// shooter_motor_plant_.Simulate();
-// SendDSPacket(true);
-// }
-// VerifyNearGoal();
+//TEST_F(ShooterTest, EmptyTest) {
+// EXPECT_TRUE(true);
//}
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, ZerosCorrectly) {
+ my_shooter_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
// Tests that the shooter zeros correctly starting on the hall effect sensor.
-TEST_F(ShooterTest, ZerosStartingOn) {
- printf("test");
- EXPECT_TRUE(true);
-}
+//TEST_F(ShooterTest, ZerosStartingOn) {
+// printf("test");
+// EXPECT_TRUE(true);
+//}
//// Tests that missing positions are correctly handled.
//TEST_F(ShooterTest, HandleMissingPosition) {