Added code for recording power and position data in control loops and added code for generalizing to multiple hall effect sensors.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 8004da3..79fe61b 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -8,6 +8,7 @@
'../control_loops/control_loops.gyp:DriveTrain',
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
+ '../control_loops/control_loops.gyp:hall_effect_lib_test',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 18df792..db77a2c 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -37,6 +37,19 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'hall_effect_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'hall_effect_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/hall_effect_lib_test.cc b/frc971/control_loops/hall_effect_lib_test.cc
new file mode 100644
index 0000000..6141853
--- /dev/null
+++ b/frc971/control_loops/hall_effect_lib_test.cc
@@ -0,0 +1,217 @@
+#include <array>
+
+#include <memory>
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "gtest/gtest.h"
+#include "frc971/control_loops/hall_effect_loop.h"
+#include "frc971/control_loops/hall_effect_loop-inl.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Functions creating a StateFeedbackLoop stuff borrowing constants from wrist.
+// The constants are not very important, they just need to be reasonable.
+StateFeedbackPlant<2, 1, 1> MakeHallEffectPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00876530955899, 0.0, 0.763669024671;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000423500644841, 0.0810618735867;
+ 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> MakeHallEffectLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.66366902467, 58.1140316091;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 31.5808145893, 0.867171288023;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeHallEffectPlant());
+}
+
+class HallEffectSimulation : public ::testing::Test {
+ public:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+ HallEffectSimulation()
+ : hall_effect_loop_(new StateFeedbackLoop<2, 1, 1>(MakeHallEffectLoop()),
+ true, 5.0),
+ hall_effect_plant_(new StateFeedbackPlant<2, 1, 1>(MakeHallEffectPlant())),
+ lower_limit_(0.0),
+ upper_limit_(2.0) {
+ hall_stop_[0] = 0.5;
+ hall_stop_[1] = 1.25;
+ hall_stop_[2] = 2.0;
+ hall_start_[0] = -0.1;
+ hall_start_[1] = 0.75;
+ hall_start_[2] = 1.5;
+ // Flush the robot state queue.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ // Sends needed messages to the queue so that the zeroing code knows
+ // if the robot is enabled or not.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Resets the hall_effect_loop_ state to uninitialized, sets the position
+ // of the simulation to initial_position and sets the zero_down value of
+ // the hall_effect_loop.
+ void Reinitialize(double initial_position, bool zero_down) {
+ initial_position_ = initial_position;
+ hall_effect_loop_.zero_down_ = zero_down;
+ hall_effect_plant_->X(0, 0) = initial_position_;
+ hall_effect_plant_->X(1, 0) = 0.0;
+ hall_effect_plant_->Y =
+ hall_effect_plant_->C * hall_effect_plant_->X;
+ last_position_ = hall_effect_plant_->Y(0, 0);
+ calibration_[0] = -initial_position;
+ calibration_[1] = -initial_position;
+ calibration_[2] = -initial_position;
+ }
+
+ // Returns the absolute angle (change since last Reinitialize).
+ double GetAbsolutePosition() const {
+ return hall_effect_plant_->Y(0, 0);
+ }
+
+ // Returns adjusted angle (the "real" angle).
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Resets the state feedback loop to a certain position and sets the
+ // HallEffectLoop to either zero up or down.
+ // good_position refers to whether or not the position from the queue
+ // would have been good or not.
+ // Simulates the system for one timestep.
+ void Simulate(double goal, bool good_position) {
+ last_position_ = GetAbsolutePosition();
+ for (int i = 0; i < 3; ++i) {
+ if (last_position_ > hall_start_[i] && last_position_ < hall_stop_[i]) {
+ hall_effect_[i] = true;
+ } else {
+ hall_effect_[i] = false;
+ }
+ }
+ if (hall_effect_loop_.zero_down_) {
+ hall_effect_loop_.UpdateZeros(hall_stop_, hall_effect_, calibration_,
+ 1.0, GetPosition(), good_position);
+ } else {
+ hall_effect_loop_.UpdateZeros(hall_start_, hall_effect_, calibration_,
+ 1.0, GetPosition(), good_position);
+ }
+ if (hall_effect_loop_.state_ == HallEffectLoop<3>::READY) {
+ hall_effect_loop_.loop_->R(0, 0) = goal;
+ hall_effect_loop_.loop_->R(1, 0) = 0.0;
+ }
+ // Deal with updating all the necessary observers and the such.
+ hall_effect_loop_.loop_->Update(true, false);
+ hall_effect_plant_->U << hall_effect_loop_.loop_->U(0, 0);
+ hall_effect_plant_->Update();
+ }
+
+ void VerifyNearGoal(double goal) {
+ EXPECT_NEAR(goal, GetAbsolutePosition(), 1e-4);
+ }
+
+ virtual ~HallEffectSimulation() {
+ ::aos::robot_state.Clear();
+ }
+
+ HallEffectLoop<3> hall_effect_loop_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> hall_effect_plant_;
+ private:
+ // Physical limits (normally in constants.cpp).
+ double lower_limit_;
+ double upper_limit_;
+ // Edges of the hall effect sensors (normally in constants.cpp).
+ ::std::array<double, 3> hall_start_;
+ ::std::array<double, 3> hall_stop_;
+ // Array of whether any given hall effect sensor is activated.
+ ::std::array<bool, 3> hall_effect_;
+ // Array of calibration values (normally from queue).
+ ::std::array<double, 3> calibration_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_[3];
+}; // class HallEffectSimulation
+
+// Just test normal zeroing, and then going to a goal.
+TEST_F(HallEffectSimulation, ZerosCorrectly) {
+ Reinitialize(1.3, true);
+ for (int i = 0; i < 200; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, true);
+ }
+ VerifyNearGoal(0.55);
+}
+
+// Tests whether it zeros correctly starting on a sensor.
+TEST_F(HallEffectSimulation, ZeroStartingOn) {
+ Reinitialize(1.0, true);
+ for (int i = 0; i < 200; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, true);
+ }
+ VerifyNearGoal(0.55);
+}
+
+// Tests non-zero_down behavior (zero_up).
+TEST_F(HallEffectSimulation, ZeroUp) {
+ Reinitialize(1.4, false);
+ for (int i = 0; i < 200; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, true);
+ }
+ VerifyNearGoal(0.55);
+}
+
+// Tests dealing with missing positions.
+TEST_F(HallEffectSimulation, ZerosUp) {
+ Reinitialize(1.4, true);
+ for (int i = 0; i < 5; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, true);
+ }
+ for (int i = 0; i < 50; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, false);
+ }
+ EXPECT_TRUE(hall_effect_loop_.state_ != HallEffectLoop<3>::READY);
+ for (int i = 0; i < 150; ++i) {
+ SendDSPacket(true);
+ Simulate(0.55, true);
+ }
+ VerifyNearGoal(0.55);
+}
+
+// Tests whether the zeroing goal really does get limitted if it gets too high.
+TEST_F(HallEffectSimulation, LimitsZeroingGoal) {
+ Reinitialize(.6, true);
+ SendDSPacket(true);
+ Simulate(.1, true);
+ hall_effect_loop_.zeroing_position_ = -30;
+ hall_effect_loop_.loop_->R(0, 0) = -30;
+ hall_effect_loop_.loop_->Update(true, false);
+ hall_effect_loop_.LimitZeroingGoal();
+ EXPECT_NEAR(hall_effect_loop_.zeroing_position_, 0, .6);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/hall_effect_loop-inl.h b/frc971/control_loops/hall_effect_loop-inl.h
new file mode 100644
index 0000000..f5deb67
--- /dev/null
+++ b/frc971/control_loops/hall_effect_loop-inl.h
@@ -0,0 +1,200 @@
+#ifndef FRC971_CONTROL_LOOPS_HALL_EFFECT_INL_H_
+#define FRC971_CONTROL_LOOPS_HALL_EFFECT_INL_H_
+
+#include "frc971/control_loops/hall_effect_loop.h"
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/logging/logging.h"
+
+namespace frc971 {
+namespace control_loops {
+
+template <int kNumHallEffect>
+HallEffectLoop<kNumHallEffect>::HallEffectLoop(
+ StateFeedbackLoop<2, 1, 1>* state_feedback_loop,
+ bool zero_down, double max_zeroing_voltage)
+ : kMaxZeroingVoltage(max_zeroing_voltage),
+ zero_down_(zero_down),
+ state_(UNINITIALIZED),
+ loop_(state_feedback_loop),
+ current_position_(0.0),
+ last_off_position_(0.0),
+ last_calibration_sensor_(-1),
+ zeroing_position_(0.0),
+ zero_offset_(0.0),
+ old_zero_offset_(0.0) {
+}
+
+template <int kNumHallEffect>
+int HallEffectLoop<kNumHallEffect>::HallEffect() const {
+ for (int i = 0; i < kNumHallEffect; ++i) {
+ if (hall_effect_[i]) {
+ return i;
+ }
+ }
+ return -1;
+}
+
+template <int kNumHallEffect>
+int HallEffectLoop<kNumHallEffect>::WhichHallEffect() const {
+ if (zero_down_) {
+ for (int i = 0; i < kNumHallEffect; ++i) {
+ if (calibration_[i] + hall_effect_angle_[i] < last_off_position_ &&
+ calibration_[i] + hall_effect_angle_[i] >= current_position_) {
+ return i;
+ }
+ }
+ } else {
+ for (int i = 0; i < kNumHallEffect; ++i) {
+ if (calibration_[i] + hall_effect_angle_[i] > last_off_position_ &&
+ calibration_[i] + hall_effect_angle_[i] <= current_position_) {
+ return i;
+ }
+ }
+ }
+ return -1;
+}
+
+template <int kNumHallEffect>
+void HallEffectLoop<kNumHallEffect>::LimitZeroingGoal() {
+ if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
+ double excess = (loop_->U_uncapped(0, 0) - kMaxZeroingVoltage)
+ / loop_->K(0, 0);
+ zeroing_position_ -= excess;
+ }
+ if (loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
+ double excess = (loop_->U_uncapped(0, 0) + kMaxZeroingVoltage)
+ / loop_->K(0, 0);
+ zeroing_position_ -= excess;
+ }
+}
+
+template <int kNumHallEffect>
+void HallEffectLoop<kNumHallEffect>::UpdateZeros(
+ ::std::array<double, kNumHallEffect> hall_effect_angle,
+ ::std::array<bool, kNumHallEffect> hall_effect,
+ ::std::array<double, kNumHallEffect> calibration,
+ double zeroing_speed,
+ double position, bool good_position) {
+ hall_effect_angle_ = hall_effect_angle;
+ hall_effect_ = hall_effect;
+ calibration_ = calibration;
+ zeroing_speed_ = zeroing_speed;
+ current_position_ = position;
+
+ if (!zero_down_) {
+ zeroing_speed_ *= -1;
+ }
+
+ // Deal with getting all the position variables updated.
+ absolute_position_ = current_position_;
+ if (good_position) {
+ if (state_ == READY) {
+ absolute_position_ -= zero_offset_;
+ }
+ loop_->Y << absolute_position_;
+ if (HallEffect() == -1) {
+ last_off_position_ = current_position_;
+ }
+ } else {
+ absolute_position_ = loop_->X_hat(0, 0);
+ }
+
+ // switch for dealing with various zeroing states.
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(DEBUG, "status_: UNINITIALIZED\n");
+ last_calibration_sensor_ = -1;
+ if (good_position) {
+ // Reset the zeroing goal.
+ zeroing_position_ = absolute_position_;
+ // Clear the observer state.
+ loop_->X_hat << absolute_position_, 0.0;
+ // Only progress if we are enabled.
+ if (::aos::robot_state->enabled) {
+ if (HallEffect() != -1) {
+ state_ = MOVING_OFF;
+ } else {
+ state_ = ZEROING;
+ }
+ } else {
+ loop_->R << absolute_position_, 0.0;
+ }
+ }
+ break;
+ case MOVING_OFF:
+ LOG(DEBUG, "status_: MOVING_OFF\n");
+ // Move off the hall effect sensor.
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (good_position && (HallEffect() == -1)) {
+ // We are now off the sensor. Time to zero now.
+ state_ = ZEROING;
+ } else {
+ // Slowly creep off the sensor.
+ zeroing_position_ += zeroing_speed_ * dt;
+ loop_->R << zeroing_position_, zeroing_speed_;
+ break;
+ }
+ case ZEROING:
+ LOG(DEBUG, "status_: ZEROING\n");
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (good_position && (HallEffect() != -1)) {
+ state_ = READY;
+ // Verify that the calibration number is between the last off position
+ // and the current on position. If this is not true, move off and try
+ // again.
+ if (WhichHallEffect() == -1) {
+ LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
+ LOG(ERROR,
+ "Last off position was %f, current is %f,\n",
+ last_off_position_, current_position_);
+ state_ = MOVING_OFF;
+ } else {
+ // Save the zero, and then offset the observer to deal with the
+ // phantom step change.
+ const double old_zero_offset = zero_offset_;
+ zero_offset_ = calibration_[WhichHallEffect()];
+ loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
+ loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ last_calibration_sensor_ = WhichHallEffect();
+ }
+ } else {
+ // Slowly creep towards the sensor.
+ zeroing_position_ -= zeroing_speed_ * dt;
+ loop_->R << zeroing_position_, -zeroing_speed_;
+ }
+ break;
+
+ case READY:
+ {
+ LOG(DEBUG, "status_: READY\n");
+ break;
+ }
+
+ case ESTOP:
+ LOG(WARNING, "have already given up\n");
+ return;
+ }
+
+ if (state_ == MOVING_OFF || state_ == ZEROING) {
+ LimitZeroingGoal();
+ }
+
+ if (good_position) {
+ LOG(DEBUG,
+ "calibration sensor: %d zero_offset: %f absolute_position: %f\n",
+ last_calibration_sensor_, zero_offset_, absolute_position_);
+ }
+
+} // UpdateZeros
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_HALL_EFFECT_INL_H_
diff --git a/frc971/control_loops/hall_effect_loop.h b/frc971/control_loops/hall_effect_loop.h
new file mode 100644
index 0000000..53c45dc
--- /dev/null
+++ b/frc971/control_loops/hall_effect_loop.h
@@ -0,0 +1,109 @@
+#ifndef FRC971_CONTROL_LOOPS_HALL_EFFECT_H_
+#define FRC971_CONTROL_LOOPS_HALL_EFFECT_H_
+
+#include <memory>
+#include <array>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// A parent class for creating things such as the Angle Adjust and Wrist
+// which zero to some n number of hall effect sensors.
+// kNumHallEffect is the number of hall effect sensors being used.
+template <int kNumHallEffect>
+class HallEffectLoop {
+ public:
+ // zero_down refers to whether the device should zero by traveling
+ // down or up. max_zeroing_voltage is the maximum voltage to be applied
+ // while zeroing.
+ HallEffectLoop(StateFeedbackLoop<2, 1, 1> *state_feedback_loop,
+ bool zero_down, double max_zeroing_voltage);
+ // UpdateZeros deals with all of the zeroing code and takes the hall
+ // effect constants.
+ // hall_effect_angle is the angle of the appropriate edge of the sensor.
+ // This should match the zero_down variable. If it should zero by going
+ // down it should be the upper edge, or vice-versa.
+ // hall_effect and calibration are the hall effect values and calibration
+ // values from the queue.
+ // zeroing_speed is from the constants file and is the speed at which
+ // to move the device when zeroing, for safety.
+ // position is the encoder value, which should be position->before_angle
+ // from the queues.
+ // good_position is whether or not RunItertion had a good position value.
+ // Note: Does NOT perform the Update operation the state feedback loop.
+ void UpdateZeros(
+ ::std::array<double, kNumHallEffect> hall_effect_angle,
+ ::std::array<bool, kNumHallEffect> hall_effect,
+ ::std::array<double, kNumHallEffect> calibration,
+ double zeroing_speed,
+ double position, bool good_position=true);
+
+ static const double dt;
+
+ const double kMaxZeroingVoltage;
+
+ // Whether to zero down or up.
+ bool zero_down_;
+
+ // Enum to store the state of the internal zeroing state machine.
+ // UNINITIALIZED is if the arm (or device) is not zeroed and disabled.
+ // MOVING_OFF is if the arm is on the Hall Effect sensor and still in
+ // the zeroing process.
+ // ZEROING is if it is off the sensor and trying to find it.
+ // READY is if it is zeroed and good for operation.
+ // ESTOP doesn't do anything in terms of updating the goal or the such.
+ enum State {
+ UNINITIALIZED,
+ MOVING_OFF,
+ ZEROING,
+ READY,
+ ESTOP
+ };
+
+ // Internal state for zeroing.
+ State state_;
+
+ // Returns the number of the activated Hall Effect sensor, given
+ // an array of all the hall effect sensors.
+ // if no sensor is currently activated, then return -1.
+ int HallEffect() const;
+ // Returns which Hall Effect sensor has a calibration value between
+ // last_off_position_ and the current_position. If none does,
+ // then returns -1.
+ int WhichHallEffect() const;
+
+ // Limits the zeroing_position to prevent it from increasing beyond
+ // where the goal is high enough to get the max voltage you want while
+ // zeroing.
+ void LimitZeroingGoal();
+
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ // Cache values for hall effect sensors.
+ ::std::array<double, kNumHallEffect> hall_effect_angle_;
+ ::std::array<bool, kNumHallEffect> hall_effect_;
+ ::std::array<double, kNumHallEffect> calibration_;
+ double zeroing_speed_;
+ // Most recent encoder value and the one before it.
+ double current_position_;
+ double last_off_position_;
+ double absolute_position_;
+ // The sensor last used for calibration.
+ int last_calibration_sensor_;
+ // Position that is incremented for the goal when zeroing at Hall Effect.
+ double zeroing_position_;
+ // The offset of the encoder value from the actual position.
+ double zero_offset_;
+ double old_zero_offset_;
+};
+
+template <int kNumHallEffect>
+/*static*/ const double HallEffectLoop<kNumHallEffect>::dt = 0.01;
+
+} // namespace control_loops
+} // namespace frc971
+#endif // FRC971_CONTROL_LOOPS_HALL_EFFECT_H_
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 8fe18f8..6046c9a 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -153,6 +153,22 @@
}
}
+ // Starts the file for storing recorded data.
+ // file_name is the name of the file to write to.
+ virtual void StartDataFile(const char *file_name) {
+ FILE *data_file = fopen(file_name, "w");
+ fprintf(data_file, "time, power, position");
+ fclose(data_file);
+ }
+
+ // Records a single data point to the file of file_name,
+ // using a time of time.
+ virtual void RecordDatum(const char *file_name, double time) {
+ FILE *data_file = fopen(file_name, "a");
+ fprintf(data_file, "\n%f, %f, %f", time, U[0], Y[0]);
+ fclose(data_file);
+ }
+
protected:
// these are accessible from non-templated subclasses
static const int kNumStates = number_of_states;