Added angle adjust control loop.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 79fe61b..05fc9b8 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -9,6 +9,8 @@
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
'../control_loops/control_loops.gyp:hall_effect_lib_test',
+ '../control_loops/control_loops.gyp:angle_adjust_lib_test',
+ '../control_loops/control_loops.gyp:angle_adjust',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index a7bbfa5..6c7c54b 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -2,6 +2,7 @@
#include <stddef.h>
#include <math.h>
+#include <array>
#include "aos/common/inttypes.h"
#include "aos/common/messages/RobotState.q.h"
@@ -11,6 +12,9 @@
#define M_PI 3.14159265358979323846
#endif
+// Note: So far, none of the Angle Adjust numbers have been measured.
+// Do not rely on them for real life.
+
namespace frc971 {
namespace constants {
@@ -28,6 +32,23 @@
const double kPracticeWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
const double kCompWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const int kAngleAdjustHallEffect = 2;
+
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
+
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
+
+const double kPracticeAngleAdjustHorizontalUpperPhysicalLimit =
+ 3.0;
+const double kCompAngleAdjustHorizontalUpperPhysicalLimit =
+ 3.0;
+
const double kPracticeWristUpperLimit = 93 * M_PI / 180.0;
const double kCompWristUpperLimit = 93 * M_PI / 180.0;
@@ -36,6 +57,23 @@
const double kWristZeroingSpeed = 1.0;
+const double kPracticeAngleAdjustHorizontalLowerPhysicalLimit =
+ 0.0;
+const double kCompAngleAdjustHorizontalLowerPhysicalLimit =
+ 0.0;
+
+const double kPracticeAngleAdjustHorizontalUpperLimit =
+ 3.0;
+const double kCompAngleAdjustHorizontalUpperLimit =
+ 3.0;
+
+const double kPracticeAngleAdjustHorizontalLowerLimit =
+ 0.0;
+const double kCompAngleAdjustHorizontalLowerLimit =
+ 0.0;
+
+const double kAngleAdjustHorizontalZeroingSpeed = 1.0;
+
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -55,6 +93,20 @@
// Zeroing speed.
double wrist_zeroing_speed;
+ // AngleAdjust hall effect positive and negative edges.
+ ::std::array<double, 2> angle_adjust_horizontal_hall_effect_start_angle;
+ ::std::array<double, 2> angle_adjust_horizontal_hall_effect_stop_angle;
+
+ // Upper and lower extreme limits of travel for the angle adjust.
+ double angle_adjust_horizontal_upper_limit;
+ double angle_adjust_horizontal_lower_limit;
+ // Physical limits. These are here for testing.
+ double angle_adjust_horizontal_upper_physical_limit;
+ double angle_adjust_horizontal_lower_physical_limit;
+
+ // Zeroing speed.
+ double angle_adjust_horizontal_zeroing_speed;
+
// what camera_center returns
int camera_center;
};
@@ -77,6 +129,13 @@
kCompWristUpperPhysicalLimit,
kCompWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kCompAngleAdjustHorizontalHallEffectStartAngle,
+ kCompAngleAdjustHorizontalHallEffectStopAngle,
+ kCompAngleAdjustHorizontalUpperLimit,
+ kCompAngleAdjustHorizontalLowerLimit,
+ kCompAngleAdjustHorizontalUpperPhysicalLimit,
+ kCompAngleAdjustHorizontalLowerPhysicalLimit,
+ kAngleAdjustHorizontalZeroingSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -87,6 +146,13 @@
kPracticeWristUpperPhysicalLimit,
kPracticeWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kPracticeAngleAdjustHorizontalHallEffectStartAngle,
+ kPracticeAngleAdjustHorizontalHallEffectStopAngle,
+ kPracticeAngleAdjustHorizontalUpperLimit,
+ kPracticeAngleAdjustHorizontalLowerLimit,
+ kPracticeAngleAdjustHorizontalUpperPhysicalLimit,
+ kPracticeAngleAdjustHorizontalLowerPhysicalLimit,
+ kAngleAdjustHorizontalZeroingSpeed,
kPracticeCameraCenter};
break;
default:
@@ -147,6 +213,55 @@
return true;
}
+bool angle_adjust_horizontal_hall_effect_start_angle(
+ ::std::array<double, 2> *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_hall_effect_start_angle;
+ return true;
+}
+bool angle_adjust_horizontal_hall_effect_stop_angle(
+ ::std::array<double, 2> *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_hall_effect_stop_angle;
+ return true;
+}
+bool angle_adjust_horizontal_upper_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_upper_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_lower_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_lower_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_upper_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_upper_physical_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_lower_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_lower_physical_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_zeroing_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_horizontal_zeroing_speed;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 187ab6a..be166ac 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -1,5 +1,7 @@
#include <stdint.h>
+#include <array>
+
namespace frc971 {
namespace constants {
@@ -25,6 +27,19 @@
// Returns the speed to move the wrist at when zeroing in rad/sec
bool wrist_zeroing_speed(double *speed);
+bool angle_adjust_horizontal_hall_effect_start_angle(
+ ::std::array<double, 2> *angle);
+bool angle_adjust_horizontal_hall_effect_stop_angle(
+ ::std::array<double, 2> *angle);
+// These are the soft stops for up and down.
+bool angle_adjust_horizontal_lower_limit(double *angle);
+bool angle_adjust_horizontal_upper_limit(double *angle);
+// These are the hard stops. Don't use these for anything but testing.
+bool angle_adjust_horizontal_lower_physical_limit(double *angle);
+bool angle_adjust_horizontal_upper_physical_limit(double *angle);
+
+// Returns speed to move the angle adjust when zeroing, in rad/sec
+bool angle_adjust_horizontal_zeroing_speed(double *speed);
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
diff --git a/frc971/control_loops/angle_adjust.cc b/frc971/control_loops/angle_adjust.cc
new file mode 100644
index 0000000..a99e9ec
--- /dev/null
+++ b/frc971/control_loops/angle_adjust.cc
@@ -0,0 +1,160 @@
+#include "frc971/control_loops/angle_adjust.h"
+#include "frc971/control_loops/hall_effect_loop.h"
+#include "frc971/control_loops/hall_effect_loop-inl.h"
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/angle_adjust_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+AngleAdjustMotor::AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust)
+ : aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
+ my_angle_adjust),
+ hall_effect_(new StateFeedbackLoop<2, 1, 1>(MakeAngleAdjustLoop()), true),
+ error_count_(0),
+ time_(0.0) {
+ if (testing) {
+ hall_effect_.loop_->StartDataFile("angle_adjust.csv");
+ }
+}
+
+bool AngleAdjustMotor::FetchConstants() {
+ if (!constants::angle_adjust_horizontal_lower_limit(
+ &horizontal_lower_limit_)) {
+ LOG(ERROR, "Failed to fetch the horizontal lower limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_horizontal_upper_limit(
+ &horizontal_upper_limit_)) {
+ LOG(ERROR, "Failed to fetch the horizontal upper limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_horizontal_hall_effect_stop_angle(
+ &horizontal_hall_effect_stop_angle_)) {
+ LOG(ERROR, "Failed to fetch the hall effect stop angle constants.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_horizontal_zeroing_speed(
+ &horizontal_zeroing_speed_)) {
+ LOG(ERROR, "Failed to fetch the horizontal zeroing speed constant.\n");
+ return false;
+ }
+
+ return true;
+}
+
+double AngleAdjustMotor::ClipGoal(double goal) const {
+ return std::min(horizontal_upper_limit_,
+ std::max(horizontal_lower_limit_, goal));
+}
+
+double AngleAdjustMotor::LimitVoltage(double absolute_position,
+ double voltage) const {
+ if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
+ if (absolute_position >= horizontal_upper_limit_) {
+ voltage = std::min(0.0, voltage);
+ }
+ if (absolute_position <= horizontal_lower_limit_) {
+ voltage = std::max(0.0, voltage);
+ }
+ }
+
+ double limit = (hall_effect_.state_ == HallEffectLoop<2>::READY) ? 12.0 : 5.0;
+ // TODO(aschuh): Remove this line when we are done testing.
+ //limit = std::min(0.3, limit);
+ voltage = std::min(limit, voltage);
+ voltage = std::max(-limit, voltage);
+ return voltage;
+}
+
+// Positive angle is up, and positive power is up.
+void AngleAdjustMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * /*status*/) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ // Cache the constants to avoid error handling down below.
+ if (!FetchConstants()) {
+ LOG(WARNING, "Failed to fetch constants.\n");
+ return;
+ }
+
+ // Uninitialize the bot if too many cycles pass without an encoder.
+ if (position == NULL) {
+ LOG(WARNING, "no new pos given\n");
+ error_count_++;
+ } else {
+ error_count_ = 0;
+ }
+ if (error_count_ >= 4) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ hall_effect_.state_ = HallEffectLoop<2>::UNINITIALIZED;
+ }
+
+ double absolute_position = hall_effect_.loop_->X_hat(0, 0);
+ // Compute the absolute position of the angle adjust.
+ if (position) {
+ hall_effect_sensors_[0] = position->bottom_hall_effect;
+ hall_effect_sensors_[1] = position->middle_hall_effect;
+ calibration_values_[0] = position->bottom_calibration;
+ calibration_values_[1] = position->middle_calibration;
+ absolute_position = position->before_angle;
+ }
+
+ hall_effect_.UpdateZeros(horizontal_hall_effect_stop_angle_,
+ hall_effect_sensors_,
+ calibration_values_,
+ horizontal_zeroing_speed_,
+ absolute_position,
+ position != NULL);
+
+ if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
+ const double limited_goal = ClipGoal(goal->goal);
+ hall_effect_.loop_->R << limited_goal, 0.0;
+ }
+
+ // Update the observer.
+ hall_effect_.loop_->Update(position != NULL, output == NULL);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f bottom_hall: %s middle_hall: %s\n",
+ position->before_angle,
+ position->bottom_hall_effect ? "true" : "false",
+ position->middle_hall_effect ? "true" : "false");
+ }
+
+ if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
+ LOG(DEBUG, "calibrated with: %s hall effect\n",
+ hall_effect_.last_calibration_sensor_ ? "bottom" : "middle");
+ }
+
+ if (output) {
+ output->voltage = LimitVoltage(hall_effect_.absolute_position_,
+ hall_effect_.loop_->U(0, 0));
+ }
+
+ if (testing) {
+ hall_effect_.loop_->RecordDatum("angle_adjust.csv", time_);
+ }
+ time_ += dt;
+} // RunIteration
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust.h b/frc971/control_loops/angle_adjust.h
new file mode 100644
index 0000000..43af550
--- /dev/null
+++ b/frc971/control_loops/angle_adjust.h
@@ -0,0 +1,84 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
+
+#include <memory>
+#include <array>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust_motor_plant.h"
+#include "frc971/control_loops/hall_effect_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Allows the control loop to add the tests to access private members.
+namespace testing {
+class AngleAdjustTest_RezeroWithMissingPos_Test;
+class AngleAdjustTest_DisableGoesUninitialized_Test;
+}
+
+class AngleAdjustMotor
+ : public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
+ public:
+ explicit AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust =
+ &control_loops::angle_adjust);
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // Allows the testing code to access some of private members.
+ friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
+ friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;
+
+ // Whether or not we are testing it currently;
+ // turns on/off recording data.
+ static const bool testing = true;
+
+ static constexpr double dt = 0.01;
+
+ // Fetches and locally caches the latest set of constants.
+ // Returns whether it succeeded or not.
+ bool FetchConstants();
+
+ // Clips the goal to be inside the limits and returns the clipped goal.
+ // Requires the constants to have already been fetched.
+ double ClipGoal(double goal) const;
+ // Limits the voltage depending whether the angle adjust has been zeroed or
+ // is out of range to make it safer to use. May or may not be necessary.
+ double LimitVoltage(double absolute_position, double voltage) const;
+
+ // Hall Effect class for dealing with hall effect sensors.
+ HallEffectLoop<2> hall_effect_;
+
+ // Missed position packet count.
+ int error_count_;
+
+ // Local cache of angle adjust geometry constants.
+ double horizontal_lower_limit_;
+ double horizontal_upper_limit_;
+ ::std::array<double, 2> horizontal_hall_effect_stop_angle_;
+ double horizontal_zeroing_speed_;
+
+ // Stores information from the queue about the hall effect sensors.
+ // Because we don't have arrays in the queue, we needs these to convert
+ // to a more convenient format.
+ ::std::array<bool, 2> hall_effect_sensors_;
+ ::std::array<double, 2> calibration_values_;
+
+ // Time for recording data.
+ double time_;
+
+ DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
diff --git a/frc971/control_loops/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust_lib_test.cc
new file mode 100644
index 0000000..3eda6da
--- /dev/null
+++ b/frc971/control_loops/angle_adjust_lib_test.cc
@@ -0,0 +1,311 @@
+#include <unistd.h>
+
+#include <memory>
+#include <array>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust.h"
+#include "frc971/control_loops/hall_effect_loop.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the angle_adjust and sends out queue messages containing the
+// position.
+class AngleAdjustMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // angle_adjust, which will be treated as 0 by the encoder.
+ AngleAdjustMotorSimulation(double initial_position)
+ : angle_adjust_plant_(new StateFeedbackPlant<2, 1, 1>(MakeAngleAdjustPlant())),
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ angle_adjust_plant_->X(0, 0) = initial_position_;
+ angle_adjust_plant_->X(1, 0) = 0.0;
+ angle_adjust_plant_->Y = angle_adjust_plant_->C * angle_adjust_plant_->X;
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ calibration_value_[0] = 0.0;
+ calibration_value_[1] = 0.0;
+ }
+
+ // Returns the absolute angle of the angle_adjust.
+ double GetAbsolutePosition() const {
+ return angle_adjust_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the angle_adjust.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ ::std::array<double, 2> horizontal_hall_effect_start_angle;
+ ASSERT_TRUE(constants::angle_adjust_horizontal_hall_effect_start_angle(
+ &horizontal_hall_effect_start_angle));
+ ::std::array<double, 2> horizontal_hall_effect_stop_angle;
+ ASSERT_TRUE(constants::angle_adjust_horizontal_hall_effect_stop_angle(
+ &horizontal_hall_effect_stop_angle));
+
+ ::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
+ my_angle_adjust_loop_.position.MakeMessage();
+ position->before_angle = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position >= horizontal_hall_effect_start_angle[0] &&
+ abs_position <= horizontal_hall_effect_stop_angle[0]) {
+ position->bottom_hall_effect = true;
+ } else {
+ position->bottom_hall_effect = false;
+ }
+ if (abs_position >= horizontal_hall_effect_start_angle[1] &&
+ abs_position <= horizontal_hall_effect_stop_angle[1]) {
+ position->middle_hall_effect = true;
+ } else {
+ position->middle_hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ if ((last_position_ < horizontal_hall_effect_start_angle[0] ||
+ last_position_ > horizontal_hall_effect_stop_angle[0]) &&
+ (position->bottom_hall_effect)) {
+ calibration_value_[0] = -initial_position_;
+ }
+ if ((last_position_ < horizontal_hall_effect_start_angle[1] ||
+ last_position_ > horizontal_hall_effect_stop_angle[1]) &&
+ (position->middle_hall_effect)) {
+ calibration_value_[1] = -initial_position_;
+ }
+
+ position->bottom_calibration = calibration_value_[0];
+ position->middle_calibration = calibration_value_[1];
+ position.Send();
+ }
+
+ // Simulates the angle_adjust moving for one timestep.
+ void Simulate() {
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ EXPECT_TRUE(my_angle_adjust_loop_.output.FetchLatest());
+ angle_adjust_plant_->U << my_angle_adjust_loop_.output->voltage;
+ angle_adjust_plant_->Update();
+
+ // Assert that we are in the right physical range.
+ double horizontal_upper_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_horizontal_upper_physical_limit(
+ &horizontal_upper_physical_limit));
+ double horizontal_lower_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_horizontal_lower_physical_limit(
+ &horizontal_lower_physical_limit));
+
+ EXPECT_GE(horizontal_upper_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(horizontal_lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
+ private:
+ AngleAdjustLoop my_angle_adjust_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_[2];
+};
+
+class AngleAdjustTest : 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.
+ AngleAdjustLoop my_angle_adjust_loop_;
+
+ // Create a loop and simulation plant.
+ AngleAdjustMotor angle_adjust_motor_;
+ AngleAdjustMotorSimulation angle_adjust_motor_plant_;
+
+ AngleAdjustTest() : my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status"),
+ angle_adjust_motor_(&my_angle_adjust_loop_),
+ angle_adjust_motor_plant_(.75) {
+ // 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() {
+ my_angle_adjust_loop_.goal.FetchLatest();
+ my_angle_adjust_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_angle_adjust_loop_.goal->goal,
+ angle_adjust_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~AngleAdjustTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the angle_adjust zeros correctly and goes to a position.
+TEST_F(AngleAdjustTest, ZerosCorrectly) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on the hall effect sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOn) {
+ angle_adjust_motor_plant_.Reinitialize(0.25);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(AngleAdjustTest, HandleMissingPosition) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ }
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(AngleAdjustTest, RezeroWithMissingPos) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).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) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_EQ(HallEffectLoop<2>::UNINITIALIZED,
+ angle_adjust_motor_.hall_effect_.state_);
+ }
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 430) {
+ EXPECT_TRUE(
+ HallEffectLoop<2>::ZEROING == angle_adjust_motor_.hall_effect_.state_
+ || HallEffectLoop<2>::MOVING_OFF ==
+ angle_adjust_motor_.hall_effect_.state_);
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(AngleAdjustTest, DisableGoesUninitialized) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ angle_adjust_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_EQ(HallEffectLoop<2>::UNINITIALIZED,
+ angle_adjust_motor_.hall_effect_.state_);
+ }
+ } else {
+ SendDSPacket(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_EQ(HallEffectLoop<2>::ZEROING,
+ angle_adjust_motor_.hall_effect_.state_);
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly from above the second sensor.
+TEST_F(AngleAdjustTest, ZerosCorrectlyAboveSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.75);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on
+// the second hall effect sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOnSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.25);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust_main.cc b/frc971/control_loops/angle_adjust_main.cc
new file mode 100644
index 0000000..68226be
--- /dev/null
+++ b/frc971/control_loops/angle_adjust_main.cc
@@ -0,0 +1,5 @@
+#include "frc971/control_loops/angle_adjust.h"
+
+#include "aos/aos_core.h"
+
+AOS_RUN_LOOP(frc971::control_loops::AngleAdjustMotor);
diff --git a/frc971/control_loops/angle_adjust_motor.q b/frc971/control_loops/angle_adjust_motor.q
new file mode 100644
index 0000000..2a7c562
--- /dev/null
+++ b/frc971/control_loops/angle_adjust_motor.q
@@ -0,0 +1,25 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group AngleAdjustLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Position {
+ // Angle of the encoder.
+ double before_angle;
+ double after_angle;
+ bool bottom_hall_effect;
+ bool middle_hall_effect;
+ // The exact position when the corresponding hall_effect changed.
+ double bottom_calibration;
+ double middle_calibration;
+ };
+
+ queue aos.control_loops.Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group AngleAdjustLoop angle_adjust;
diff --git a/frc971/control_loops/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust_motor_plant.cc
new file mode 100644
index 0000000..4862516
--- /dev/null
+++ b/frc971/control_loops/angle_adjust_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/angle_adjust_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00135041324202, 0.0, 0.000610875713246;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00111752476609, 0.129120861909;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.900610875713, 1.85371819524;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 12.7787251077, -5.83693180893;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeAngleAdjustPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust_motor_plant.h
new file mode 100644
index 0000000..0bcd582
--- /dev/null
+++ b/frc971/control_loops/angle_adjust_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index db77a2c..3766af4 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -2,6 +2,7 @@
'variables': {
'loop_files': [
'DriveTrain.q',
+ 'angle_adjust_motor.q',
]
},
'targets': [
@@ -50,6 +51,52 @@
],
},
{
+ 'target_name': 'angle_adjust_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'angle_adjust.cc',
+ 'angle_adjust_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ 'state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'control_loops',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ 'angle_adjust_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'angle_adjust_lib',
+ 'control_loops',
+ ],
+ },
+ {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
new file mode 100755
index 0000000..ed04198
--- /dev/null
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -0,0 +1,89 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class AngleAdjust(control_loop.ControlLoop):
+ def __init__(self):
+ super(AngleAdjust, self).__init__("AngleAdjust")
+ # Stall Torque in N m
+ self.stall_torque = .428
+ # Stall Current in Amps
+ self.stall_current = 63.8
+ # Free Speed in RPM
+ self.free_speed = 16000.0
+ # Free Current in Amps
+ self.free_current = 1.2
+ # Moment of inertia of the angle adjust about the shooter's pivot in kg m^2
+ self.J = 0.41085133
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio of the gearbox multiplied by the ratio of the radii of
+ # the output and the angle adjust curve, which is essentially another gear.
+ self.G = (1.0 / 50.0) * (0.01905 / 0.41964)
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.89, .85])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ angle_adjust = AngleAdjust()
+ simulated_x = []
+ for _ in xrange(100):
+ angle_adjust.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(angle_adjust.X[0, 0])
+
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ angle_adjust = AngleAdjust()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
+ angle_adjust.UpdateObserver(U)
+ angle_adjust.Update(U)
+ close_loop_x.append(angle_adjust.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ angle_adjust.DumpHeaderFile(argv[1])
+ angle_adjust.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))