Split the zeroed joint out into a seperate class that the wrist now uses.
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index b49e99c..85cc221 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -18,59 +18,33 @@
WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
: aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- loop_(new WristStateFeedbackLoop(MakeWristLoop(), this)),
- state_(UNINITIALIZED),
- error_count_(0),
- zero_offset_(0.0),
- capped_goal_(false) {
+ zeroed_joint_(MakeWristLoop()) {
}
-bool WristMotor::FetchConstants() {
- if (!constants::wrist_lower_limit(&wrist_lower_limit_)) {
+bool WristMotor::FetchConstants(
+ ZeroedJoint<1>::ConfigurationData *config_data) {
+ if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
return false;
}
- if (!constants::wrist_upper_limit(&wrist_upper_limit_)) {
+ if (!constants::wrist_upper_limit(&config_data->upper_limit)) {
LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
return false;
}
if (!constants::wrist_hall_effect_start_angle(
- &wrist_hall_effect_start_angle_)) {
+ &config_data->hall_effect_start_angle[0])) {
LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
return false;
}
- if (!constants::wrist_zeroing_speed(
- &wrist_zeroing_speed_)) {
+ if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
return false;
}
+ config_data->max_zeroing_voltage = 5.0;
return true;
}
-double WristMotor::ClipGoal(double goal) const {
- return std::min(wrist_upper_limit_,
- std::max(wrist_lower_limit_, goal));
-}
-
-const double kMaxZeroingVoltage = 5.0;
-
-void WristMotor::WristStateFeedbackLoop::CapU() {
- if (wrist_motor_->state_ == READY) {
- if (Y(0, 0) >= wrist_motor_->wrist_upper_limit_) {
- U(0, 0) = std::min(0.0, U(0, 0));
- }
- if (Y(0, 0) <= wrist_motor_->wrist_lower_limit_) {
- U(0, 0) = std::max(0.0, U(0, 0));
- }
- }
-
- double limit = (wrist_motor_->state_ == READY) ? 12.0 : kMaxZeroingVoltage;
-
- U(0, 0) = std::min(limit, U(0, 0));
- U(0, 0) = std::max(-limit, U(0, 0));
-}
-
// Positive angle is up, and positive power is up.
void WristMotor::RunIteration(
const ::aos::control_loops::Goal *goal,
@@ -85,151 +59,38 @@
}
// Cache the constants to avoid error handling down below.
- if (!FetchConstants()) {
+ ZeroedJoint<1>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
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_);
- state_ = UNINITIALIZED;
+ zeroed_joint_.set_config_data(config_data);
}
- // Compute the absolute position of the wrist.
- double absolute_position;
- if (position) {
- absolute_position =
- position->pos + wrist_hall_effect_start_angle_;
- if (state_ == READY) {
- absolute_position -= zero_offset_;
- }
- loop_->Y << absolute_position;
- if (!position->hall_effect) {
- last_off_position_ = position->pos;
- }
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
} else {
- // Dead recon for now.
- absolute_position = loop_->X_hat(0, 0);
+ 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);
}
- switch (state_) {
- case UNINITIALIZED:
- if (position) {
- // Reset the zeroing goal.
- zeroing_position_ = absolute_position;
- // Clear the observer state.
- loop_->X_hat << absolute_position, 0.0;
- // Set the goal to here to make it so it doesn't move when disabled.
- loop_->R = loop_->X_hat;
- // Only progress if we are enabled.
- if (::aos::robot_state->enabled) {
- if (position->hall_effect) {
- state_ = MOVING_OFF;
- } else {
- state_ = ZEROING;
- }
- }
- }
- break;
- case MOVING_OFF:
- // Move off the hall effect sensor.
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && !position->hall_effect) {
- // We are now off the sensor. Time to zero now.
- state_ = ZEROING;
- } else {
- // Slowly creep off the sensor.
- zeroing_position_ -= wrist_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, -wrist_zeroing_speed_;
- break;
- }
- case ZEROING:
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && position->hall_effect) {
- 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->calibration <= last_off_position_ ||
- position->calibration > position->pos) {
- 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->pos, position->calibration);
- 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->calibration;
- loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
- loop_->Y(0, 0) += old_zero_offset - zero_offset_;
- }
- } else {
- // Slowly creep towards the sensor.
- zeroing_position_ += wrist_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, wrist_zeroing_speed_;
- }
- break;
-
- case READY:
- {
- const double limited_goal = ClipGoal(goal->goal);
- loop_->R << limited_goal, 0.0;
- break;
- }
-
- case ESTOP:
- LOG(WARNING, "have already given up\n");
- return;
- }
-
- // Update the observer.
- loop_->Update(position != NULL, output == NULL);
-
- capped_goal_ = false;
- // Verify that the zeroing goal hasn't run away.
- switch (state_) {
- case UNINITIALIZED:
- case READY:
- case ESTOP:
- // Not zeroing. No worries.
- break;
- case MOVING_OFF:
- case ZEROING:
- // Check if we have cliped and adjust the goal.
- if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
- double dx = (loop_->U_uncapped(0, 0) -
- kMaxZeroingVoltage) / loop_->K(0, 0);
- zeroing_position_ -= dx;
- capped_goal_ = true;
- } else if(loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
- double dx = (loop_->U_uncapped(0, 0) +
- kMaxZeroingVoltage) / loop_->K(0, 0);
- zeroing_position_ -= dx;
- capped_goal_ = true;
- }
- break;
- }
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
if (position) {
- LOG(DEBUG, "pos=%f zero=%f currently %f hall: %s\n",
- position->pos, zero_offset_, absolute_position,
- position->hall_effect ? "true" : "false");
+ //LOG(DEBUG, "pos=%f zero=%f currently %f hall: %s\n",
+ //position->pos, zero_offset_, absolute_position,
+ //position->hall_effect ? "true" : "false");
}
if (output) {
- output->voltage = loop_->U(0, 0);
+ 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 55d14c8..1dd9afd 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -8,19 +8,15 @@
#include "frc971/control_loops/wrist/wrist_motor.q.h"
#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+#include "frc971/control_loops/zeroed_joint.h"
+
namespace frc971 {
namespace control_loops {
-
namespace testing {
-class WristTest_RezeroWithMissingPos_Test;
-class WristTest_DisableGoesUninitialized_Test;
-class WristTest_NoWindup_Test;
class WristTest_NoWindupPositive_Test;
class WristTest_NoWindupNegative_Test;
};
-class WristMotor;
-
class WristMotor
: public aos::control_loops::ControlLoop<control_loops::WristLoop> {
public:
@@ -28,7 +24,16 @@
control_loops::WristLoop *my_wrist = &control_loops::wrist);
// True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return capped_goal_; }
+ 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 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(); }
protected:
virtual void RunIteration(
@@ -39,68 +44,14 @@
private:
// Friend the test classes for acces to the internal state.
- friend class testing::WristTest_RezeroWithMissingPos_Test;
- friend class testing::WristTest_DisableGoesUninitialized_Test;
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
- friend class WristStateFeedbackLoop;
// Fetches and locally caches the latest set of constants.
- bool FetchConstants();
+ bool FetchConstants(ZeroedJoint<1>::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;
-
- // This class implements the CapU function correctly given all the extra
- // information that we know about from the wrist motor.
- class WristStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
- public:
- WristStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
- WristMotor *wrist_motor)
- : StateFeedbackLoop<2, 1, 1>(loop),
- wrist_motor_(wrist_motor) {
- }
-
- // Caps U, but this time respects the state of the wrist as well.
- virtual void CapU();
- private:
- WristMotor *wrist_motor_;
- };
-
- // The state feedback control loop to talk to.
- ::std::unique_ptr<WristStateFeedbackLoop> loop_;
-
- // Enum to store the state of the internal zeroing state machine.
- enum State {
- UNINITIALIZED,
- MOVING_OFF,
- ZEROING,
- READY,
- ESTOP
- };
-
- // Internal state for zeroing.
- State state_;
-
- // Missed position packet count.
- int error_count_;
- // Offset from the raw encoder value to the absolute angle.
- double zero_offset_;
- // Position that gets incremented when zeroing the wrist to slowly move it to
- // the hall effect sensor.
- double zeroing_position_;
- // Last position at which the hall effect sensor was off.
- double last_off_position_;
-
- // Local cache of the wrist geometry constants.
- double wrist_lower_limit_;
- double wrist_upper_limit_;
- double wrist_hall_effect_start_angle_;
- double wrist_zeroing_speed_;
-
- // True if the zeroing goal was capped during this cycle.
- bool capped_goal_;
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
DISALLOW_COPY_AND_ASSIGN(WristMotor);
};
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index aa0cb78..83703fa 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -218,12 +218,12 @@
} else {
if (i > 310) {
// Should be re-zeroing now.
- EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
}
my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
}
if (i == 430) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
}
wrist_motor_.Iterate();
@@ -245,7 +245,7 @@
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(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
// When disabled, we should be applying 0 power.
EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
EXPECT_EQ(0, my_wrist_loop_.output->voltage);
@@ -255,7 +255,7 @@
}
if (i == 202) {
// Verify that we are zeroing after the bot gets enabled again.
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
}
wrist_motor_.Iterate();
@@ -273,26 +273,26 @@
for (int i = 0; i < 500; ++i) {
wrist_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroing_position_;
- wrist_motor_.zeroing_position_ = -100.0;
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
} else if (i == 51) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position,
+ wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
- if (wrist_motor_.state_ != WristMotor::READY) {
+ if (!wrist_motor_.is_ready()) {
if (wrist_motor_.capped_goal()) {
++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
} else {
- ASSERT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
}
}
-
wrist_motor_.Iterate();
wrist_motor_plant_.Simulate();
SendDSPacket(true);
@@ -310,24 +310,24 @@
for (int i = 0; i < 500; ++i) {
wrist_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroing_position_;
- wrist_motor_.zeroing_position_ = 100.0;
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
} else {
if (i == 51) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
}
- if (wrist_motor_.state_ != WristMotor::READY) {
+ if (!wrist_motor_.is_ready()) {
if (wrist_motor_.capped_goal()) {
++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
} else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
}
}
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
new file mode 100644
index 0000000..f56c84a
--- /dev/null
+++ b/frc971/control_loops/zeroed_joint.h
@@ -0,0 +1,325 @@
+#ifndef FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+#define FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+
+#include <memory>
+
+#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 {
+namespace testing {
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
+};
+
+template<int kNumZeroSensors>
+class ZeroedJoint;
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+template<int kNumZeroSensors>
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ zeroed_joint_(zeroed_joint) {
+ }
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+ private:
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
+};
+
+template<int kNumZeroSensors>
+void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
+ if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
+ if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
+ U(0, 0) = std::min(0.0, U(0, 0));
+ }
+ if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
+ U(0, 0) = std::max(0.0, U(0, 0));
+ }
+ }
+
+ const bool is_ready =
+ zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
+ double limit = is_ready ?
+ 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
+
+ U(0, 0) = std::min(limit, U(0, 0));
+ U(0, 0) = std::max(-limit, U(0, 0));
+}
+
+
+// Class to zero and control a joint with any number of zeroing sensors with a
+// state feedback controller.
+template<int kNumZeroSensors>
+class ZeroedJoint {
+ public:
+ // Sturcture to hold the hardware configuration information.
+ struct ConfigurationData {
+ // Angle at the lower hardware limit.
+ double lower_limit;
+ // Angle at the upper hardware limit.
+ double upper_limit;
+ // Speed (and direction) to move while zeroing.
+ double zeroing_speed;
+ // Maximum voltage to apply when zeroing.
+ double max_zeroing_voltage;
+ // Angles where we see a positive edge from the hall effect sensors.
+ double hall_effect_start_angle[kNumZeroSensors];
+ };
+
+ // Current position data for the encoder and hall effect information.
+ struct PositionData {
+ // Current encoder position.
+ double position;
+ // Array of hall effect values.
+ bool hall_effects[kNumZeroSensors];
+ // Array of the last positive edge position for the sensors.
+ double hall_effect_positions[kNumZeroSensors];
+ };
+
+ ZeroedJoint(StateFeedbackLoop<2, 1, 1> loop)
+ : loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
+ state_(UNINITIALIZED),
+ error_count_(0),
+ zero_offset_(0.0),
+ capped_goal_(false) {
+ }
+
+ // Copies the provided configuration data locally.
+ void set_config_data(const ConfigurationData &config_data) {
+ config_data_ = 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 {
+ return ::std::min(config_data_.upper_limit,
+ std::max(config_data_.lower_limit, goal));
+ }
+
+ // Updates the loop and state machine.
+ // position is null if the position data is stale, output_enabled is true if
+ // the output will actually go to the motors, and goal_angle and goal_velocity
+ // are the goal position and velocities.
+ double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity);
+
+ // True if the code is zeroing.
+ bool is_zeroing() const { return state_ == ZEROING; }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return state_ == UNINITIALIZED; }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return state_ == READY; }
+
+ // Returns the uncapped voltage.
+ double U_uncapped() const { return loop_->U_uncapped(0, 0); }
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return capped_goal_; }
+
+ private:
+ friend class ZeroedStateFeedbackLoop<kNumZeroSensors>;
+ // Friend the wrist test cases so that they can simulate windeup.
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
+
+ ConfigurationData config_data_;
+
+ // Enum to store the state of the internal zeroing state machine.
+ enum State {
+ UNINITIALIZED,
+ MOVING_OFF,
+ ZEROING,
+ READY,
+ ESTOP
+ };
+
+ // Internal state for zeroing.
+ State state_;
+
+ // Missed position packet count.
+ int error_count_;
+ // Offset from the raw encoder value to the absolute angle.
+ double zero_offset_;
+ // Position that gets incremented when zeroing the wrist to slowly move it to
+ // the hall effect sensor.
+ double zeroing_position_;
+ // Last position at which the hall effect sensor was off.
+ double last_off_position_;
+
+ // True if the zeroing goal was capped during this cycle.
+ bool capped_goal_;
+
+ DISALLOW_COPY_AND_ASSIGN(ZeroedJoint);
+};
+
+// Updates the zeroed joint controller and state machine.
+template <int kNumZeroSensors>
+double ZeroedJoint<kNumZeroSensors>::Update(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity) {
+ // 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_);
+ state_ = UNINITIALIZED;
+ }
+
+ // Compute the absolute position of the wrist.
+ double absolute_position;
+ if (position) {
+ absolute_position = position->position;
+ if (state_ == READY) {
+ 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;
+ }
+ }
+ } else {
+ // Dead recon for now.
+ absolute_position = loop_->X_hat(0, 0);
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ printf("uninit\n");
+ if (position) {
+ // Reset the zeroing goal.
+ zeroing_position_ = absolute_position;
+ // Clear the observer state.
+ loop_->X_hat << absolute_position, 0.0;
+ // Set the goal to here to make it so it doesn't move when disabled.
+ 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;
+ }
+ }
+ }
+ }
+ 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;
+ }
+ 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;
+ } 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_;
+ }
+ } else {
+ // Slowly creep towards the sensor.
+ zeroing_position_ += config_data_.zeroing_speed / 100;
+ loop_->R << zeroing_position_, config_data_.zeroing_speed;
+ }
+ 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.
+ switch (state_) {
+ case UNINITIALIZED:
+ case READY:
+ case ESTOP:
+ // Not zeroing. No worries.
+ break;
+ case MOVING_OFF:
+ case ZEROING:
+ // Check if we have cliped and adjust the goal.
+ if (loop_->U_uncapped(0, 0) > config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) -
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ } else if(loop_->U_uncapped(0, 0) < -config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) +
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ }
+ break;
+ }
+ return loop_->U(0, 0);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_