Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/control_loops/fridge/arm_motor_plant.cc b/y2015/control_loops/fridge/arm_motor_plant.cc
new file mode 100644
index 0000000..6e3205a
--- /dev/null
+++ b/y2015/control_loops/fridge/arm_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "y2015/control_loops/fridge/arm_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, -0.18154390621, 0.919241022297;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 1, 0, 1, 0, -1, 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> MakeArmController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.759390396955, -0.759390396955, 54.1048167043, -54.1048167043;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.197397150694, 1.08682415753;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeArmPlant() {
+ ::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>(MakeArmPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeArmLoop() {
+ ::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>(MakeArmController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/fridge/arm_motor_plant.h b/y2015/control_loops/fridge/arm_motor_plant.h
new file mode 100644
index 0000000..3bf8d09
--- /dev/null
+++ b/y2015/control_loops/fridge/arm_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
+#define Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeArmController();
+
+StateFeedbackPlant<4, 2, 2> MakeArmPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeArmLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/fridge/elevator_motor_plant.cc b/y2015/control_loops/fridge/elevator_motor_plant.cc
new file mode 100644
index 0000000..995d838
--- /dev/null
+++ b/y2015/control_loops/fridge/elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "y2015/control_loops/fridge/elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00435668193669, 0.0, 0.0, 0.0, 0.754212786054, 0.0, 0.0, 0.0, 0.0, 0.997194498569, 0.00435222083164, 0.0, 0.0, -1.07131589702, 0.751658962986;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 3.82580284276e-05, 3.82580284276e-05, 0.0146169286307, 0.0146169286307, 3.82387898839e-05, -3.82387898839e-05, 0.0146019613563, -0.0146019613563;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 1, 0, 1, 0, -1, 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> MakeElevatorController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.677106393027, 0.677106393027, 35.5375738607, 35.5375738607, 0.674426730777, -0.674426730777, 34.7138874344, -34.7138874344;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 321.310606763, 11.7674534233, 601.047935717, 12.6977148843, 321.310606764, 11.7674534233, -601.047935716, -12.6977148843;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00577646258091, 0.0, 0.0, 0.0, 1.32588576923, 0.0, 0.0, 0.0, 0.0, 0.996613922337, -0.00577054766522, 0.0, 0.0, 1.42044250221, 1.32216599481;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeElevatorPlant() {
+ ::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>(MakeElevatorPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeElevatorLoop() {
+ ::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>(MakeElevatorController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/fridge/elevator_motor_plant.h b/y2015/control_loops/fridge/elevator_motor_plant.h
new file mode 100644
index 0000000..e68e6d7
--- /dev/null
+++ b/y2015/control_loops/fridge/elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
+#define Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeElevatorController();
+
+StateFeedbackPlant<4, 2, 2> MakeElevatorPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeElevatorLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
new file mode 100644
index 0000000..083baf2
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -0,0 +1,720 @@
+#include "y2015/control_loops/fridge/fridge.h"
+
+#include <cmath>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "y2015/control_loops/fridge/elevator_motor_plant.h"
+#include "y2015/control_loops/fridge/integral_arm_plant.h"
+#include "frc971/control_loops/voltage_cap/voltage_cap.h"
+#include "frc971/zeroing/zeroing.h"
+
+#include "y2015/constants.h"
+
+namespace frc971 {
+namespace control_loops {
+
+namespace {
+constexpr double kZeroingVoltage = 4.0;
+constexpr double kElevatorZeroingVelocity = 0.10;
+// What speed we move to our safe height at.
+constexpr double kElevatorSafeHeightVelocity = 0.3;
+constexpr double kArmZeroingVelocity = 0.20;
+} // namespace
+
+template <int S>
+void CappedStateFeedbackLoop<S>::CapU() {
+ VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
+ &this->mutable_U(1, 0));
+}
+
+template <int S>
+Eigen::Matrix<double, 2, 1>
+CappedStateFeedbackLoop<S>::UnsaturateOutputGoalChange() {
+ // Compute the K matrix used to compensate for position errors.
+ Eigen::Matrix<double, 2, 2> Kp;
+ Kp.setZero();
+ Kp.col(0) = this->K().col(0);
+ Kp.col(1) = this->K().col(2);
+
+ Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
+
+ // Compute how much we need to change R in order to achieve the change in U
+ // that was observed.
+ Eigen::Matrix<double, 2, 1> deltaR =
+ -Kp_inv * (this->U_uncapped() - this->U());
+ return deltaR;
+}
+
+Fridge::Fridge(control_loops::FridgeQueue *fridge)
+ : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
+ arm_loop_(new CappedStateFeedbackLoop<5>(
+ StateFeedbackLoop<5, 2, 2>(MakeIntegralArmLoop()))),
+ elevator_loop_(new CappedStateFeedbackLoop<4>(
+ StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
+ left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
+ right_arm_estimator_(constants::GetValues().fridge.right_arm_zeroing),
+ left_elevator_estimator_(constants::GetValues().fridge.left_elev_zeroing),
+ right_elevator_estimator_(
+ constants::GetValues().fridge.right_elev_zeroing),
+ last_profiling_type_(ProfilingType::ANGLE_HEIGHT_PROFILING),
+ kinematics_(constants::GetValues().fridge.arm_length,
+ constants::GetValues().fridge.elevator.upper_limit,
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.arm.upper_limit,
+ constants::GetValues().fridge.arm.lower_limit),
+ arm_profile_(::aos::controls::kLoopFrequency),
+ elevator_profile_(::aos::controls::kLoopFrequency),
+ x_profile_(::aos::controls::kLoopFrequency),
+ y_profile_(::aos::controls::kLoopFrequency) {}
+
+void Fridge::UpdateZeroingState() {
+ if (left_elevator_estimator_.offset_ratio_ready() < 1.0 ||
+ right_elevator_estimator_.offset_ratio_ready() < 1.0 ||
+ left_arm_estimator_.offset_ratio_ready() < 1.0 ||
+ right_arm_estimator_.offset_ratio_ready() < 1.0) {
+ state_ = INITIALIZING;
+ } else if (!left_elevator_estimator_.zeroed() ||
+ !right_elevator_estimator_.zeroed()) {
+ state_ = ZEROING_ELEVATOR;
+ } else if (!left_arm_estimator_.zeroed() || !right_arm_estimator_.zeroed()) {
+ state_ = ZEROING_ARM;
+ } else {
+ state_ = RUNNING;
+ }
+}
+
+void Fridge::Correct() {
+ {
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left_elevator(), right_elevator();
+ elevator_loop_->Correct(Y);
+ }
+
+ {
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left_arm(), right_arm();
+ arm_loop_->Correct(Y);
+ }
+}
+
+void Fridge::SetElevatorOffset(double left_offset, double right_offset) {
+ LOG(INFO, "Changing Elevator offset from %f, %f to %f, %f\n",
+ left_elevator_offset_, right_elevator_offset_, left_offset, right_offset);
+ double left_doffset = left_offset - left_elevator_offset_;
+ double right_doffset = right_offset - right_elevator_offset_;
+
+ // Adjust the average height and height difference between the two sides.
+ // The derivatives of both should not need to be updated since the speeds
+ // haven't changed.
+ // The height difference is calculated as left - right, not right - left.
+ elevator_loop_->mutable_X_hat(0, 0) += (left_doffset + right_doffset) / 2;
+ elevator_loop_->mutable_X_hat(2, 0) += (left_doffset - right_doffset) / 2;
+
+ // Modify the zeroing goal.
+ elevator_goal_ += (left_doffset + right_doffset) / 2;
+
+ // Update the cached offset values to the actual values.
+ left_elevator_offset_ = left_offset;
+ right_elevator_offset_ = right_offset;
+}
+
+void Fridge::SetArmOffset(double left_offset, double right_offset) {
+ LOG(INFO, "Changing Arm offset from %f, %f to %f, %f\n", left_arm_offset_,
+ right_arm_offset_, left_offset, right_offset);
+ double left_doffset = left_offset - left_arm_offset_;
+ double right_doffset = right_offset - right_arm_offset_;
+
+ // Adjust the average angle and angle difference between the two sides.
+ // The derivatives of both should not need to be updated since the speeds
+ // haven't changed.
+ arm_loop_->mutable_X_hat(0, 0) += (left_doffset + right_doffset) / 2;
+ arm_loop_->mutable_X_hat(2, 0) += (left_doffset - right_doffset) / 2;
+
+ // Modify the zeroing goal.
+ arm_goal_ += (left_doffset + right_doffset) / 2;
+
+ // Update the cached offset values to the actual values.
+ left_arm_offset_ = left_offset;
+ right_arm_offset_ = right_offset;
+}
+
+double Fridge::estimated_left_elevator() {
+ return current_position_.elevator.left.encoder +
+ left_elevator_estimator_.offset();
+}
+double Fridge::estimated_right_elevator() {
+ return current_position_.elevator.right.encoder +
+ right_elevator_estimator_.offset();
+}
+
+double Fridge::estimated_elevator() {
+ return (estimated_left_elevator() + estimated_right_elevator()) / 2.0;
+}
+
+double Fridge::estimated_left_arm() {
+ return current_position_.arm.left.encoder + left_arm_estimator_.offset();
+}
+double Fridge::estimated_right_arm() {
+ return current_position_.arm.right.encoder + right_arm_estimator_.offset();
+}
+double Fridge::estimated_arm() {
+ return (estimated_left_arm() + estimated_right_arm()) / 2.0;
+}
+
+double Fridge::left_elevator() {
+ return current_position_.elevator.left.encoder + left_elevator_offset_;
+}
+double Fridge::right_elevator() {
+ return current_position_.elevator.right.encoder + right_elevator_offset_;
+}
+
+double Fridge::elevator() { return (left_elevator() + right_elevator()) / 2.0; }
+
+double Fridge::left_arm() {
+ return current_position_.arm.left.encoder + left_arm_offset_;
+}
+double Fridge::right_arm() {
+ return current_position_.arm.right.encoder + right_arm_offset_;
+}
+double Fridge::arm() { return (left_arm() + right_arm()) / 2.0; }
+
+double Fridge::elevator_zeroing_velocity() {
+ double average_elevator =
+ (constants::GetValues().fridge.elevator.lower_limit +
+ constants::GetValues().fridge.elevator.upper_limit) /
+ 2.0;
+
+ const double pulse_width = ::std::max(
+ constants::GetValues().fridge.left_elev_zeroing.index_difference,
+ constants::GetValues().fridge.right_elev_zeroing.index_difference);
+
+ if (elevator_zeroing_velocity_ == 0) {
+ if (estimated_elevator() > average_elevator) {
+ elevator_zeroing_velocity_ = -kElevatorZeroingVelocity;
+ } else {
+ elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
+ }
+ } else if (elevator_zeroing_velocity_ > 0 &&
+ estimated_elevator() > average_elevator + 1.1 * pulse_width) {
+ elevator_zeroing_velocity_ = -kElevatorZeroingVelocity;
+ } else if (elevator_zeroing_velocity_ < 0 &&
+ estimated_elevator() < average_elevator - 1.1 * pulse_width) {
+ elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
+ }
+ return elevator_zeroing_velocity_;
+}
+
+double Fridge::UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
+double Fridge::arm_zeroing_velocity() {
+ const double average_arm = (constants::GetValues().fridge.arm.lower_limit +
+ constants::GetValues().fridge.arm.upper_limit) /
+ 2.0;
+ const double pulse_width = ::std::max(
+ constants::GetValues().fridge.right_arm_zeroing.index_difference,
+ constants::GetValues().fridge.left_arm_zeroing.index_difference);
+
+ if (arm_zeroing_velocity_ == 0) {
+ if (estimated_arm() > average_arm) {
+ arm_zeroing_velocity_ = -kArmZeroingVelocity;
+ } else {
+ arm_zeroing_velocity_ = kArmZeroingVelocity;
+ }
+ } else if (arm_zeroing_velocity_ > 0.0 &&
+ estimated_arm() > average_arm + 1.1 * pulse_width) {
+ arm_zeroing_velocity_ = -kArmZeroingVelocity;
+ } else if (arm_zeroing_velocity_ < 0.0 && estimated_arm() < average_arm) {
+ arm_zeroing_velocity_ = kArmZeroingVelocity;
+ }
+ return arm_zeroing_velocity_;
+}
+
+void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *unsafe_goal,
+ const control_loops::FridgeQueue::Position *position,
+ control_loops::FridgeQueue::Output *output,
+ control_loops::FridgeQueue::Status *status) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ left_elevator_estimator_.Reset();
+ right_elevator_estimator_.Reset();
+ left_arm_estimator_.Reset();
+ right_arm_estimator_.Reset();
+ state_ = UNINITIALIZED;
+ }
+
+ // Get a reference to the constants struct since we use it so often in this
+ // code.
+ const auto &values = constants::GetValues();
+
+ // Bool to track if we should turn the motors on or not.
+ bool disable = output == nullptr;
+
+ // Save the current position so it can be used easily in the class.
+ current_position_ = *position;
+
+ left_elevator_estimator_.UpdateEstimate(position->elevator.left);
+ right_elevator_estimator_.UpdateEstimate(position->elevator.right);
+ left_arm_estimator_.UpdateEstimate(position->arm.left);
+ right_arm_estimator_.UpdateEstimate(position->arm.right);
+
+ if (state_ != UNINITIALIZED) {
+ Correct();
+ }
+
+ // Zeroing will work as follows:
+ // At startup, record the offset of the two halves of the two subsystems.
+ // Then, start moving the elevator towards the center until both halves are
+ // zeroed.
+ // Then, start moving the claw towards the center until both halves are
+ // zeroed.
+ // Then, done!
+
+ // We'll then need code to do sanity checking on values.
+
+ // Now, we need to figure out which way to go.
+
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(DEBUG, "Uninitialized\n");
+ // Startup. Assume that we are at the origin everywhere.
+ // This records the encoder offset between the two sides of the elevator.
+ left_elevator_offset_ = -position->elevator.left.encoder;
+ right_elevator_offset_ = -position->elevator.right.encoder;
+ left_arm_offset_ = -position->arm.left.encoder;
+ right_arm_offset_ = -position->arm.right.encoder;
+ elevator_loop_->mutable_X_hat().setZero();
+ arm_loop_->mutable_X_hat().setZero();
+ LOG(INFO, "Initializing arm offsets to %f, %f\n", left_arm_offset_,
+ right_arm_offset_);
+ LOG(INFO, "Initializing elevator offsets to %f, %f\n",
+ left_elevator_offset_, right_elevator_offset_);
+ Correct();
+ state_ = INITIALIZING;
+ disable = true;
+ break;
+
+ case INITIALIZING:
+ LOG(DEBUG, "Waiting for accurate initial position.\n");
+ disable = true;
+ // Update state_ to accurately represent the state of the zeroing
+ // estimators.
+ UpdateZeroingState();
+ if (state_ != INITIALIZING) {
+ // Set the goals to where we are now.
+ elevator_goal_ = elevator();
+ arm_goal_ = arm();
+ }
+ break;
+
+ case ZEROING_ELEVATOR:
+ LOG(DEBUG, "Zeroing elevator\n");
+
+ // Update state_ to accurately represent the state of the zeroing
+ // estimators.
+ UpdateZeroingState();
+ if (left_elevator_estimator_.zeroed() &&
+ right_elevator_estimator_.zeroed()) {
+ SetElevatorOffset(left_elevator_estimator_.offset(),
+ right_elevator_estimator_.offset());
+ LOG(DEBUG, "Zeroed the elevator!\n");
+
+ if (elevator() < values.fridge.arm_zeroing_height &&
+ state_ != INITIALIZING) {
+ // Move the elevator to a safe height before we start zeroing the arm,
+ // so that we don't crash anything.
+ LOG(DEBUG, "Moving elevator to safe height.\n");
+ if (elevator_goal_ < values.fridge.arm_zeroing_height) {
+ elevator_goal_ += kElevatorSafeHeightVelocity *
+ ::aos::controls::kLoopFrequency.ToSeconds();
+ elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
+ state_ = ZEROING_ELEVATOR;
+ } else {
+ // We want it stopped at whatever height it's currently set to.
+ elevator_goal_velocity_ = 0;
+ }
+ }
+ } else if (!disable) {
+ elevator_goal_velocity_ = elevator_zeroing_velocity();
+ elevator_goal_ += elevator_goal_velocity_ *
+ ::aos::controls::kLoopFrequency.ToSeconds();
+ }
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the elevator is zeroed and
+ // we reach into the elevator's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << elevator_goal_, elevator_goal_velocity_;
+ elevator_profile_.MoveCurrentState(current);
+ }
+ break;
+
+ case ZEROING_ARM:
+ LOG(DEBUG, "Zeroing the arm\n");
+
+ if (elevator() < values.fridge.arm_zeroing_height - 0.10 ||
+ elevator_goal_ < values.fridge.arm_zeroing_height) {
+ LOG(INFO,
+ "Going back to ZEROING_ELEVATOR until it gets high enough to "
+ "safely zero the arm\n");
+ state_ = ZEROING_ELEVATOR;
+ break;
+ }
+
+ // Update state_ to accurately represent the state of the zeroing
+ // estimators.
+ UpdateZeroingState();
+ if (left_arm_estimator_.zeroed() && right_arm_estimator_.zeroed()) {
+ SetArmOffset(left_arm_estimator_.offset(),
+ right_arm_estimator_.offset());
+ LOG(DEBUG, "Zeroed the arm!\n");
+ } else if (!disable) {
+ arm_goal_velocity_ = arm_zeroing_velocity();
+ arm_goal_ +=
+ arm_goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ }
+
+ // Bypass motion profiles while we are zeroing.
+ // This is also an important step right after the arm is zeroed and
+ // we reach into the arm's state matrix and change it based on the
+ // newly-obtained offset.
+ {
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << arm_goal_, arm_goal_velocity_;
+ arm_profile_.MoveCurrentState(current);
+ }
+ break;
+
+ case RUNNING:
+ LOG(DEBUG, "Running!\n");
+ if (unsafe_goal) {
+ // Handle the case where we switch between the types of profiling.
+ ProfilingType new_profiling_type =
+ static_cast<ProfilingType>(unsafe_goal->profiling_type);
+
+ if (last_profiling_type_ != new_profiling_type) {
+ // Reset the height/angle profiles.
+ Eigen::Matrix<double, 2, 1> current;
+ current.setZero();
+ current << arm_goal_, arm_goal_velocity_;
+ arm_profile_.MoveCurrentState(current);
+ current << elevator_goal_, elevator_goal_velocity_;
+ elevator_profile_.MoveCurrentState(current);
+
+ // Reset the x/y profiles.
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_result;
+ kinematics_.ForwardKinematic(elevator_goal_, arm_goal_,
+ elevator_goal_velocity_,
+ arm_goal_velocity_, &x_y_result);
+ current << x_y_result.fridge_x, x_y_result.fridge_x_velocity;
+ x_profile_.MoveCurrentState(current);
+ current << x_y_result.fridge_h, x_y_result.fridge_h_velocity;
+ y_profile_.MoveCurrentState(current);
+
+ last_profiling_type_ = new_profiling_type;
+ }
+
+ if (new_profiling_type == ProfilingType::ANGLE_HEIGHT_PROFILING) {
+ // Pick a set of sane arm defaults if none are specified.
+ arm_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_angular_velocity, 1.0));
+ arm_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_angular_acceleration, 3.0));
+ elevator_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_velocity, 0.50));
+ elevator_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_acceleration, 2.0));
+
+ // Use the profiles to limit the arm's movements.
+ const double unfiltered_arm_goal = ::std::max(
+ ::std::min(unsafe_goal->angle, values.fridge.arm.upper_limit),
+ values.fridge.arm.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> arm_goal_state = arm_profile_.Update(
+ unfiltered_arm_goal, unsafe_goal->angular_velocity);
+ arm_goal_ = arm_goal_state(0, 0);
+ arm_goal_velocity_ = arm_goal_state(1, 0);
+
+ // Use the profiles to limit the elevator's movements.
+ const double unfiltered_elevator_goal =
+ ::std::max(::std::min(unsafe_goal->height,
+ values.fridge.elevator.upper_limit),
+ values.fridge.elevator.lower_limit);
+ ::Eigen::Matrix<double, 2, 1> elevator_goal_state =
+ elevator_profile_.Update(unfiltered_elevator_goal,
+ unsafe_goal->velocity);
+ elevator_goal_ = elevator_goal_state(0, 0);
+ elevator_goal_velocity_ = elevator_goal_state(1, 0);
+ } else if (new_profiling_type == ProfilingType::X_Y_PROFILING) {
+ // Use x/y profiling
+ aos::util::ElevatorArmKinematics::KinematicResult kinematic_result;
+
+ x_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_x_velocity, 0.5));
+ x_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_x_acceleration, 2.0));
+ y_profile_.set_maximum_velocity(
+ UseUnlessZero(unsafe_goal->max_y_velocity, 0.50));
+ y_profile_.set_maximum_acceleration(
+ UseUnlessZero(unsafe_goal->max_y_acceleration, 2.0));
+
+ // Limit the goals before we update the profiles.
+ kinematics_.InverseKinematic(
+ unsafe_goal->x, unsafe_goal->y, unsafe_goal->x_velocity,
+ unsafe_goal->y_velocity, &kinematic_result);
+
+ // Use the profiles to limit the x movements.
+ ::Eigen::Matrix<double, 2, 1> x_goal_state = x_profile_.Update(
+ kinematic_result.fridge_x, kinematic_result.fridge_x_velocity);
+
+ // Use the profiles to limit the y movements.
+ ::Eigen::Matrix<double, 2, 1> y_goal_state = y_profile_.Update(
+ kinematic_result.fridge_h, kinematic_result.fridge_h_velocity);
+
+ // Convert x/y goal states into arm/elevator goals.
+ // The inverse kinematics functions automatically perform range
+ // checking and adjust the results so that they're always valid.
+ kinematics_.InverseKinematic(x_goal_state(0, 0), y_goal_state(0, 0),
+ x_goal_state(1, 0), y_goal_state(1, 0),
+ &kinematic_result);
+
+ // Store the appropriate inverse kinematic results in the
+ // arm/elevator goals.
+ arm_goal_ = kinematic_result.arm_angle;
+ arm_goal_velocity_ = kinematic_result.arm_velocity;
+
+ elevator_goal_ = kinematic_result.elevator_height;
+ elevator_goal_velocity_ = kinematic_result.arm_velocity;
+ } else {
+ LOG(ERROR, "Unknown profiling_type: %d\n",
+ unsafe_goal->profiling_type);
+ }
+ }
+
+ // Update state_ to accurately represent the state of the zeroing
+ // estimators.
+ UpdateZeroingState();
+
+ if (state_ != RUNNING && state_ != ESTOP) {
+ state_ = UNINITIALIZED;
+ }
+ break;
+
+ case ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Commence death if either left/right tracking error gets too big. This
+ // should run immediately after the SetArmOffset and SetElevatorOffset
+ // functions to double-check that the hardware is in a sane state.
+ if (::std::abs(left_arm() - right_arm()) >=
+ values.max_allowed_left_right_arm_difference) {
+ LOG(ERROR, "The arms are too far apart. |%f - %f| > %f\n", left_arm(),
+ right_arm(), values.max_allowed_left_right_arm_difference);
+
+ // Indicate an ESTOP condition and stop the motors.
+ if (output) {
+ state_ = ESTOP;
+ }
+ disable = true;
+ }
+
+ if (::std::abs(left_elevator() - right_elevator()) >=
+ values.max_allowed_left_right_elevator_difference) {
+ LOG(ERROR, "The elevators are too far apart. |%f - %f| > %f\n",
+ left_elevator(), right_elevator(),
+ values.max_allowed_left_right_elevator_difference);
+
+ // Indicate an ESTOP condition and stop the motors.
+ if (output) {
+ state_ = ESTOP;
+ }
+ disable = true;
+ }
+
+ // Limit the goals so we can't exceed the hardware limits if we are RUNNING.
+ if (state_ == RUNNING) {
+ // Limit the arm goal to min/max allowable angles.
+ if (arm_goal_ >= values.fridge.arm.upper_limit) {
+ LOG(WARNING, "Arm goal above limit, %f > %f\n", arm_goal_,
+ values.fridge.arm.upper_limit);
+ arm_goal_ = values.fridge.arm.upper_limit;
+ }
+ if (arm_goal_ <= values.fridge.arm.lower_limit) {
+ LOG(WARNING, "Arm goal below limit, %f < %f\n", arm_goal_,
+ values.fridge.arm.lower_limit);
+ arm_goal_ = values.fridge.arm.lower_limit;
+ }
+
+ // Limit the elevator goal to min/max allowable heights.
+ if (elevator_goal_ >= values.fridge.elevator.upper_limit) {
+ LOG(WARNING, "Elevator goal above limit, %f > %f\n", elevator_goal_,
+ values.fridge.elevator.upper_limit);
+ elevator_goal_ = values.fridge.elevator.upper_limit;
+ }
+ if (elevator_goal_ <= values.fridge.elevator.lower_limit) {
+ LOG(WARNING, "Elevator goal below limit, %f < %f\n", elevator_goal_,
+ values.fridge.elevator.lower_limit);
+ elevator_goal_ = values.fridge.elevator.lower_limit;
+ }
+ }
+
+ // Check the lower level hardware limit as well.
+ if (state_ == RUNNING) {
+ if (left_arm() >= values.fridge.arm.upper_hard_limit ||
+ left_arm() <= values.fridge.arm.lower_hard_limit) {
+ LOG(ERROR, "Left arm at %f out of bounds [%f, %f], ESTOPing\n",
+ left_arm(), values.fridge.arm.lower_hard_limit,
+ values.fridge.arm.upper_hard_limit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+
+ if (right_arm() >= values.fridge.arm.upper_hard_limit ||
+ right_arm() <= values.fridge.arm.lower_hard_limit) {
+ LOG(ERROR, "Right arm at %f out of bounds [%f, %f], ESTOPing\n",
+ right_arm(), values.fridge.arm.lower_hard_limit,
+ values.fridge.arm.upper_hard_limit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+
+ if (left_elevator() >= values.fridge.elevator.upper_hard_limit) {
+ LOG(ERROR, "Left elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ left_elevator(), values.fridge.elevator.lower_hard_limit,
+ values.fridge.elevator.upper_hard_limit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+
+ if (right_elevator() >= values.fridge.elevator.upper_hard_limit) {
+ LOG(ERROR, "Right elevator at %f out of bounds [%f, %f], ESTOPing\n",
+ right_elevator(), values.fridge.elevator.lower_hard_limit,
+ values.fridge.elevator.upper_hard_limit);
+ if (output) {
+ state_ = ESTOP;
+ }
+ }
+ }
+
+ // Set the goals.
+ arm_loop_->mutable_R() << arm_goal_, arm_goal_velocity_, 0.0, 0.0, 0.0;
+ elevator_loop_->mutable_R() << elevator_goal_, elevator_goal_velocity_, 0.0,
+ 0.0;
+
+ const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+ arm_loop_->set_max_voltage(max_voltage);
+ elevator_loop_->set_max_voltage(max_voltage);
+
+ if (state_ == ESTOP) {
+ disable = true;
+ }
+ arm_loop_->Update(disable);
+ elevator_loop_->Update(disable);
+
+ if (state_ == INITIALIZING || state_ == ZEROING_ELEVATOR ||
+ state_ == ZEROING_ARM) {
+ if (arm_loop_->U() != arm_loop_->U_uncapped()) {
+ Eigen::Matrix<double, 2, 1> deltaR =
+ arm_loop_->UnsaturateOutputGoalChange();
+
+ // Move the average arm goal by the amount observed.
+ LOG(WARNING, "Moving arm goal by %f to handle saturation\n",
+ deltaR(0, 0));
+ arm_goal_ += deltaR(0, 0);
+ }
+
+ if (elevator_loop_->U() != elevator_loop_->U_uncapped()) {
+ Eigen::Matrix<double, 2, 1> deltaR =
+ elevator_loop_->UnsaturateOutputGoalChange();
+
+ // Move the average elevator goal by the amount observed.
+ LOG(WARNING, "Moving elevator goal by %f to handle saturation\n",
+ deltaR(0, 0));
+ elevator_goal_ += deltaR(0, 0);
+ }
+ }
+
+ if (output) {
+ output->left_arm = arm_loop_->U(0, 0);
+ output->right_arm = arm_loop_->U(1, 0);
+ output->left_elevator = elevator_loop_->U(0, 0);
+ output->right_elevator = elevator_loop_->U(1, 0);
+ if (unsafe_goal) {
+ output->grabbers = unsafe_goal->grabbers;
+ } else {
+ output->grabbers.top_front = false;
+ output->grabbers.top_back = false;
+ output->grabbers.bottom_front = false;
+ output->grabbers.bottom_back = false;
+ }
+ }
+
+ // TODO(austin): Populate these fully.
+ status->zeroed = state_ == RUNNING;
+
+ status->angle = arm_loop_->X_hat(0, 0);
+ status->angular_velocity = arm_loop_->X_hat(1, 0);
+ status->height = elevator_loop_->X_hat(0, 0);
+ status->velocity = elevator_loop_->X_hat(1, 0);
+
+ status->goal_angle = arm_goal_;
+ status->goal_angular_velocity = arm_goal_velocity_;
+ status->goal_height = elevator_goal_;
+ status->goal_velocity = elevator_goal_velocity_;
+
+ // Populate the same status, but in X/Y co-ordinates.
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_status;
+ kinematics_.ForwardKinematic(status->height, status->angle,
+ status->velocity, status->angular_velocity,
+ &x_y_status);
+ status->x = x_y_status.fridge_x;
+ status->y = x_y_status.fridge_h;
+ status->x_velocity = x_y_status.fridge_x_velocity;
+ status->y_velocity = x_y_status.fridge_h_velocity;
+
+ kinematics_.ForwardKinematic(status->goal_height, status->goal_angle,
+ status->goal_velocity, status->goal_angular_velocity,
+ &x_y_status);
+ status->goal_x = x_y_status.fridge_x;
+ status->goal_y = x_y_status.fridge_h;
+ status->goal_x_velocity = x_y_status.fridge_x_velocity;
+ status->goal_y_velocity = x_y_status.fridge_h_velocity;
+
+ if (unsafe_goal) {
+ status->grabbers = unsafe_goal->grabbers;
+ } else {
+ status->grabbers.top_front = false;
+ status->grabbers.top_back = false;
+ status->grabbers.bottom_front = false;
+ status->grabbers.bottom_back = false;
+ }
+ zeroing::PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
+ zeroing::PopulateEstimatorState(right_arm_estimator_,
+ &status->right_arm_state);
+ zeroing::PopulateEstimatorState(left_elevator_estimator_,
+ &status->left_elevator_state);
+ zeroing::PopulateEstimatorState(right_elevator_estimator_,
+ &status->right_elevator_state);
+ status->estopped = (state_ == ESTOP);
+ status->state = state_;
+ last_state_ = state_;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/fridge/fridge.gyp b/y2015/control_loops/fridge/fridge.gyp
new file mode 100644
index 0000000..76844c4
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge.gyp
@@ -0,0 +1,89 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'replay_fridge',
+ 'type': 'executable',
+ 'variables': {
+ 'no_rsync': 1,
+ },
+ 'sources': [
+ 'replay_fridge.cc',
+ ],
+ 'dependencies': [
+ 'fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:replay_control_loop',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'fridge_queue',
+ 'type': 'static_library',
+ 'sources': ['fridge.q'],
+ 'variables': {
+ 'header_path': 'y2015/control_loops/fridge',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ '<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'fridge_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'fridge.cc',
+ 'integral_arm_plant.cc',
+ 'elevator_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'fridge_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'fridge_lib_test.cc',
+ 'arm_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'fridge_lib',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ ],
+ },
+ {
+ 'target_name': 'fridge',
+ 'type': 'executable',
+ 'sources': [
+ 'fridge_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'fridge_lib',
+ ],
+ },
+ ],
+}
diff --git a/y2015/control_loops/fridge/fridge.h b/y2015/control_loops/fridge/fridge.h
new file mode 100644
index 0000000..0448e09
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge.h
@@ -0,0 +1,172 @@
+#ifndef Y2015_CONTROL_LOOPS_FRIDGE_H_
+#define Y2015_CONTROL_LOOPS_FRIDGE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2015/util/kinematics.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class FridgeTest_DisabledGoalTest_Test;
+class FridgeTest_ArmGoalPositiveWindupTest_Test;
+class FridgeTest_ElevatorGoalPositiveWindupTest_Test;
+class FridgeTest_ArmGoalNegativeWindupTest_Test;
+class FridgeTest_ElevatorGoalNegativeWindupTest_Test;
+class FridgeTest_SafeArmZeroing_Test;
+} // namespace testing
+
+template<int S>
+class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
+ public:
+ CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
+ : StateFeedbackLoop<S, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+ }
+
+ void CapU() override;
+
+ // Returns the amount to change the position goals (average and difference) in
+ // order to no longer saturate the controller.
+ Eigen::Matrix<double, 2, 1> UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
+
+class Fridge
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
+ public:
+ explicit Fridge(
+ control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
+
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Estimating the starting location.
+ INITIALIZING = 1,
+ // Moving the elevator to find an index pulse.
+ ZEROING_ELEVATOR = 2,
+ // Moving the arm to find an index pulse.
+ ZEROING_ARM = 3,
+ // All good!
+ RUNNING = 4,
+ // Internal error caused the fridge to abort.
+ ESTOP = 5,
+ };
+
+ enum class ProfilingType : int32_t {
+ // Use angle/height to specify the fridge goal.
+ ANGLE_HEIGHT_PROFILING = 0,
+ // Use x/y co-ordinates to specify the fridge goal.
+ X_Y_PROFILING = 1,
+ };
+
+ State state() const { return state_; }
+
+ protected:
+ void RunIteration(const control_loops::FridgeQueue::Goal *goal,
+ const control_loops::FridgeQueue::Position *position,
+ control_loops::FridgeQueue::Output *output,
+ control_loops::FridgeQueue::Status *status) override;
+
+ private:
+ friend class testing::FridgeTest_DisabledGoalTest_Test;
+ friend class testing::FridgeTest_ElevatorGoalPositiveWindupTest_Test;
+ friend class testing::FridgeTest_ArmGoalPositiveWindupTest_Test;
+ friend class testing::FridgeTest_ElevatorGoalNegativeWindupTest_Test;
+ friend class testing::FridgeTest_ArmGoalNegativeWindupTest_Test;
+ friend class testing::FridgeTest_SafeArmZeroing_Test;
+
+ // Sets state_ to the correct state given the current state of the zeroing
+ // estimators.
+ void UpdateZeroingState();
+
+ void SetElevatorOffset(double left_offset, double right_offset);
+ void SetArmOffset(double left_offset, double right_offset);
+
+ // Getters for the current elevator positions.
+ double left_elevator();
+ double right_elevator();
+ double elevator();
+
+ // Getters for the current arm positions.
+ double left_arm();
+ double right_arm();
+ double arm();
+
+ // Our best guess at the current position of the elevator.
+ double estimated_left_elevator();
+ double estimated_right_elevator();
+ double estimated_elevator();
+
+ // Our best guess at the current position of the arm.
+ double estimated_left_arm();
+ double estimated_right_arm();
+ double estimated_arm();
+
+ // Returns the current zeroing velocity for either subsystem.
+ // If the subsystem is too far away from the center, these will switch
+ // directions.
+ double elevator_zeroing_velocity();
+ double arm_zeroing_velocity();
+
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ double UseUnlessZero(double target_value, double default_value);
+
+ // The state feedback control loop or loops to talk to.
+ ::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
+
+ zeroing::ZeroingEstimator left_arm_estimator_;
+ zeroing::ZeroingEstimator right_arm_estimator_;
+ zeroing::ZeroingEstimator left_elevator_estimator_;
+ zeroing::ZeroingEstimator right_elevator_estimator_;
+
+ // Offsets from the encoder position to the absolute position. Add these to
+ // the encoder position to get the absolute position.
+ double left_elevator_offset_ = 0.0;
+ double right_elevator_offset_ = 0.0;
+ double left_arm_offset_ = 0.0;
+ double right_arm_offset_ = 0.0;
+
+ // Current velocity to move at while zeroing.
+ double elevator_zeroing_velocity_ = 0.0;
+ double arm_zeroing_velocity_ = 0.0;
+
+ // The goals for the elevator and arm.
+ double elevator_goal_ = 0.0;
+ double arm_goal_ = 0.0;
+
+ double arm_goal_velocity_ = 0.0;
+ double elevator_goal_velocity_ = 0.0;
+
+ State state_ = UNINITIALIZED;
+ State last_state_ = UNINITIALIZED;
+
+ control_loops::FridgeQueue::Position current_position_;
+
+ ProfilingType last_profiling_type_;
+ aos::util::ElevatorArmKinematics kinematics_;
+
+ aos::util::TrapezoidProfile arm_profile_;
+ aos::util::TrapezoidProfile elevator_profile_;
+
+ aos::util::TrapezoidProfile x_profile_;
+ aos::util::TrapezoidProfile y_profile_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
+
diff --git a/y2015/control_loops/fridge/fridge.q b/y2015/control_loops/fridge/fridge.q
new file mode 100644
index 0000000..257374d
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge.q
@@ -0,0 +1,152 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+// Represents states for all of the box-grabbing pistons.
+// true is grabbed and false is retracted for all of them.
+struct GrabberPistons {
+ bool top_front;
+ bool top_back;
+ bool bottom_front;
+ bool bottom_back;
+};
+
+queue_group FridgeQueue {
+ implements aos.control_loops.ControlLoop;
+
+ // All angles are in radians with 0 sticking straight up.
+ // Rotating up and into the robot (towards the back
+ // where boxes are placed) is positive. Positive output voltage moves all
+ // mechanisms in the direction with positive sensor values.
+
+ // Elevator heights are the vertical distance (in meters) from a defined zero.
+ // In this case, zero is where the portion of the carriage that Spencer
+ // removed lines up with the bolt.
+
+ // X/Y positions are distances (in meters) the fridge is away from its origin
+ // position. Origin is considered at a height of zero and an angle of zero.
+ // Positive X positions are towards the front of the robot and negative X is
+ // towards the back of the robot (which is where we score).
+ // Y is positive going up and negative when it goes below the origin.
+
+ message Goal {
+ // Profile type.
+ // Set to 0 for angle/height profiling.
+ // Set to 1 for x/y profiling.
+ int32_t profiling_type;
+
+ // Angle of the arm.
+ double angle;
+ // Height of the elevator.
+ double height;
+
+ // Angular velocity of the arm.
+ float angular_velocity;
+ // Linear velocity of the elevator.
+ float velocity;
+
+ // Maximum arm profile angular velocity or 0 for the default.
+ float max_angular_velocity;
+ // Maximum elevator profile velocity or 0 for the default.
+ float max_velocity;
+
+ // Maximum arm profile acceleration or 0 for the default.
+ float max_angular_acceleration;
+ // Maximum elevator profile acceleration or 0 for the default.
+ float max_acceleration;
+
+ // X position of the fridge.
+ double x;
+ // Y position of the fridge.
+ double y;
+
+ // Velocity of the x position of the fridge.
+ float x_velocity;
+ // Velocity of the y position of the fridge.
+ float y_velocity;
+
+ // Maximum x profile velocity or 0 for the default.
+ float max_x_velocity;
+ // Maximum y profile velocity or 0 for the default.
+ float max_y_velocity;
+
+ // Maximum x profile acceleration or 0 for the default.
+ float max_x_acceleration;
+ // Maximum y profile acceleration or 0 for the default.
+ float max_y_acceleration;
+
+ // TODO(austin): Do I need acceleration here too?
+
+ GrabberPistons grabbers;
+ };
+
+ message Position {
+ PotAndIndexPair arm;
+ PotAndIndexPair elevator;
+ };
+
+ message Status {
+ // Are both the arm and elevator zeroed?
+ bool zeroed;
+
+ // Estimated angle of the arm.
+ double angle;
+ // Estimated angular velocity of the arm.
+ float angular_velocity;
+ // Estimated height of the elevator.
+ double height;
+ // Estimated velocity of the elvator.
+ float velocity;
+ // state of the grabber pistons
+ GrabberPistons grabbers;
+
+ // Goal angle and velocity of the arm.
+ double goal_angle;
+ float goal_angular_velocity;
+ // Goal height and velocity of the elevator.
+ double goal_height;
+ float goal_velocity;
+
+ // Estimated X/Y position of the fridge.
+ // These are translated directly from the height/angle statuses.
+ double x;
+ double y;
+ float x_velocity;
+ float y_velocity;
+
+ // X/Y goals of the fridge.
+ // These are translated directly from the height/angle goals.
+ double goal_x;
+ double goal_y;
+ float goal_x_velocity;
+ float goal_y_velocity;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // The internal state of the state machine.
+ int32_t state;
+
+ EstimatorState left_elevator_state;
+ EstimatorState right_elevator_state;
+ EstimatorState left_arm_state;
+ EstimatorState right_arm_state;
+ };
+
+ message Output {
+ double left_arm;
+ double right_arm;
+ double left_elevator;
+ double right_elevator;
+
+ GrabberPistons grabbers;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group FridgeQueue fridge_queue;
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
new file mode 100644
index 0000000..7719fb4
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -0,0 +1,735 @@
+#include "y2015/control_loops/fridge/fridge.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2015/util/kinematics.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "y2015/control_loops/fridge/arm_motor_plant.h"
+#include "y2015/control_loops/fridge/elevator_motor_plant.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+#include "y2015/constants.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+// Class which simulates the fridge and sends out queue messages with the
+// position.
+class FridgeSimulation {
+ public:
+ static constexpr double kNoiseScalar = 0.1;
+ // Constructs a simulation.
+ FridgeSimulation()
+ : arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
+ elevator_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
+ left_arm_pot_encoder_(
+ constants::GetValues().fridge.left_arm_zeroing.index_difference),
+ right_arm_pot_encoder_(
+ constants::GetValues().fridge.right_arm_zeroing.index_difference),
+ left_elevator_pot_encoder_(
+ constants::GetValues().fridge.left_elev_zeroing.index_difference),
+ right_elevator_pot_encoder_(
+ constants::GetValues().fridge.right_elev_zeroing.index_difference),
+ fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
+ ".frc971.control_loops.fridge_queue.goal",
+ ".frc971.control_loops.fridge_queue.position",
+ ".frc971.control_loops.fridge_queue.output",
+ ".frc971.control_loops.fridge_queue.status") {
+ // Initialize the elevator.
+ InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.lower_limit);
+ // Initialize the arm.
+ InitializeArmPosition(0.0);
+ }
+
+ void InitializeElevatorPosition(double start_pos) {
+ InitializeElevatorPosition(start_pos, start_pos);
+ }
+
+ void InitializeElevatorPosition(double left_start_pos,
+ double right_start_pos) {
+ InitializeElevatorPosition(
+ left_start_pos, right_start_pos,
+ kNoiseScalar *
+ constants::GetValues().fridge.right_elev_zeroing.index_difference);
+ }
+
+ void InitializeElevatorPosition(double left_start_pos, double right_start_pos,
+ double pot_noise_stddev) {
+ // Update the internal state of the elevator plant.
+ elevator_plant_->mutable_X(0, 0) = (left_start_pos + right_start_pos) / 2.0;
+ elevator_plant_->mutable_X(1, 0) = 0.0;
+ elevator_plant_->mutable_X(2, 0) = (left_start_pos - right_start_pos) / 2.0;
+ elevator_plant_->mutable_X(3, 0) = 0.0;
+
+ right_elevator_pot_encoder_.Initialize(right_start_pos, pot_noise_stddev);
+ left_elevator_pot_encoder_.Initialize(left_start_pos, pot_noise_stddev);
+ elevator_initial_difference_ = left_start_pos - right_start_pos;
+ }
+
+ void InitializeArmPosition(double start_pos) {
+ InitializeArmPosition(start_pos, start_pos);
+ }
+
+ void InitializeArmPosition(double left_start_pos, double right_start_pos) {
+ InitializeArmPosition(
+ left_start_pos, right_start_pos,
+ kNoiseScalar *
+ constants::GetValues().fridge.right_arm_zeroing.index_difference);
+ }
+ void InitializeArmPosition(double left_start_pos, double right_start_pos,
+ double pot_noise_stddev) {
+ // Update the internal state of the arm plant.
+ arm_plant_->mutable_X(0, 0) = (left_start_pos + right_start_pos) / 2.0;
+ arm_plant_->mutable_X(1, 0) = 0.0;
+ arm_plant_->mutable_X(2, 0) = (left_start_pos - right_start_pos) / 2.0;
+ arm_plant_->mutable_X(3, 0) = 0.0;
+
+ left_arm_pot_encoder_.Initialize(left_start_pos, pot_noise_stddev);
+ right_arm_pot_encoder_.Initialize(right_start_pos, pot_noise_stddev);
+ arm_initial_difference_ = left_start_pos - right_start_pos;
+ }
+
+ // Changes the left-right calculations in the limit checks to measure absolute
+ // differences instead of differences relative to the starting offset.
+ void ErrorOnAbsoluteDifference() {
+ arm_initial_difference_ = 0.0;
+ elevator_initial_difference_ = 0.0;
+ }
+
+ // Sends a queue message with the position.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::FridgeQueue::Position> position =
+ fridge_queue_.position.MakeMessage();
+
+ left_arm_pot_encoder_.GetSensorValues(&position->arm.left);
+ right_arm_pot_encoder_.GetSensorValues(&position->arm.right);
+ left_elevator_pot_encoder_.GetSensorValues(&position->elevator.left);
+ right_elevator_pot_encoder_.GetSensorValues(&position->elevator.right);
+
+ position.Send();
+ }
+
+ // Sets the difference between the commanded and applied power for the arm.
+ // This lets us test that the integrator for the arm works.
+ void set_arm_power_error(double arm_power_error) {
+ arm_power_error_ = arm_power_error;
+ }
+ // Simulates for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(fridge_queue_.output.FetchLatest());
+
+ // Feed voltages into physics simulation.
+ if (arm_power_error_ != 0.0) {
+ arm_plant_->mutable_U() << ::aos::Clip(
+ fridge_queue_.output->left_arm + arm_power_error_, -12, 12),
+ ::aos::Clip(fridge_queue_.output->right_arm + arm_power_error_, -12,
+ 12);
+ } else {
+ arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
+ fridge_queue_.output->right_arm;
+ }
+ elevator_plant_->mutable_U() << fridge_queue_.output->left_elevator,
+ fridge_queue_.output->right_elevator;
+
+ // Use the plant to generate the next physical state given the voltages to
+ // the motors.
+ arm_plant_->Update();
+ elevator_plant_->Update();
+
+ const double left_arm_angle = arm_plant_->Y(0, 0);
+ const double right_arm_angle = arm_plant_->Y(1, 0);
+ const double left_elevator_height = elevator_plant_->Y(0, 0);
+ const double right_elevator_height = elevator_plant_->Y(1, 0);
+
+ // TODO (phil) Do some sanity tests on the arm angles and the elevator
+ // heights. For example, we need to make sure that both sides are within a
+ // certain distance of each other and they haven't crashed into the top or
+ // the bottom.
+
+ // Use the physical state to simulate sensor readings.
+ left_arm_pot_encoder_.MoveTo(left_arm_angle);
+ right_arm_pot_encoder_.MoveTo(right_arm_angle);
+ left_elevator_pot_encoder_.MoveTo(left_elevator_height);
+ right_elevator_pot_encoder_.MoveTo(right_elevator_height);
+
+ // Verify that the arm and elevator sides don't change much from their
+ // initial difference. Use the initial difference instead of the absolute
+ // difference to handle starting too far apart to test e-stopping.
+ EXPECT_NEAR(left_arm_angle - right_arm_angle, arm_initial_difference_,
+ 5.0 * M_PI / 180.0);
+ EXPECT_NEAR(left_elevator_height - right_elevator_height,
+ elevator_initial_difference_, 0.0254);
+
+ // Validate that the arm is within range.
+ EXPECT_GE(left_arm_angle,
+ constants::GetValues().fridge.arm.lower_hard_limit);
+ EXPECT_GE(right_arm_angle,
+ constants::GetValues().fridge.arm.lower_hard_limit);
+ EXPECT_LE(left_arm_angle,
+ constants::GetValues().fridge.arm.upper_hard_limit);
+ EXPECT_LE(right_arm_angle,
+ constants::GetValues().fridge.arm.upper_hard_limit);
+
+ // Validate that the elevator is within range.
+ EXPECT_GE(left_elevator_height,
+ constants::GetValues().fridge.elevator.lower_hard_limit);
+ EXPECT_GE(right_elevator_height,
+ constants::GetValues().fridge.elevator.lower_hard_limit);
+ EXPECT_LE(left_elevator_height,
+ constants::GetValues().fridge.elevator.upper_hard_limit);
+ EXPECT_LE(right_elevator_height,
+ constants::GetValues().fridge.elevator.upper_hard_limit);
+ }
+
+ private:
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> arm_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> elevator_plant_;
+
+ PositionSensorSimulator left_arm_pot_encoder_;
+ PositionSensorSimulator right_arm_pot_encoder_;
+ PositionSensorSimulator left_elevator_pot_encoder_;
+ PositionSensorSimulator right_elevator_pot_encoder_;
+
+ FridgeQueue fridge_queue_;
+
+ double elevator_initial_difference_ = 0.0;
+ double arm_initial_difference_ = 0.0;
+ double arm_power_error_ = 0.0;
+};
+
+class FridgeTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ FridgeTest()
+ : fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
+ ".frc971.control_loops.fridge_queue.goal",
+ ".frc971.control_loops.fridge_queue.position",
+ ".frc971.control_loops.fridge_queue.output",
+ ".frc971.control_loops.fridge_queue.status"),
+ fridge_(&fridge_queue_),
+ fridge_plant_(),
+ kinematics_(constants::GetValues().fridge.arm_length,
+ constants::GetValues().fridge.elevator.upper_limit,
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.arm.upper_limit,
+ constants::GetValues().fridge.arm.lower_limit) {
+ set_team_id(kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ fridge_queue_.goal.FetchLatest();
+ fridge_queue_.status.FetchLatest();
+ EXPECT_TRUE(fridge_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(fridge_queue_.status.get() != nullptr);
+ if (fridge_queue_.goal->profiling_type == 0) {
+ EXPECT_NEAR(fridge_queue_.goal->angle, fridge_queue_.status->angle,
+ 0.001);
+ EXPECT_NEAR(fridge_queue_.goal->height, fridge_queue_.status->height,
+ 0.001);
+ } else if (fridge_queue_.goal->profiling_type == 1) {
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_status;
+ kinematics_.ForwardKinematic(fridge_queue_.status->height,
+ fridge_queue_.status->angle, 0.0, 0.0, &x_y_status);
+ EXPECT_NEAR(fridge_queue_.goal->x, x_y_status.fridge_x, 0.001);
+ EXPECT_NEAR(fridge_queue_.goal->y, x_y_status.fridge_h, 0.001);
+ } else {
+ // Unhandled profiling type.
+ EXPECT_TRUE(false);
+ }
+ }
+
+ // Runs one iteration of the whole simulation and checks that separation
+ // remains reasonable.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ FridgeQueue fridge_queue_;
+
+ // Create a control loop and simulation.
+ Fridge fridge_;
+ FridgeSimulation fridge_plant_;
+
+ aos::util::ElevatorArmKinematics kinematics_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(FridgeTest, DoesNothing) {
+ // Set the goals to the starting values. This should theoretically guarantee
+ // that the controller does nothing.
+ const auto &values = constants::GetValues();
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(0.0)
+ .height(values.fridge.elevator.lower_limit)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send());
+
+ // Run a few iterations.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(FridgeTest, ReachesXYGoal) {
+ // Set a reasonable goal.
+ const auto &values = constants::GetValues();
+ const double soft_limit = values.fridge.elevator.lower_limit;
+ const double height = soft_limit + 0.4;
+ const double angle = M_PI / 6.0;
+
+ aos::util::ElevatorArmKinematics::KinematicResult x_y_goals;
+ kinematics_.ForwardKinematic(height, angle, 0.0, 0.0, &x_y_goals);
+
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .profiling_type(1)
+ .x(x_y_goals.fridge_x)
+ .y(x_y_goals.fridge_h)
+ .max_x_velocity(20)
+ .max_y_velocity(20)
+ .max_x_acceleration(20)
+ .max_y_acceleration(20)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(FridgeTest, ReachesGoal) {
+ // Set a reasonable goal.
+ const auto &values = constants::GetValues();
+ const double soft_limit = values.fridge.elevator.lower_limit;
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .height(soft_limit + 0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop doesn't try and go beyond the physical range of the
+// mechanisms.
+TEST_F(FridgeTest, RespectsRange) {
+ // Put the arm up to get it out of the way.
+ // We're going to send the elevator to -5, which should be significantly too
+ // low.
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(M_PI)
+ .height(-5.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ fridge_queue_.status.FetchLatest();
+ const auto &values = constants::GetValues();
+ EXPECT_NEAR(values.fridge.elevator.lower_limit, fridge_queue_.status->height,
+ 0.001);
+ EXPECT_NEAR(values.fridge.arm.upper_limit, fridge_queue_.status->angle,
+ 0.001);
+
+ // Put the arm down to get it out of the way.
+ // We're going to give the elevator some ridiculously high goal.
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(-M_PI)
+ .height(50.0)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ // Check that we are near our soft limit.
+ fridge_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.fridge.elevator.upper_limit, fridge_queue_.status->height,
+ 0.001);
+ EXPECT_NEAR(values.fridge.arm.lower_limit, fridge_queue_.status->angle,
+ 0.001);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(FridgeTest, ZeroTest) {
+ fridge_queue_.goal.MakeWithBuilder()
+ .angle(0.0)
+ .height(0.5)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send();
+ RunForTime(Time::InSeconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the lower hardstops doesn't cause an abort.
+TEST_F(FridgeTest, LowerHardstopStartup) {
+ fridge_plant_.InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.lower_hard_limit,
+ constants::GetValues().fridge.elevator.lower_hard_limit);
+ fridge_plant_.InitializeArmPosition(
+ constants::GetValues().fridge.arm.lower_hard_limit,
+ constants::GetValues().fridge.arm.lower_hard_limit);
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+ // We have to wait for it to put the elevator in a safe position as well.
+ RunForTime(Time::InMS(8000));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(FridgeTest, UpperHardstopStartup) {
+ fridge_plant_.InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.upper_hard_limit,
+ constants::GetValues().fridge.elevator.upper_hard_limit);
+ fridge_plant_.InitializeArmPosition(
+ constants::GetValues().fridge.arm.upper_hard_limit,
+ constants::GetValues().fridge.arm.upper_hard_limit);
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+ RunForTime(Time::InMS(5000));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting with an initial arm difference triggers an ESTOP.
+TEST_F(FridgeTest, ArmFarApartEstop) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+
+ do {
+ fridge_plant_.InitializeArmPosition(
+ constants::GetValues().fridge.arm.lower_limit,
+ constants::GetValues().fridge.arm.lower_limit + 0.2);
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ } while (fridge_.state() == Fridge::INITIALIZING);
+
+ EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
+
+ // TODO(austin): We should estop earlier once Phil's code to detect issues
+ // before the index pulse is merged.
+ while (fridge_.state() != Fridge::RUNNING &&
+ fridge_.state() != Fridge::ESTOP) {
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ }
+
+ EXPECT_EQ(Fridge::ESTOP, fridge_.state());
+}
+
+// Tests that starting with an initial elevator difference triggers an ESTOP.
+TEST_F(FridgeTest, ElevatorFarApartEstop) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+
+ do {
+ fridge_plant_.InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.elevator.lower_limit + 0.1);
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ } while (fridge_.state() == Fridge::INITIALIZING);
+
+ EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
+
+ // TODO(austin): We should estop earlier once Phil's code to detect issues
+ // before the index pulse is merged.
+ while (fridge_.state() != Fridge::RUNNING &&
+ fridge_.state() != Fridge::ESTOP) {
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ }
+
+ EXPECT_EQ(Fridge::ESTOP, fridge_.state());
+}
+
+// Tests that starting with an initial elevator difference converges back to 0
+// error when zeroed.
+TEST_F(FridgeTest, ElevatorFixError) {
+ fridge_queue_.goal.MakeWithBuilder()
+ .angle(0.0)
+ .height(0.2)
+ .max_velocity(20)
+ .max_acceleration(20)
+ .Send();
+
+ do {
+ fridge_plant_.InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.elevator.lower_limit + 0.01);
+ fridge_plant_.ErrorOnAbsoluteDifference();
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ } while (fridge_.state() == Fridge::INITIALIZING);
+
+ EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
+
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that starting with an initial arm difference converges back to 0
+// error when zeroed.
+TEST_F(FridgeTest, ArmFixError) {
+ fridge_queue_.goal.MakeWithBuilder()
+ .angle(0.0)
+ .height(0.2)
+ .max_angular_velocity(20)
+ .max_angular_acceleration(20)
+ .Send();
+
+ do {
+ fridge_plant_.InitializeArmPosition(0.0, 0.02);
+ fridge_plant_.ErrorOnAbsoluteDifference();
+ SendMessages(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ TickTime();
+ } while (fridge_.state() == Fridge::INITIALIZING);
+
+ EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
+
+ RunForTime(Time::InSeconds(5));
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(FridgeTest, ResetTest) {
+ fridge_plant_.InitializeElevatorPosition(
+ constants::GetValues().fridge.elevator.upper_hard_limit,
+ constants::GetValues().fridge.elevator.upper_hard_limit);
+ fridge_plant_.InitializeArmPosition(
+ constants::GetValues().fridge.arm.upper_hard_limit,
+ constants::GetValues().fridge.arm.upper_hard_limit);
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+ RunForTime(Time::InMS(5000));
+
+ EXPECT_EQ(Fridge::RUNNING, fridge_.state());
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(Time::InMS(100));
+ EXPECT_NE(Fridge::RUNNING, fridge_.state());
+ RunForTime(Time::InMS(6000));
+ EXPECT_EQ(Fridge::RUNNING, fridge_.state());
+ VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(FridgeTest, DisabledGoalTest) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+
+ RunForTime(Time::InMS(100), false);
+ EXPECT_EQ(0.0, fridge_.elevator_goal_);
+ EXPECT_EQ(0.0, fridge_.arm_goal_);
+
+ // Now make sure they move correctly
+ RunForTime(Time::InMS(4000), true);
+ EXPECT_NE(0.0, fridge_.elevator_goal_);
+ EXPECT_NE(0.0, fridge_.arm_goal_);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(FridgeTest, ElevatorGoalPositiveWindupTest) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+
+ while (fridge_.state() != Fridge::ZEROING_ELEVATOR) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_fridge_goal = fridge_.elevator_goal_;
+ fridge_.elevator_goal_ += 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_fridge_goal, fridge_.elevator_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(fridge_.elevator_loop_->U(), fridge_.elevator_loop_->U_uncapped());
+}
+
+// Tests that the arm zeroing goals don't wind up too far.
+TEST_F(FridgeTest, ArmGoalPositiveWindupTest) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+
+ int i = 0;
+ while (fridge_.state() != Fridge::ZEROING_ARM) {
+ RunIteration();
+ ++i;
+ ASSERT_LE(i, 10000);
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_fridge_goal = fridge_.arm_goal_;
+ fridge_.arm_goal_ += 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_fridge_goal, fridge_.arm_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(fridge_.arm_loop_->U(), fridge_.arm_loop_->U_uncapped());
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(FridgeTest, ElevatorGoalNegativeWindupTest) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+
+ while (fridge_.state() != Fridge::ZEROING_ELEVATOR) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_fridge_goal = fridge_.elevator_goal_;
+ fridge_.elevator_goal_ -= 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_fridge_goal, fridge_.elevator_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(fridge_.elevator_loop_->U(), fridge_.elevator_loop_->U_uncapped());
+}
+
+// Tests that the arm zeroing goals don't wind up too far.
+TEST_F(FridgeTest, ArmGoalNegativeWindupTest) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
+
+ while (fridge_.state() != Fridge::ZEROING_ARM) {
+ RunIteration();
+ }
+
+ // Kick it.
+ RunForTime(Time::InMS(50));
+ double orig_fridge_goal = fridge_.arm_goal_;
+ fridge_.arm_goal_ -= 100.0;
+
+ RunIteration();
+ EXPECT_NEAR(orig_fridge_goal, fridge_.arm_goal_, 0.05);
+
+ RunIteration();
+
+ EXPECT_EQ(fridge_.arm_loop_->U(), fridge_.arm_loop_->U_uncapped());
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(FridgeTest, ZeroNoGoal) {
+ RunForTime(Time::InSeconds(5));
+
+ EXPECT_EQ(Fridge::RUNNING, fridge_.state());
+}
+
+// Tests that if we start at the bottom, the elevator moves to a safe height
+// before zeroing the arm.
+TEST_F(FridgeTest, SafeArmZeroing) {
+ auto &values = constants::GetValues();
+ fridge_plant_.InitializeElevatorPosition(
+ values.fridge.elevator.lower_hard_limit);
+ fridge_plant_.InitializeArmPosition(M_PI / 4.0);
+
+ const auto start_time = Time::Now();
+ double last_arm_goal = fridge_.arm_goal_;
+ while (Time::Now() < start_time + Time::InMS(4000)) {
+ RunIteration();
+
+ if (fridge_.state() != Fridge::ZEROING_ELEVATOR) {
+ // Wait until we are zeroing the elevator.
+ continue;
+ }
+
+ fridge_queue_.status.FetchLatest();
+ ASSERT_TRUE(fridge_queue_.status.get() != nullptr);
+ if (fridge_queue_.status->height > values.fridge.arm_zeroing_height) {
+ // We had better not be trying to zero the arm...
+ EXPECT_EQ(last_arm_goal, fridge_.arm_goal_);
+ last_arm_goal = fridge_.arm_goal_;
+ }
+ }
+}
+
+// Tests that the arm integrator works.
+TEST_F(FridgeTest, ArmIntegratorTest) {
+ fridge_plant_.InitializeArmPosition(
+ (constants::GetValues().fridge.arm.lower_hard_limit +
+ constants::GetValues().fridge.arm.lower_hard_limit) /
+ 2.0);
+ fridge_plant_.set_arm_power_error(1.0);
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
+
+ RunForTime(Time::InMS(8000));
+
+ VerifyNearGoal();
+}
+
+// Phil:
+// TODO(austin): Check that we e-stop if encoder index pulse is not n
+// revolutions away from last one. (got extra counts from noise, etc).
+// TODO(austin): Check that we e-stop if pot disagrees too much with encoder
+// after we are zeroed.
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/fridge/fridge_main.cc b/y2015/control_loops/fridge/fridge_main.cc
new file mode 100644
index 0000000..e0fd6ea
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge_main.cc
@@ -0,0 +1,11 @@
+#include "y2015/control_loops/fridge/fridge.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::Fridge fridge;
+ fridge.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/control_loops/fridge/integral_arm_plant.cc b/y2015/control_loops/fridge/integral_arm_plant.cc
new file mode 100644
index 0000000..da269d7
--- /dev/null
+++ b/y2015/control_loops/fridge/integral_arm_plant.cc
@@ -0,0 +1,49 @@
+#include "y2015/control_loops/fridge/integral_arm_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients() {
+ Eigen::Matrix<double, 5, 5> A;
+ A << 1.0, 0.00479642025454, 0.0, 0.0, 4.92993559969e-05, 0.0, 0.919688585028, 0.0, 0.0, 0.0194484035162, 0.0, 0.0, 0.999539771613, 0.00479566382645, 0.0, 0.0, 0.0, -0.18154390621, 0.919241022297, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 5, 2> B;
+ B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.46477449538e-05, -2.46477449538e-05, 0.00972266818532, -0.00972266818532, 0.0, 0.0;
+ Eigen::Matrix<double, 2, 5> C;
+ C << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 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<5, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController() {
+ Eigen::Matrix<double, 5, 2> L;
+ L << 0.461805946837, 0.461805946837, 5.83483983392, 5.83483983392, 0.429725467802, -0.429725467802, 0.18044816586, -0.18044816586, 31.0623964848, 31.0623964848;
+ Eigen::Matrix<double, 2, 5> K;
+ K << 320.979606093, 21.0129517955, 884.233784759, 36.3637782119, 1.0, 320.979606095, 21.0129517956, -884.233784749, -36.3637782119, 1.0;
+ Eigen::Matrix<double, 5, 5> A_inv;
+ A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 5.21292341391e-05, 0.0, 1.08732457517, 0.0, 0.0, -0.0211467270909, 0.0, 0.0, 0.999513354044, -0.00521444313273, 0.0, 0.0, 0.0, 0.197397150694, 1.08682415753, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ return StateFeedbackController<5, 2, 2>(L, K, A_inv, MakeIntegralArmPlantCoefficients());
+}
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<5, 2, 2>>(new StateFeedbackPlantCoefficients<5, 2, 2>(MakeIntegralArmPlantCoefficients()));
+ return StateFeedbackPlant<5, 2, 2>(&plants);
+}
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<5, 2, 2>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<5, 2, 2>>(new StateFeedbackController<5, 2, 2>(MakeIntegralArmController()));
+ return StateFeedbackLoop<5, 2, 2>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2015/control_loops/fridge/integral_arm_plant.h b/y2015/control_loops/fridge/integral_arm_plant.h
new file mode 100644
index 0000000..80a4876
--- /dev/null
+++ b/y2015/control_loops/fridge/integral_arm_plant.h
@@ -0,0 +1,20 @@
+#ifndef Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+#define Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<5, 2, 2> MakeIntegralArmPlantCoefficients();
+
+StateFeedbackController<5, 2, 2> MakeIntegralArmController();
+
+StateFeedbackPlant<5, 2, 2> MakeIntegralArmPlant();
+
+StateFeedbackLoop<5, 2, 2> MakeIntegralArmLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_INTEGRAL_ARM_PLANT_H_
diff --git a/y2015/control_loops/fridge/replay_fridge.cc b/y2015/control_loops/fridge/replay_fridge.cc
new file mode 100644
index 0000000..65cc98a
--- /dev/null
+++ b/y2015/control_loops/fridge/replay_fridge.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2015/control_loops/fridge/fridge.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" fridge 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::FridgeQueue>
+ replayer(&::frc971::control_loops::fridge_queue, "fridge");
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
+
+ ::aos::Cleanup();
+}