Claw now zeros!
- Debugged plant
- Switched to a simple controller architecture.
- Fixed controller.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index e3e7e6d..96f4447 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -8,8 +8,11 @@
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/claw/top_claw_motor_plant.h"
-#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+// TODO(austin): Switch the control loop over to a seperation and a bottom
+// position. This will make things a lot more robust and allows for different
+// stiffnesses.
// Zeroing plan.
// There are 2 types of zeros. Enabled and disabled ones.
@@ -46,42 +49,24 @@
namespace frc971 {
namespace control_loops {
-void ZeroedStateFeedbackLoop::CapU() {
- const double old_voltage = voltage_;
- voltage_ += U(0, 0);
-
- uncapped_voltage_ = voltage_;
-
- 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
- // against last cycle's voltage.
- if (X_hat(2, 0) > last_voltage_ + 2.0) {
- voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
- LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
- } else if (X_hat(2, 0) < last_voltage_ -2.0) {
- voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
- LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+void ClawLimitedLoop::CapU() {
+ double max_value = ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
+ if (max_value > 12.0) {
+ LOG(DEBUG, "Capping U because max is %f\n", max_value);
+ U = U * 12.0 / max_value;
+ LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
}
-
- 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));
-
- last_voltage_ = voltage_;
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
: aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
- top_claw_(MakeTopClawLoop()),
+ top_claw_(this),
has_bottom_claw_goal_(false),
bottom_claw_goal_(0.0),
- bottom_claw_(MakeBottomClawLoop()),
+ bottom_claw_(this),
+ claw_(MakeClawLoop()),
was_enabled_(false),
doing_calibration_fine_tune_(false) {}
@@ -95,56 +80,74 @@
// 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.
+ // TODO(austin): Compute the last off range min and max and compare the edge
+ // value to the middle of the range. This will be quite a bit more reliable.
+
if (front_hall_effect_posedge_count_changed()) {
- if (encoder() - last_encoder() < 0) {
+ if (posedge_value_ - last_encoder() < 0) {
*edge_angle = claw_values.front.upper_angle;
+ LOG(INFO, "%s Posedge front upper edge -> %f\n", name_, *edge_angle);
} else {
*edge_angle = claw_values.front.lower_angle;
+ LOG(INFO, "%s Posedge front lower edge -> %f\n", name_, *edge_angle);
}
*edge_encoder = posedge_value_;
return true;
}
if (front_hall_effect_negedge_count_changed()) {
- if (encoder() - last_encoder() > 0) {
+ LOG(INFO, "%s Value is %f last is %f\n", name_, negedge_value_, last_encoder());
+ if (negedge_value_ - last_encoder() > 0) {
*edge_angle = claw_values.front.upper_angle;
+ LOG(INFO, "%s Negedge front upper edge -> %f\n", name_, *edge_angle);
} else {
*edge_angle = claw_values.front.lower_angle;
+ LOG(INFO, "%s Negedge front lower edge -> %f\n", name_, *edge_angle);
}
*edge_encoder = negedge_value_;
return true;
}
if (calibration_hall_effect_posedge_count_changed()) {
- if (encoder() - last_encoder() < 0) {
+ if (posedge_value_ - last_encoder() < 0) {
*edge_angle = claw_values.calibration.upper_angle;
+ LOG(INFO, "%s Posedge calibration upper edge -> %f\n", name_,
+ *edge_angle);
} else {
*edge_angle = claw_values.calibration.lower_angle;
+ LOG(INFO, "%s Posedge calibration lower edge -> %f\n", name_,
+ *edge_angle);
}
*edge_encoder = posedge_value_;
return true;
}
if (calibration_hall_effect_negedge_count_changed()) {
- if (encoder() - last_encoder() > 0) {
+ if (negedge_value_ - last_encoder() > 0) {
*edge_angle = claw_values.calibration.upper_angle;
+ LOG(INFO, "%s Negedge calibration upper edge -> %f\n", name_, *edge_angle);
} else {
*edge_angle = claw_values.calibration.lower_angle;
+ LOG(INFO, "%s Negedge calibration lower edge -> %f\n", name_, *edge_angle);
}
*edge_encoder = negedge_value_;
return true;
}
if (back_hall_effect_posedge_count_changed()) {
- if (encoder() - last_encoder() < 0) {
+ if (posedge_value_ - last_encoder() < 0) {
*edge_angle = claw_values.back.upper_angle;
+ LOG(INFO, "%s Posedge back upper edge -> %f\n", name_, *edge_angle);
} else {
*edge_angle = claw_values.back.lower_angle;
+ LOG(INFO, "%s Posedge back lower edge -> %f\n", name_, *edge_angle);
}
*edge_encoder = posedge_value_;
return true;
}
if (back_hall_effect_negedge_count_changed()) {
- if (encoder() - last_encoder() > 0) {
+ if (negedge_value_ - last_encoder() > 0) {
*edge_angle = claw_values.back.upper_angle;
+ LOG(INFO, "%s Negedge back upper edge -> %f\n", name_, *edge_angle);
} else {
*edge_angle = claw_values.back.lower_angle;
+ LOG(INFO, "%s Negedge back lower edge -> %f\n", name_, *edge_angle);
}
*edge_encoder = negedge_value_;
return true;
@@ -152,6 +155,48 @@
return false;
}
+void TopZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
+ double edge_angle) {
+ double old_offset = offset_;
+ offset_ = edge_angle - edge_encoder;
+ const double doffset = offset_ - old_offset;
+ motor_->ChangeTopOffset(doffset);
+}
+
+void BottomZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
+ double edge_angle) {
+ double old_offset = offset_;
+ offset_ = edge_angle - edge_encoder;
+ const double doffset = offset_ - old_offset;
+ motor_->ChangeBottomOffset(doffset);
+}
+
+void ClawMotor::ChangeTopOffset(double doffset) {
+ claw_.ChangeTopOffset(doffset);
+ if (has_top_claw_goal_) {
+ top_claw_goal_ += doffset;
+ }
+}
+
+void ClawMotor::ChangeBottomOffset(double doffset) {
+ claw_.ChangeBottomOffset(doffset);
+ if (has_bottom_claw_goal_) {
+ bottom_claw_goal_ += doffset;
+ }
+}
+
+void ClawLimitedLoop::ChangeTopOffset(double doffset) {
+ Y_(1, 0) += doffset;
+ X_hat(1, 0) += doffset;
+ LOG(INFO, "Changing top offset by %f\n", doffset);
+}
+void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
+ Y_(0, 0) += doffset;
+ X_hat(0, 0) += doffset;
+ X_hat(1, 0) -= doffset;
+ LOG(INFO, "Changing bottom offset by %f\n", doffset);
+}
+
// Positive angle is up, and positive power is up.
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
@@ -168,11 +213,12 @@
}
// TODO(austin): Handle the disabled state and the disabled -> enabled
- // transition in all of these states.
- // TODO(austin): Handle zeroing while disabled.
+ // transition in all of these states.
+ // TODO(austin): Handle zeroing while disabled correctly (only use a single
+ // edge and direction when zeroing.)
// TODO(austin): Save all the counters so we know when something actually
- // happens.
+ // happens.
// TODO(austin): Helpers to find the position of the claw on an edge.
// TODO(austin): This may not be necesary because of the ControlLoop class.
@@ -184,22 +230,41 @@
const frc971::constants::Values &values = constants::GetValues();
if (position) {
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << position->bottom.position + bottom_claw_.offset(),
+ position->top.position + top_claw_.offset();
+ claw_.Correct(Y);
+
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;
+ top_claw_goal_ = top_claw_.absolute_position();
+ initial_seperation_ =
+ top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
if (!has_bottom_claw_goal_) {
has_bottom_claw_goal_ = true;
- bottom_claw_goal_ = position->bottom.position;
+ bottom_claw_goal_ = bottom_claw_.absolute_position();
+ initial_seperation_ =
+ top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
+ LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
+ top_claw_.absolute_position(), bottom_claw_.absolute_position());
}
bool autonomous = ::aos::robot_state->autonomous;
bool enabled = ::aos::robot_state->enabled;
+ enum CalibrationMode {
+ READY,
+ FINE_TUNE,
+ UNKNOWN_LOCATION
+ };
+
+ CalibrationMode mode;
+
if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
(autonomous &&
@@ -213,6 +278,11 @@
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
top_claw_goal_ = goal->bottom_angle + goal->seperation_angle;
+ has_bottom_claw_goal_ = true;
+ has_top_claw_goal_ = true;
+ doing_calibration_fine_tune_ = false;
+
+ mode = READY;
} else if (top_claw_.zeroing_state() !=
ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
@@ -220,79 +290,95 @@
// Time to fine tune the zero.
// Limit the goals here.
if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
- // always get the bottom claw to calibrated first
- if (!doing_calibration_fine_tune_) {
- if (position->bottom.position > values.start_fine_tune_pos -
- values.claw_unimportant_epsilon &&
- position->bottom.position < values.start_fine_tune_pos +
- values.claw_unimportant_epsilon) {
- doing_calibration_fine_tune_ = true;
- bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
- } else {
- // send bottom to zeroing start
- bottom_claw_goal_ = values.start_fine_tune_pos;
- }
- } else {
- if (position->bottom.front_hall_effect ||
- position->bottom.back_hall_effect ||
- position->top.front_hall_effect ||
- position->top.back_hall_effect) {
- // this should not happen, but now we know it won't
- doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.start_fine_tune_pos;
- }
- if (position->bottom.calibration_hall_effect) {
- // do calibration
- bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
- values.lower_claw.calibration.lower_angle);
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calinrated so we are done fine tuning bottom
- doing_calibration_fine_tune_ = false;
- }
- }
- // now set the top claw to track
- top_claw_goal_ = bottom_claw_goal_ + goal->seperation_angle;
+ // always get the bottom claw to calibrated first
+ LOG(DEBUG, "Calibrating the bottom of the claw\n");
+ if (!doing_calibration_fine_tune_) {
+ if (::std::abs(bottom_absolute_position() -
+ values.start_fine_tune_pos) <
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ bottom_claw_goal_ += values.claw_zeroing_speed * dt;
+ LOG(DEBUG, "Ready to fine tune the bottom\n");
+ } else {
+ // send bottom to zeroing start
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ LOG(DEBUG, "Going to the start position for the bottom\n");
+ }
+ } else {
+ bottom_claw_goal_ += values.claw_zeroing_speed * dt;
+ if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
+ bottom_claw_.front_hall_effect() ||
+ bottom_claw_.back_hall_effect()) {
+ // We shouldn't hit a limit, but if we do, go back to the zeroing
+ // point and try again.
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ LOG(DEBUG, "Found a limit, starting over.\n");
+ }
+ // TODO(austin): We have a count for this... Need to double check that
+ // it ticked, just in case.
+ if (bottom_claw_.calibration_hall_effect()) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else {
+ LOG(DEBUG, "Fine tuning\n");
+ }
+ }
+ // now set the top claw to track
+
+ // TODO(austin): Some safe distance!
+ top_claw_goal_ = bottom_claw_goal_ + values.claw_zeroing_separation;
} else {
- // bottom claw must be calibrated, start on the top
- if (!doing_calibration_fine_tune_) {
- if (position->top.position > values.start_fine_tune_pos -
- values.claw_unimportant_epsilon &&
- position->top.position < values.start_fine_tune_pos +
- values.claw_unimportant_epsilon) {
- doing_calibration_fine_tune_ = true;
- top_claw_goal_ += values.claw_zeroing_off_speed * dt;
- } else {
- // send top to zeroing start
- top_claw_goal_ = values.start_fine_tune_pos;
- }
- } else {
- if (position->top.front_hall_effect ||
- position->top.back_hall_effect ||
- position->top.front_hall_effect ||
- position->top.back_hall_effect) {
- // this should not happen, but now we know it won't
- doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.start_fine_tune_pos;
- }
- if (position->top.calibration_hall_effect) {
- // do calibration
- top_claw_.SetCalibration(
- position->top.posedge_value,
- values.upper_claw.calibration.lower_angle);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calinrated so we are done fine tuning top
- doing_calibration_fine_tune_ = false;
- }
- }
- // now set the bottom claw to track
- bottom_claw_goal_ = top_claw_goal_ - goal->seperation_angle;
+ // bottom claw must be calibrated, start on the top
+ if (!doing_calibration_fine_tune_) {
+ if (::std::abs(top_absolute_position() - values.start_fine_tune_pos) <
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ top_claw_goal_ += values.claw_zeroing_speed * dt;
+ LOG(DEBUG, "Ready to fine tune the top\n");
+ } else {
+ // send top to zeroing start
+ top_claw_goal_ = values.start_fine_tune_pos;
+ LOG(DEBUG, "Going to the start position for the top\n");
+ }
+ } else {
+ top_claw_goal_ += values.claw_zeroing_speed * dt;
+ if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
+ bottom_claw_.front_hall_effect() ||
+ bottom_claw_.back_hall_effect()) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.start_fine_tune_pos;
+ LOG(DEBUG, "Found a limit, starting over.\n");
+ }
+ if (top_claw_.calibration_hall_effect()) {
+ // do calibration
+ top_claw_.SetCalibration(position->top.posedge_value,
+ values.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calinrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ }
+ }
+ // now set the bottom claw to track
+ bottom_claw_goal_ = top_claw_goal_ - values.claw_zeroing_separation;
}
+ mode = FINE_TUNE;
} else {
+ doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
top_claw_goal_ = position->top.position;
bottom_claw_goal_ = position->bottom.position;
+ initial_seperation_ =
+ position->top.position - position->bottom.position;
} else {
has_top_claw_goal_ = false;
has_bottom_claw_goal_ = false;
@@ -317,14 +403,16 @@
top_claw_goal_ += values.claw_zeroing_off_speed * dt;
bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
// TODO(austin): Goal velocity too!
+ LOG(DEBUG, "Bottom is known.\n");
}
} else {
// We don't know where either claw is. Slowly start moving down to find
// any hall effect.
if (enabled) {
- top_claw_goal_-= values.claw_zeroing_off_speed * dt;
+ top_claw_goal_ -= values.claw_zeroing_off_speed * dt;
bottom_claw_goal_ -= values.claw_zeroing_off_speed * dt;
// TODO(austin): Goal velocity too!
+ LOG(DEBUG, "Both are unknown.\n");
}
}
@@ -339,22 +427,52 @@
bottom_claw_.SetCalibrationOnEdge(
values.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
}
+ mode = UNKNOWN_LOCATION;
}
// TODO(austin): Handle disabled.
// TODO(austin): ...
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;
+ claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, 0, 0;
+ double separation = -971;
+ if (position != nullptr) {
+ separation = position->top.position - position->bottom.position;
+ }
+ LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
+ claw_.R(1, 0), separation);
- top_claw_.Update(output == nullptr);
- bottom_claw_.Update(output == nullptr);
+ claw_.Update(output == nullptr);
} else {
- top_claw_.ZeroPower();
- bottom_claw_.ZeroPower();
+ claw_.Update(true);
}
+ (void)mode;
+
+ /*
+ switch (mode) {
+ case READY:
+ break;
+ case FINE_TUNE:
+ break;
+ case UNKNOWN_LOCATION:
+ if (top_claw_->uncapped_voltage() > values.max_zeroing_voltage) {
+ double dx =
+ (top_claw_->uncapped_voltage() - values.max_zeroing_voltage) /
+ top_claw_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ } else if (top_claw_->uncapped_voltage() < -values.max_zeroing_voltage) {
+ double dx =
+ (top_claw_->uncapped_voltage() + values.max_zeroing_voltage) /
+ top_claw_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ }
+ break;
+ }
+ */
+
if (position) {
//LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
//position->top_calibration_hall_effect ? "true" : "false",
@@ -362,8 +480,8 @@
}
if (output) {
- output->top_claw_voltage = top_claw_.voltage();
- output->bottom_claw_voltage = bottom_claw_.voltage();
+ output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
}
status->done = false;
//::std::abs(zeroed_joint_.absolute_position() - goal->bottom_angle -