Claw gets further when zeroing.
- Claw now roughly finds out where it is.
- The state feedback loop now has a Correct method which takes the
measurement to use.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 815bebb..14745b5 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -53,7 +53,7 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
- 1.0,
+ 0.5,
0.1,
0.0,
1.57,
@@ -72,7 +72,7 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
- 1.0,
+ 0.5,
0.1,
0.0,
1.57,
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index fb16c51..8c35733 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -52,7 +52,7 @@
uncapped_voltage_ = voltage_;
- double limit = zeroing_state_ != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+ double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
// Make sure that reality and the observer can't get too far off. There is a
// delay by one cycle between the applied voltage and X_hat(2, 0), so compare
@@ -68,8 +68,8 @@
voltage_ = std::min(limit, voltage_);
voltage_ = std::max(-limit, voltage_);
U(0, 0) = voltage_ - old_voltage;
- //LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
- //LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+ LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
+ LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
last_voltage_ = voltage_;
}
@@ -86,11 +86,77 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
+ const constants::Values::Claw &claw_values, double *edge_encoder,
+ double *edge_angle) {
+
+ // TODO(austin): Validate that the hall effect edge makes sense.
+ // We must now be on the side of the edge that we expect to be, and the
+ // encoder must have been on either side of the edge before and after.
+
+ if (front_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.front.upper_angle;
+ } else {
+ *edge_angle = claw_values.front.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (front_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.front.upper_angle;
+ } else {
+ *edge_angle = claw_values.front.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ if (calibration_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.calibration.upper_angle;
+ } else {
+ *edge_angle = claw_values.calibration.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (calibration_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.calibration.upper_angle;
+ } else {
+ *edge_angle = claw_values.calibration.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ if (back_hall_effect_posedge_count_changed()) {
+ if (encoder() - last_encoder() < 0) {
+ *edge_angle = claw_values.back.upper_angle;
+ } else {
+ *edge_angle = claw_values.back.lower_angle;
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (back_hall_effect_negedge_count_changed()) {
+ if (encoder() - last_encoder() > 0) {
+ *edge_angle = claw_values.back.upper_angle;
+ } else {
+ *edge_angle = claw_values.back.lower_angle;
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+ return false;
+}
+
// Positive angle is up, and positive power is up.
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
::aos::control_loops::Status *status) {
+ constexpr double dt = 0.01;
// Disable the motors now so that all early returns will return with the
// motors disabled.
@@ -114,7 +180,12 @@
return;
}
+ const frc971::constants::Values &values = constants::GetValues();
+
if (position) {
+ top_claw_.SetPositionValues(position->top);
+ bottom_claw_.SetPositionValues(position->bottom);
+
if (!has_top_claw_goal_) {
has_top_claw_goal_ = true;
top_claw_goal_ = position->top.position;
@@ -123,35 +194,10 @@
has_bottom_claw_goal_ = true;
bottom_claw_goal_ = position->bottom.position;
}
-
- top_claw_.set_front_hall_effect_posedge_count(
- position->top.front_hall_effect_posedge_count);
- top_claw_.set_front_hall_effect_negedge_count(
- position->top.front_hall_effect_negedge_count);
- top_claw_.set_calibration_hall_effect_posedge_count(
- position->top.calibration_hall_effect_posedge_count);
- top_claw_.set_calibration_hall_effect_negedge_count(
- position->top.calibration_hall_effect_negedge_count);
- top_claw_.set_back_hall_effect_posedge_count(
- position->top.back_hall_effect_posedge_count);
- top_claw_.set_back_hall_effect_negedge_count(
- position->top.back_hall_effect_negedge_count);
-
- bottom_claw_.set_front_hall_effect_posedge_count(
- position->bottom.front_hall_effect_posedge_count);
- bottom_claw_.set_front_hall_effect_negedge_count(
- position->bottom.front_hall_effect_negedge_count);
- bottom_claw_.set_calibration_hall_effect_posedge_count(
- position->bottom.calibration_hall_effect_posedge_count);
- bottom_claw_.set_calibration_hall_effect_negedge_count(
- position->bottom.calibration_hall_effect_negedge_count);
- bottom_claw_.set_back_hall_effect_posedge_count(
- position->bottom.back_hall_effect_posedge_count);
- bottom_claw_.set_back_hall_effect_negedge_count(
- position->bottom.back_hall_effect_negedge_count);
}
bool autonomous = ::aos::robot_state->autonomous;
+ bool enabled = ::aos::robot_state->enabled;
if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
@@ -164,6 +210,8 @@
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
// Ready to use the claw.
// Limit the goals here.
+ bottom_claw_goal_ = goal->bottom_angle;
+ top_claw_goal_ = goal->bottom_angle + goal->seperation_angle;
} else if (top_claw_.zeroing_state() !=
ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
@@ -175,9 +223,18 @@
}
} else {
if (!was_enabled_ && enabled) {
-
+ if (position) {
+ top_claw_goal_ = position->top.position;
+ bottom_claw_goal_ = position->bottom.position;
+ } else {
+ has_top_claw_goal_ = false;
+ has_bottom_claw_goal_ = false;
+ }
}
- // Limit the goals here.
+
+ // TODO(austin): Limit the goals here.
+ // Need to prevent windup, limit voltage, deal with windup on only 1 claw,
+ // ...
if (top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
}
@@ -187,28 +244,49 @@
if (bottom_claw_.zeroing_state() !=
ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
- // Time to slowly move back up to find any position to narrow down the
- // zero.
+ if (enabled) {
+ // Time to slowly move back up to find any position to narrow down the
+ // zero.
+ top_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ // TODO(austin): Goal velocity too!
+ }
} else {
// We don't know where either claw is. Slowly start moving down to find
// any hall effect.
- LOG(INFO, "Unknown position\n");
+ if (enabled) {
+ top_claw_goal_-= values.claw_zeroing_off_speed * dt;
+ bottom_claw_goal_ -= values.claw_zeroing_off_speed * dt;
+ // TODO(austin): Goal velocity too!
+ }
+ }
+
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ top_claw_.SetCalibrationOnEdge(
+ values.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
}
}
// TODO(austin): Handle disabled.
- if (position) {
- top_claw_.Y << position->top.position;
- bottom_claw_.Y << position->bottom.position;
- }
-
// TODO(austin): ...
- top_claw_.R << goal->bottom_angle + goal->seperation_angle, 0.0, 0.0;
- bottom_claw_.R << goal->bottom_angle, 0.0, 0.0;
+ if (has_top_claw_goal_ && has_bottom_claw_goal_) {
+ top_claw_.R << top_claw_goal_, 0.0, 0.0;
+ bottom_claw_.R << bottom_claw_goal_, 0.0, 0.0;
- top_claw_.Update(position != nullptr, output == nullptr);
- bottom_claw_.Update(position != nullptr, output == nullptr);
+ top_claw_.Update(output == nullptr);
+ bottom_claw_.Update(output == nullptr);
+ } else {
+ top_claw_.ZeroPower();
+ bottom_claw_.ZeroPower();
+ }
if (position) {
//LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index fc5e917..b585fbb 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/top_claw_motor_plant.h"
@@ -32,6 +33,7 @@
voltage_(0.0),
last_voltage_(0.0),
uncapped_voltage_(0.0),
+ offset_(0.0),
front_hall_effect_posedge_count_(0.0),
previous_front_hall_effect_posedge_count_(0.0),
front_hall_effect_negedge_count_(0.0),
@@ -44,7 +46,11 @@
previous_back_hall_effect_posedge_count_(0.0),
back_hall_effect_negedge_count_(0.0),
previous_back_hall_effect_negedge_count_(0.0),
- zeroing_state_(UNKNOWN_POSITION) {}
+ zeroing_state_(UNKNOWN_POSITION),
+ posedge_value_(0.0),
+ negedge_value_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0) {}
const static int kZeroingMaxVoltage = 5;
@@ -77,6 +83,42 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle) {
+ offset_ = edge_angle - edge_encoder;
+ }
+
+ bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+ }
+
+ void SetPositionValues(const Claw &claw) {
+ set_front_hall_effect_posedge_count(claw.front_hall_effect_posedge_count);
+ set_front_hall_effect_negedge_count(claw.front_hall_effect_negedge_count);
+ set_calibration_hall_effect_posedge_count(
+ claw.calibration_hall_effect_posedge_count);
+ set_calibration_hall_effect_negedge_count(
+ claw.calibration_hall_effect_negedge_count);
+ set_back_hall_effect_posedge_count(claw.back_hall_effect_posedge_count);
+ set_back_hall_effect_negedge_count(claw.back_hall_effect_negedge_count);
+
+ posedge_value_ = claw.posedge_value;
+ negedge_value_ = claw.negedge_value;
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << claw.position;
+ Correct(Y);
+ }
+
#define COUNT_SETTER_GETTER(variable) \
void set_##variable(int32_t value) { \
previous_##variable##_ = variable##_; \
@@ -87,6 +129,7 @@
return previous_##variable##_ != variable##_; \
}
+ // TODO(austin): Pull all this out of the new sub structure.
COUNT_SETTER_GETTER(front_hall_effect_posedge_count);
COUNT_SETTER_GETTER(front_hall_effect_negedge_count);
COUNT_SETTER_GETTER(calibration_hall_effect_posedge_count);
@@ -103,6 +146,15 @@
back_hall_effect_negedge_count_changed();
}
+ double position() const { return X_hat(0, 0); }
+ double encoder() const { return encoder_; }
+ double last_encoder() const { return last_encoder_; }
+
+ // Returns true if an edge was detected in the last cycle and then sets the
+ // edge_position to the absolute position of the edge.
+ bool GetPositionOfEdge(const constants::Values::Claw &claw,
+ double *edge_encoder, double *edge_angle);
+
#undef COUNT_SETTER_GETTER
private:
@@ -110,6 +162,9 @@
double voltage_;
double last_voltage_;
double uncapped_voltage_;
+ double offset_;
+
+ double previous_position_;
int32_t front_hall_effect_posedge_count_;
int32_t previous_front_hall_effect_posedge_count_;
@@ -125,6 +180,10 @@
int32_t previous_back_hall_effect_negedge_count_;
JointZeroingState zeroing_state_;
+ double posedge_value_;
+ double negedge_value_;
+ double encoder_;
+ double last_encoder_;
};
class ClawMotor
@@ -142,8 +201,8 @@
control_loops::ClawGroup::Output *output,
::aos::control_loops::Status *status);
- double top_absolute_position() const { return top_claw_.X_hat(0, 0); }
- double bottom_absolute_position() const { return bottom_claw_.X_hat(0, 0); }
+ double top_absolute_position() const { return top_claw_.position(); }
+ double bottom_absolute_position() const { return bottom_claw_.position(); }
private:
// Friend the test classes for acces to the internal state.
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 9e8e097..30155f5 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -46,27 +46,14 @@
void Reinitialize(double initial_top_position,
double initial_bottom_position) {
+ LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
+ initial_bottom_position);
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
last_position_.Zero();
SetPhysicalSensors(&last_position_);
}
- // Resets the plant so that it starts at initial_position.
- void ReinitializePartial(ClawType type, double initial_position) {
- StateFeedbackPlant<2, 1, 1>* plant;
- if (type == TOP_CLAW) {
- plant = top_claw_plant_.get();
- } else {
- plant = bottom_claw_plant_.get();
- }
- initial_position_[type] = initial_position;
- plant->X(0, 0) = initial_position_[type];
- plant->X(1, 0) = 0.0;
- plant->Y = plant->C() * plant->X;
- last_voltage_[type] = 0.0;
- }
-
// Returns the absolute angle of the wrist.
double GetAbsolutePosition(ClawType type) const {
if (type == TOP_CLAW) {
@@ -94,6 +81,7 @@
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
+ LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n", pos[TOP_CLAW], pos[BOTTOM_CLAW]);
const frc971::constants::Values& values = constants::GetValues();
@@ -119,6 +107,9 @@
::aos::ScopedMessagePtr<control_loops::ClawGroup::Position> position =
claw_queue_group.position.MakeMessage();
+ // Initialize all the counters to their previous values.
+ *position = last_position_;
+
SetPhysicalSensors(position.get());
const frc971::constants::Values& values = constants::GetValues();
@@ -301,6 +292,21 @@
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> bottom_claw_plant_;
private:
+ // Resets the plant so that it starts at initial_position.
+ void ReinitializePartial(ClawType type, double initial_position) {
+ StateFeedbackPlant<2, 1, 1>* plant;
+ if (type == TOP_CLAW) {
+ plant = top_claw_plant_.get();
+ } else {
+ plant = bottom_claw_plant_.get();
+ }
+ initial_position_[type] = initial_position;
+ plant->X(0, 0) = initial_position_[type];
+ plant->X(1, 0) = 0.0;
+ plant->Y = plant->C() * plant->X;
+ last_voltage_[type] = 0.0;
+ }
+
void Simulate(ClawType type, StateFeedbackPlant<2, 1, 1>* plant,
const constants::Values::Claw &claw, double nl_voltage) {
plant->U << last_voltage_[type];
@@ -346,7 +352,7 @@
".frc971.control_loops.claw_queue_group.output",
".frc971.control_loops.claw_queue_group.status"),
claw_motor_(&claw_queue_group),
- claw_motor_plant_(0.2, 0.4),
+ claw_motor_plant_(0.4, 0.2),
min_seperation_(constants::GetValues().claw_min_seperation) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
@@ -394,7 +400,7 @@
// Tests that the wrist zeros correctly starting on the hall effect sensor.
TEST_F(ClawTest, ZerosStartingOn) {
- claw_motor_plant_.Reinitialize(90 * M_PI / 180.0, 100 * M_PI / 180.0);
+ claw_motor_plant_.Reinitialize(100 * M_PI / 180.0, 90 * M_PI / 180.0);
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.1)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4a05f7a..982c6e9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -93,7 +93,9 @@
void SetRawPosition(double left, double right) {
_raw_right = right;
_raw_left = left;
- loop_->Y << left, right;
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left, right;
+ loop_->Correct(Y);
}
void SetPosition(
double left, double right, double gyro, bool control_loop_driving) {
@@ -108,8 +110,8 @@
SetRawPosition(left, right);
}
- void Update(bool update_observer, bool stop_motors) {
- loop_->Update(update_observer, stop_motors);
+ void Update(bool stop_motors) {
+ loop_->Update(stop_motors);
}
void SendMotors(Drivetrain::Output *output) {
@@ -606,7 +608,7 @@
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(position, output == NULL);
+ dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
if (control_loop_driving) {
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 38ca89c..420a0e7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -218,7 +218,6 @@
Eigen::Matrix<double, number_of_inputs, 1> U;
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -237,7 +236,6 @@
U.setZero();
U_uncapped.setZero();
U_ff.setZero();
- Y.setZero();
}
StateFeedbackLoop(const StateFeedbackController<
@@ -286,9 +284,14 @@
}
}
- // update_observer is whether or not to use the values in Y.
+ // Corrects X_hat given the observation in Y.
+ void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ Y_ = Y;
+ new_y_ = true;
+ }
+
// stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
+ void Update(bool stop_motors) {
if (stop_motors) {
U.setZero();
} else {
@@ -296,8 +299,9 @@
CapU();
}
- if (update_observer) {
- X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
+ if (new_y_) {
+ X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
+ new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
@@ -326,6 +330,11 @@
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+ // Temporary storage for a measurement until I can figure out why I can't
+ // correct when the measurement is made.
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ bool new_y_ = false;
+
int controller_index_;
};