blob: f56c84ac701738cdcb44791f0da097b677389231 [file] [log] [blame]
#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_