Modified the angle adjust code to use the new zeroed_joint.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 1b53c06..e4c804a 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -2,7 +2,6 @@
#include <stddef.h>
#include <math.h>
-#include <array>
#include "aos/common/inttypes.h"
#include "aos/common/messages/RobotState.q.h"
@@ -42,15 +41,11 @@
const int kAngleAdjustHallEffect = 2;
-const ::std::array<double, kAngleAdjustHallEffect>
- kCompAngleAdjustHallEffectStartAngle = {{-0.1, 1.0}};
-const ::std::array<double, kAngleAdjustHallEffect>
- kPracticeAngleAdjustHallEffectStartAngle = {{-0.1, 1.0}};
+const double kCompAngleAdjustHallEffectStartAngle[2] = {0.5, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.5, 1.5};
-const ::std::array<double, kAngleAdjustHallEffect>
- kCompAngleAdjustHallEffectStopAngle = {{0.5, 1.5}};
-const ::std::array<double, kAngleAdjustHallEffect>
- kPracticeAngleAdjustHallEffectStopAngle = {{0.5, 1.5}};
+const double kCompAngleAdjustHallEffectStopAngle[2] = {-0.1, 1.0};
+const double kPracticeAngleAdjustHallEffectStopAngle[2] = {-0.1, 1.0};
const double kPracticeAngleAdjustUpperPhysicalLimit = 3.0;
const double kCompAngleAdjustUpperPhysicalLimit = 3.0;
@@ -64,7 +59,7 @@
const double kPracticeAngleAdjustLowerLimit = 0.0;
const double kCompAngleAdjustLowerLimit = 0.0;
-const double kAngleAdjustZeroingSpeed = 1.0;
+const double kAngleAdjustZeroingSpeed = -1.0;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -86,8 +81,8 @@
double wrist_zeroing_speed;
// AngleAdjust hall effect positive and negative edges.
- ::std::array<double, 2> angle_adjust_hall_effect_start_angle;
- ::std::array<double, 2> angle_adjust_hall_effect_stop_angle;
+ const double *angle_adjust_hall_effect_start_angle;
+ const double *angle_adjust_hall_effect_stop_angle;
// Upper and lower extreme limits of travel for the angle adjust.
double angle_adjust_upper_limit;
@@ -205,18 +200,18 @@
return true;
}
-bool angle_adjust_hall_effect_start_angle(
- ::std::array<double, 2> *angle) {
+bool angle_adjust_hall_effect_start_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_hall_effect_start_angle;
+ angle[0] = values->angle_adjust_hall_effect_start_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_start_angle[1];
return true;
}
-bool angle_adjust_hall_effect_stop_angle(
- ::std::array<double, 2> *angle) {
+bool angle_adjust_hall_effect_stop_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_hall_effect_stop_angle;
+ angle[0] = values->angle_adjust_hall_effect_stop_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_stop_angle[1];
return true;
}
bool angle_adjust_upper_limit(double *angle) {
diff --git a/frc971/constants.h b/frc971/constants.h
index 1f3eb35..98baca4 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -1,7 +1,5 @@
#include <stdint.h>
-#include <array>
-
namespace frc971 {
namespace constants {
@@ -27,10 +25,8 @@
// Returns the speed to move the wrist at when zeroing in rad/sec
bool wrist_zeroing_speed(double *speed);
-bool angle_adjust_hall_effect_start_angle(
- ::std::array<double, 2> *angle);
-bool angle_adjust_hall_effect_stop_angle(
- ::std::array<double, 2> *angle);
+bool angle_adjust_hall_effect_start_angle(double *angle);
+bool angle_adjust_hall_effect_stop_angle(double *angle);
// These are the soft stops for up and down.
bool angle_adjust_lower_limit(double *angle);
bool angle_adjust_upper_limit(double *angle);
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index e64f5e4..8482a4b 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -9,8 +9,6 @@
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/hall_effect_loop.h"
-#include "frc971/control_loops/hall_effect_loop-inl.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
namespace frc971 {
@@ -20,63 +18,36 @@
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, 5.0),
- error_count_(0),
- time_(0.0) {
+ zeroed_joint_(MakeAngleAdjustLoop()) {
}
-/*static*/ const double AngleAdjustMotor::dt = 0.01;
-
-bool AngleAdjustMotor::FetchConstants() {
+bool AngleAdjustMotor::FetchConstants(
+ ZeroedJoint<2>::ConfigurationData *config_data) {
if (!constants::angle_adjust_lower_limit(
- &lower_limit_)) {
+ &config_data->lower_limit)) {
LOG(ERROR, "Failed to fetch the angle adjust lower limit constant.\n");
return false;
}
if (!constants::angle_adjust_upper_limit(
- &upper_limit_)) {
+ &config_data->upper_limit)) {
LOG(ERROR, "Failed to fetch the angle adjust upper limit constant.\n");
return false;
}
- if (!constants::angle_adjust_hall_effect_stop_angle(
- &hall_effect_stop_angle_)) {
- LOG(ERROR, "Failed to fetch the hall effect stop angle constants.\n");
+ if (!constants::angle_adjust_hall_effect_start_angle(
+ config_data->hall_effect_start_angle)) {
+ LOG(ERROR, "Failed to fetch the hall effect start angle constants.\n");
return false;
}
if (!constants::angle_adjust_zeroing_speed(
- &zeroing_speed_)) {
+ &config_data->zeroing_speed)) {
LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
return false;
}
+ config_data->max_zeroing_voltage = 4.0;
return true;
}
-double AngleAdjustMotor::ClipGoal(double goal) const {
- return std::min(upper_limit_,
- std::max(lower_limit_, goal));
-}
-
-double AngleAdjustMotor::LimitVoltage(double absolute_position,
- double voltage) const {
- if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
- if (absolute_position >= upper_limit_) {
- voltage = std::min(0.0, voltage);
- }
- if (absolute_position <= 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,
@@ -91,71 +62,44 @@
}
// Cache the constants to avoid error handling down below.
- if (!FetchConstants()) {
+ ZeroedJoint<2>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
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;
+ zeroed_joint_.set_config_data(config_data);
}
- 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->bottom_angle;
+ ZeroedJoint<2>::PositionData transformed_position;
+ ZeroedJoint<2>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->angle;
+ transformed_position.hall_effects[0] = position->bottom_hall_effect;
+ transformed_position.hall_effect_positions[0] =
+ position->bottom_calibration;
+ transformed_position.hall_effects[1] = position->middle_hall_effect;
+ transformed_position.hall_effect_positions[1] =
+ position->middle_calibration;
}
- // Deals with all the zeroing stuff.
- hall_effect_.UpdateZeros(hall_effect_stop_angle_,
- hall_effect_sensors_,
- calibration_values_,
- zeroing_speed_,
- absolute_position,
- position != NULL);
-
- // Only try to go to our goal if we are actually zeroed.
- 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);
-
- // Prevent the zeroing goal from running off. Needs to happen after
- // U is calculated, hence why this is after the loop_->Update.
- hall_effect_.LimitZeroingGoal();
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
if (position) {
LOG(DEBUG, "pos=%f bottom_hall: %s middle_hall: %s\n",
- position->bottom_angle,
+ position->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));
+ output->voltage = voltage;
}
-} // RunIteration
+}
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
index 6fe55ff..dfa8ad2 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -8,7 +8,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
-#include "frc971/control_loops/hall_effect_loop.h"
+#include "frc971/control_loops/zeroed_joint.h"
namespace frc971 {
namespace control_loops {
@@ -32,49 +32,32 @@
::aos::control_loops::Output *output,
::aos::control_loops::Status *status);
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
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;
-
- // The time step of the control loop.
- static const double dt;
-
// Fetches and locally caches the latest set of constants.
// Returns whether it succeeded or not.
- bool FetchConstants();
+ bool FetchConstants(ZeroedJoint<2>::ConfigurationData *config_data);
- // 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 lower_limit_;
- double upper_limit_;
- ::std::array<double, 2> hall_effect_stop_angle_;
- double 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_;
+ // The zeroed joint to use.
+ ZeroedJoint<2> zeroed_joint_;
DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
};
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
index d0215de..091f494 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
@@ -50,4 +50,3 @@
::aos::Cleanup();
return 0;
}
-
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 242f3ce..f8a7c63 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -8,7 +8,6 @@
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
#include "frc971/control_loops/angle_adjust/angle_adjust.h"
-#include "frc971/control_loops/hall_effect_loop.h"
#include "frc971/constants.h"
@@ -61,28 +60,28 @@
void SendPositionMessage() {
const double angle = GetPosition();
- ::std::array<double, 2> hall_effect_start_angle;
+ double hall_effect_start_angle[2];
ASSERT_TRUE(constants::angle_adjust_hall_effect_start_angle(
- &hall_effect_start_angle));
- ::std::array<double, 2> hall_effect_stop_angle;
+ hall_effect_start_angle));
+ double hall_effect_stop_angle[2];
ASSERT_TRUE(constants::angle_adjust_hall_effect_stop_angle(
- &hall_effect_stop_angle));
+ hall_effect_stop_angle));
::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
my_angle_adjust_loop_.position.MakeMessage();
- position->bottom_angle = angle;
+ position->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 >= hall_effect_start_angle[0] &&
- abs_position <= hall_effect_stop_angle[0]) {
+ if (abs_position <= hall_effect_start_angle[0] &&
+ abs_position >= hall_effect_stop_angle[0]) {
position->bottom_hall_effect = true;
} else {
position->bottom_hall_effect = false;
}
- if (abs_position >= hall_effect_start_angle[1] &&
- abs_position <= hall_effect_stop_angle[1]) {
+ if (abs_position <= hall_effect_start_angle[1] &&
+ abs_position >= hall_effect_stop_angle[1]) {
position->middle_hall_effect = true;
} else {
position->middle_hall_effect = false;
@@ -90,15 +89,16 @@
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
+ // TODO(aschuh): This won't deal with both edges correctly.
if ((last_position_ < hall_effect_start_angle[0] ||
last_position_ > hall_effect_stop_angle[0]) &&
(position->bottom_hall_effect)) {
- calibration_value_[0] = -initial_position_;
+ calibration_value_[0] = hall_effect_start_angle[0] - initial_position_;
}
if ((last_position_ < hall_effect_start_angle[1] ||
last_position_ > hall_effect_stop_angle[1]) &&
(position->middle_hall_effect)) {
- calibration_value_[1] = -initial_position_;
+ calibration_value_[1] = hall_effect_start_angle[1] - initial_position_;
}
position->bottom_calibration = calibration_value_[0];
@@ -154,7 +154,7 @@
".frc971.control_loops.angle_adjust.output",
".frc971.control_loops.angle_adjust.status"),
angle_adjust_motor_(&my_angle_adjust_loop_),
- angle_adjust_motor_plant_(.75) {
+ angle_adjust_motor_plant_(0.75) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -232,16 +232,13 @@
} else {
if (i > 310) {
// Should be re-zeroing now.
- EXPECT_EQ(HallEffectLoop<2>::UNINITIALIZED,
- angle_adjust_motor_.hall_effect_.state_);
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
}
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_);
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
+ angle_adjust_motor_.is_moving_off());
}
angle_adjust_motor_.Iterate();
@@ -263,16 +260,14 @@
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_);
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
}
} 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_);
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing());
}
angle_adjust_motor_.Iterate();
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_main.cc b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
index 142a8c7..b9f8189 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_main.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
@@ -2,4 +2,10 @@
#include "aos/aos_core.h"
-AOS_RUN_LOOP(frc971::control_loops::AngleAdjustMotor);
+int main() {
+ ::aos::Init();
+ ::frc971::control_loops::AngleAdjustMotor angle_adjust;
+ angle_adjust.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor.q b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
index eac3ada..a98419a 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor.q
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
@@ -6,9 +6,8 @@
implements aos.control_loops.ControlLoop;
message Position {
- // Angle of the encoder.
- double bottom_angle;
- double middle_angle;
+ // Angle of the height adjust.
+ double angle;
bool bottom_hall_effect;
bool middle_hall_effect;
// The exact position when the corresponding hall_effect changed.
diff --git a/frc971/control_loops/hall_effect_lib_test.cc b/frc971/control_loops/hall_effect_lib_test.cc
deleted file mode 100644
index 6141853..0000000
--- a/frc971/control_loops/hall_effect_lib_test.cc
+++ /dev/null
@@ -1,217 +0,0 @@
-#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
deleted file mode 100644
index 29c4273..0000000
--- a/frc971/control_loops/hall_effect_loop-inl.h
+++ /dev/null
@@ -1,198 +0,0 @@
-#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 (state_ == MOVING_OFF || state_ == ZEROING) {
- 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 (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
deleted file mode 100644
index 53c45dc..0000000
--- a/frc971/control_loops/hall_effect_loop.h
+++ /dev/null
@@ -1,109 +0,0 @@
-#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/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 85cc221..13471fa 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -75,7 +75,6 @@
transformed_position.position = position->pos;
transformed_position.hall_effects[0] = position->hall_effect;
transformed_position.hall_effect_positions[0] = position->calibration;
- printf("Position is %f\n", position->pos);
}
const double voltage = zeroed_joint_.Update(transformed_position_ptr,
@@ -90,7 +89,6 @@
if (output) {
output->voltage = voltage;
- printf("Voltage applied is %f\n", voltage);
}
}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index 1dd9afd..a53f603 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -29,6 +29,9 @@
// True if the wrist is zeroing.
bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
// True if the state machine is uninitialized.
bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index f56c84a..89a3c7b 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -5,8 +5,6 @@
#include "aos/common/control_loop/ControlLoop.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/wrist/wrist_motor_plant.h"
namespace frc971 {
namespace control_loops {
@@ -116,6 +114,9 @@
// True if the code is zeroing.
bool is_zeroing() const { return state_ == ZEROING; }
+ // True if the code is moving off the hall effect.
+ bool is_moving_off() const { return state_ == MOVING_OFF; }
+
// True if the state machine is uninitialized.
bool is_uninitialized() const { return state_ == UNINITIALIZED; }
@@ -128,6 +129,9 @@
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return capped_goal_; }
+ // Timestamp
+ static const double dt;
+
private:
friend class ZeroedStateFeedbackLoop<kNumZeroSensors>;
// Friend the wrist test cases so that they can simulate windeup.
@@ -139,6 +143,30 @@
ConfigurationData config_data_;
+ // Returns the index of the first active sensor, or -1 if none are active.
+ int ActiveSensorIndex(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ if (!position) {
+ return -1;
+ }
+ int active_index = -1;
+ for (int i = 0; i < kNumZeroSensors; ++i) {
+ if (position->hall_effects[i]) {
+ if (active_index != -1) {
+ LOG(ERROR, "More than one hall effect sensor is active\n");
+ } else {
+ active_index = i;
+ }
+ }
+ }
+ return active_index;
+ }
+ // Returns true if any of the sensors are active.
+ bool AnySensorsActive(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ return ActiveSensorIndex(position) != -1;
+ }
+
// Enum to store the state of the internal zeroing state machine.
enum State {
UNINITIALIZED,
@@ -164,9 +192,21 @@
// True if the zeroing goal was capped during this cycle.
bool capped_goal_;
+ // Returns true if number is between first and second inclusive.
+ bool is_between(double first, double second, double number) {
+ if ((number >= first || number >= second) &&
+ (number <= first || number <= second)) {
+ return true;
+ }
+ return false;
+ }
+
DISALLOW_COPY_AND_ASSIGN(ZeroedJoint);
};
+template <int kNumZeroSensors>
+/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
+
// Updates the zeroed joint controller and state machine.
template <int kNumZeroSensors>
double ZeroedJoint<kNumZeroSensors>::Update(
@@ -193,10 +233,8 @@
absolute_position -= zero_offset_;
}
loop_->Y << absolute_position;
- for (int i = 0; i < kNumZeroSensors; ++i) {
- if (!position->hall_effects[i]) {
- last_off_position_ = position->position;
- }
+ if (!AnySensorsActive(position)) {
+ last_off_position_ = position->position;
}
} else {
// Dead recon for now.
@@ -205,7 +243,6 @@
switch (state_) {
case UNINITIALIZED:
- printf("uninit\n");
if (position) {
// Reset the zeroing goal.
zeroing_position_ = absolute_position;
@@ -215,82 +252,83 @@
loop_->R = loop_->X_hat;
// Only progress if we are enabled.
if (::aos::robot_state->enabled) {
- state_ = ZEROING;
- for (int i = 0; i < kNumZeroSensors; ++i) {
- if (position->hall_effects[i]) {
- state_ = MOVING_OFF;
- }
+ if (AnySensorsActive(position)) {
+ state_ = MOVING_OFF;
+ } else {
+ state_ = ZEROING;
}
}
}
break;
case MOVING_OFF:
- printf("moving off\n");
- // Move off the hall effect sensor.
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && !position->hall_effects[0]) {
- // We are now off the sensor. Time to zero now.
- state_ = ZEROING;
- } else {
- // Slowly creep off the sensor.
- zeroing_position_ -= config_data_.zeroing_speed / 100;
- loop_->R << zeroing_position_, -config_data_.zeroing_speed;
- break;
+ {
+ // Move off the hall effect sensor.
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && !AnySensorsActive(position)) {
+ // We are now off the sensor. Time to zero now.
+ state_ = ZEROING;
+ } else {
+ // Slowly creep off the sensor.
+ zeroing_position_ -= config_data_.zeroing_speed * dt;
+ loop_->R << zeroing_position_, -config_data_.zeroing_speed;
+ break;
+ }
}
case ZEROING:
- printf("zeroing\n");
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && position->hall_effects[0]) {
- 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 (position->hall_effect_positions[0] <= last_off_position_ ||
- position->hall_effect_positions[0] > position->position) {
- LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
- LOG(ERROR,
- "Last off position was %f, current is %f, calibration is %f\n",
- last_off_position_, position->position,
- position->hall_effect_positions[0]);
- state_ = MOVING_OFF;
+ {
+ int active_sensor_index = ActiveSensorIndex(position);
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && active_sensor_index != -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.
+ const double calibration =
+ position->hall_effect_positions[active_sensor_index];
+ if (!is_between(last_off_position_, position->position,
+ calibration)) {
+ LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
+ LOG(ERROR,
+ "Last off position was %f, current is %f, calibration is %f\n",
+ last_off_position_, position->position,
+ position->hall_effect_positions[active_sensor_index]);
+ 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_ =
+ position->hall_effect_positions[active_sensor_index] -
+ config_data_.hall_effect_start_angle[active_sensor_index];
+ loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
+ loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ }
} 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_ = (position->hall_effect_positions[0] -
- config_data_.hall_effect_start_angle[0]);
- loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
- loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ // Slowly creep towards the sensor.
+ zeroing_position_ += config_data_.zeroing_speed * dt;
+ loop_->R << zeroing_position_, config_data_.zeroing_speed;
}
- } else {
- // Slowly creep towards the sensor.
- zeroing_position_ += config_data_.zeroing_speed / 100;
- loop_->R << zeroing_position_, config_data_.zeroing_speed;
+ break;
}
- break;
case READY:
{
- printf("ready\n");
const double limited_goal = ClipGoal(goal_angle);
loop_->R << limited_goal, goal_velocity;
break;
}
case ESTOP:
- printf("estop\n");
LOG(WARNING, "have already given up\n");
return 0.0;
}
// Update the observer.
- printf("Output is enabled %d\n", output_enabled);
loop_->Update(position != NULL, !output_enabled);
- printf("Voltage %f\n", loop_->U(0, 0));
capped_goal_ = false;
// Verify that the zeroing goal hasn't run away.