Claw now zeros!
- Debugged plant
- Switched to a simple controller architecture.
- Fixed controller.
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.cc b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
deleted file mode 100644
index a0eb131..0000000
--- a/frc971/control_loops/claw/bottom_claw_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeBottomClawController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.81581823335, 78.6334534274, 142.868137351;
- Eigen::Matrix<double, 1, 3> K;
- K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeBottomClawPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeBottomClawPlantCoefficients());
- return StateFeedbackPlant<3, 1, 1>(plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeBottomClawController());
- return StateFeedbackLoop<3, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.h b/frc971/control_loops/claw/bottom_claw_motor_plant.h
deleted file mode 100644
index fc905ca..0000000
--- a/frc971/control_loops/claw/bottom_claw_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeBottomClawController();
-
-StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
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 -
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index c541dad..5741896 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -22,10 +22,7 @@
'type': 'static_library',
'sources': [
'claw.cc',
- 'top_claw_motor_plant.cc',
- 'bottom_claw_motor_plant.cc',
- 'unaugmented_top_claw_motor_plant.cc',
- 'unaugmented_bottom_claw_motor_plant.cc',
+ 'claw_motor_plant.cc',
],
'dependencies': [
'claw_loops',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index c045d6f..8157b67 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -7,8 +7,7 @@
#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"
-#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+#include "frc971/control_loops/claw/claw_motor_plant.h"
namespace frc971 {
namespace control_loops {
@@ -22,18 +21,28 @@
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
// that isn't true.
+class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
+ : StateFeedbackLoop<4, 2, 2>(loop) {}
+ virtual void CapU();
+
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+};
+
+class ClawMotor;
+
// This class implements the CapU function correctly given all the extra
// information that we know about from the wrist motor.
// It does not have any zeroing logic in it, only logic to deal with a delta U
// controller.
-class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+class ZeroedStateFeedbackLoop {
public:
- ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
- : StateFeedbackLoop<3, 1, 1>(loop),
- voltage_(0.0),
- last_voltage_(0.0),
- uncapped_voltage_(0.0),
- offset_(0.0),
+ ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor)
+ : offset_(0.0),
+ name_(name),
+ motor_(motor),
front_hall_effect_posedge_count_(0.0),
previous_front_hall_effect_posedge_count_(0.0),
front_hall_effect_negedge_count_(0.0),
@@ -46,26 +55,17 @@
previous_back_hall_effect_posedge_count_(0.0),
back_hall_effect_negedge_count_(0.0),
previous_back_hall_effect_negedge_count_(0.0),
+ front_hall_effect_(false),
+ calibration_hall_effect_(false),
+ back_hall_effect_(false),
zeroing_state_(UNKNOWN_POSITION),
posedge_value_(0.0),
negedge_value_(0.0),
encoder_(0.0),
- last_encoder_(0.0){}
+ last_encoder_(0.0) {}
const static int kZeroingMaxVoltage = 5;
- // Caps U, but this time respects the state of the wrist as well.
- virtual void CapU();
-
- // Returns the accumulated voltage.
- double voltage() const { return voltage_; }
-
- // Returns the uncapped voltage.
- double uncapped_voltage() const { return uncapped_voltage_; }
-
- // Zeros the accumulator.
- void ZeroPower() { voltage_ = 0.0; }
-
enum JointZeroingState {
// We don't know where the joint is at all.
UNKNOWN_POSITION,
@@ -83,25 +83,6 @@
}
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 HalfClawPosition &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);
@@ -114,11 +95,20 @@
posedge_value_ = claw.posedge_value;
negedge_value_ = claw.negedge_value;
- Eigen::Matrix<double, 1, 1> Y;
- Y << claw.position;
- Correct(Y);
+ last_encoder_ = encoder_;
+ encoder_ = claw.position;
+
+ front_hall_effect_ = claw.front_hall_effect;
+ calibration_hall_effect_ = claw.calibration_hall_effect;
+ back_hall_effect_ = claw.back_hall_effect;
}
+ double absolute_position() const { return encoder() + offset(); }
+
+ bool front_hall_effect() const { return front_hall_effect_; }
+ bool calibration_hall_effect() const { return calibration_hall_effect_; }
+ bool back_hall_effect() const { return back_hall_effect_; }
+
#define COUNT_SETTER_GETTER(variable) \
void set_##variable(int32_t value) { \
previous_##variable##_ = variable##_; \
@@ -146,10 +136,11 @@
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_; }
+ double offset() const { return offset_; }
+
// 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,
@@ -157,14 +148,12 @@
#undef COUNT_SETTER_GETTER
- private:
+ protected:
// The accumulated voltage to apply to the motor.
- double voltage_;
- double last_voltage_;
- double uncapped_voltage_;
double offset_;
+ const char *name_;
- double previous_position_;
+ ClawMotor *motor_;
int32_t front_hall_effect_posedge_count_;
int32_t previous_front_hall_effect_posedge_count_;
@@ -178,6 +167,9 @@
int32_t previous_back_hall_effect_posedge_count_;
int32_t back_hall_effect_negedge_count_;
int32_t previous_back_hall_effect_negedge_count_;
+ bool front_hall_effect_;
+ bool calibration_hall_effect_;
+ bool back_hall_effect_;
JointZeroingState zeroing_state_;
double posedge_value_;
@@ -186,6 +178,49 @@
double last_encoder_;
};
+class TopZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ TopZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("top", motor) {}
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ 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;
+ }
+};
+
+class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ BottomZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("bottom", motor) {}
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ 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;
+ }
+};
class ClawMotor
: public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
@@ -196,14 +231,19 @@
// True if the state machine is ready.
bool is_ready() const { return false; }
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+
protected:
virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
::aos::control_loops::Status *status);
- double top_absolute_position() const { return top_claw_.position(); }
- double bottom_absolute_position() const { return bottom_claw_.position(); }
+ double top_absolute_position() const {
+ return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
+ }
+ double bottom_absolute_position() const { return claw_.X_hat(0, 0); }
private:
// Friend the test classes for acces to the internal state.
@@ -213,15 +253,22 @@
// The zeroed joint to use.
bool has_top_claw_goal_;
double top_claw_goal_;
- ZeroedStateFeedbackLoop top_claw_;
+ TopZeroedStateFeedbackLoop top_claw_;
bool has_bottom_claw_goal_;
double bottom_claw_goal_;
- ZeroedStateFeedbackLoop bottom_claw_;
+ BottomZeroedStateFeedbackLoop bottom_claw_;
+
+ // The claw loop.
+ ClawLimitedLoop claw_;
bool was_enabled_;
bool doing_calibration_fine_tune_;
+ // The initial seperation when disabled. Used as the safe seperation
+ // distance.
+ double initial_seperation_;
+
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 30155f5..2308748 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -2,14 +2,14 @@
#include <memory>
-#include "gtest/gtest.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
-#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+#include "gtest/gtest.h"
using ::aos::time::Time;
@@ -33,9 +33,7 @@
// wrist, which will be treated as 0 by the encoder.
ClawMotorSimulation(double initial_top_position,
double initial_bottom_position)
- : top_claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant())),
- bottom_claw_plant_(
- new StateFeedbackPlant<2, 1, 1>(MakeRawBottomClawPlant())),
+ : claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())),
claw_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
".frc971.control_loops.claw_queue_group.goal",
".frc971.control_loops.claw_queue_group.position",
@@ -48,6 +46,12 @@
double initial_bottom_position) {
LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
initial_bottom_position);
+ claw_plant_->X(0, 0) = initial_bottom_position;
+ claw_plant_->X(1, 0) = initial_top_position - initial_bottom_position;
+ claw_plant_->X(2, 0) = 0.0;
+ claw_plant_->X(3, 0) = 0.0;
+ claw_plant_->Y = claw_plant_->C() * claw_plant_->X;
+
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
last_position_.Zero();
@@ -57,9 +61,9 @@
// Returns the absolute angle of the wrist.
double GetAbsolutePosition(ClawType type) const {
if (type == TOP_CLAW) {
- return top_claw_plant_->Y(0, 0);
+ return claw_plant_->Y(1, 0);
} else {
- return bottom_claw_plant_->Y(0, 0);
+ return claw_plant_->Y(0, 0);
}
}
@@ -113,16 +117,26 @@
SetPhysicalSensors(position.get());
const frc971::constants::Values& values = constants::GetValues();
+ double last_top_position =
+ last_position_.top.position + initial_position_[TOP_CLAW];
+ double last_bottom_position =
+ last_position_.bottom.position + initial_position_[BOTTOM_CLAW];
+ double top_position =
+ position->top.position + initial_position_[TOP_CLAW];
+ double bottom_position =
+ position->bottom.position + initial_position_[BOTTOM_CLAW];
// Handle the front hall effect.
if (position->top.front_hall_effect &&
!last_position_.top.front_hall_effect) {
++position->top.front_hall_effect_posedge_count;
- if (last_position_.top.position < values.upper_claw.front.lower_angle) {
+ if (last_top_position < values.upper_claw.front.lower_angle) {
+ LOG(DEBUG, "Top: Positive lower edge front hall effect\n");
position->top.posedge_value =
values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Positive upper edge front hall effect\n");
position->top.posedge_value =
values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
}
@@ -131,10 +145,12 @@
last_position_.top.front_hall_effect) {
++position->top.front_hall_effect_negedge_count;
- if (position->top.position < values.upper_claw.front.lower_angle) {
+ if (top_position < values.upper_claw.front.lower_angle) {
+ LOG(DEBUG, "Top: Negative lower edge front hall effect\n");
position->top.negedge_value =
values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Negative upper edge front hall effect\n");
position->top.negedge_value =
values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
}
@@ -145,10 +161,13 @@
!last_position_.top.calibration_hall_effect) {
++position->top.calibration_hall_effect_posedge_count;
- if (last_position_.top.position < values.upper_claw.calibration.lower_angle) {
+ if (last_top_position < values.upper_claw.calibration.lower_angle) {
+ LOG(DEBUG, "Top: Positive lower edge calibration hall effect\n");
position->top.posedge_value =
- values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ values.upper_claw.calibration.lower_angle -
+ initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Positive upper edge calibration hall effect\n");
position->top.posedge_value =
values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
}
@@ -157,10 +176,12 @@
last_position_.top.calibration_hall_effect) {
++position->top.calibration_hall_effect_negedge_count;
- if (position->top.position < values.upper_claw.calibration.lower_angle) {
+ if (top_position < values.upper_claw.calibration.lower_angle) {
+ LOG(DEBUG, "Top: Negative lower edge calibration hall effect\n");
position->top.negedge_value =
values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Negative upper edge calibration hall effect\n");
position->top.negedge_value =
values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
}
@@ -171,10 +192,12 @@
!last_position_.top.back_hall_effect) {
++position->top.back_hall_effect_posedge_count;
- if (last_position_.top.position < values.upper_claw.back.lower_angle) {
+ if (last_top_position < values.upper_claw.back.lower_angle) {
+ LOG(DEBUG, "Top: Positive lower edge back hall effect\n");
position->top.posedge_value =
values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Positive upper edge back hall effect\n");
position->top.posedge_value =
values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
}
@@ -183,10 +206,12 @@
last_position_.top.back_hall_effect) {
++position->top.back_hall_effect_negedge_count;
- if (position->top.position < values.upper_claw.back.lower_angle) {
+ if (top_position < values.upper_claw.back.lower_angle) {
+ LOG(DEBUG, "Top: Negative upper edge back hall effect\n");
position->top.negedge_value =
values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
} else {
+ LOG(DEBUG, "Top: Negative lower edge back hall effect\n");
position->top.negedge_value =
values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
}
@@ -198,24 +223,28 @@
!last_position_.bottom.front_hall_effect) {
++position->bottom.front_hall_effect_posedge_count;
- if (last_position_.bottom.position < values.lower_claw.front.lower_angle) {
+ if (last_bottom_position < values.lower_claw.front.lower_angle) {
+ LOG(DEBUG, "Bottom: Positive lower edge front hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.front.lower_angle - initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Positive upper edge front hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.front.upper_angle - initial_position_[BOTTOM_CLAW];
}
}
if (!position->bottom.front_hall_effect &&
last_position_.bottom.front_hall_effect) {
++position->bottom.front_hall_effect_negedge_count;
- if (position->bottom.position < values.lower_claw.front.lower_angle) {
+ if (bottom_position < values.lower_claw.front.lower_angle) {
+ LOG(DEBUG, "Bottom: Negative lower edge front hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.front.lower_angle - initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Negative upper edge front hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.front.upper_angle - initial_position_[BOTTOM_CLAW];
}
}
@@ -224,24 +253,32 @@
!last_position_.bottom.calibration_hall_effect) {
++position->bottom.calibration_hall_effect_posedge_count;
- if (last_position_.bottom.position < values.lower_claw.calibration.lower_angle) {
+ if (last_bottom_position < values.lower_claw.calibration.lower_angle) {
+ LOG(DEBUG, "Bottom: Positive lower edge calibration hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.calibration.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Positive upper edge calibration hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.calibration.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
if (!position->bottom.calibration_hall_effect &&
last_position_.bottom.calibration_hall_effect) {
++position->bottom.calibration_hall_effect_negedge_count;
- if (position->bottom.position < values.lower_claw.calibration.lower_angle) {
+ if (bottom_position < values.lower_claw.calibration.lower_angle) {
+ LOG(DEBUG, "Bottom: Negative lower edge calibration hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.calibration.lower_angle -
+ initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Negative upper edge calibration hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.calibration.upper_angle -
+ initial_position_[BOTTOM_CLAW];
}
}
@@ -250,24 +287,28 @@
!last_position_.bottom.back_hall_effect) {
++position->bottom.back_hall_effect_posedge_count;
- if (last_position_.bottom.position < values.lower_claw.back.lower_angle) {
+ if (last_bottom_position < values.lower_claw.back.lower_angle) {
+ LOG(DEBUG, "Bottom: Positive lower edge back hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.back.lower_angle - initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Positive upper edge back hall effect\n");
position->bottom.posedge_value =
- values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.back.upper_angle - initial_position_[BOTTOM_CLAW];
}
}
if (!position->bottom.back_hall_effect &&
last_position_.bottom.back_hall_effect) {
++position->bottom.back_hall_effect_negedge_count;
- if (position->bottom.position < values.lower_claw.back.lower_angle) {
+ if (bottom_position < values.lower_claw.back.lower_angle) {
+ LOG(DEBUG, "Bottom: Negative lower edge back hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.back.lower_angle - initial_position_[BOTTOM_CLAW];
} else {
+ LOG(DEBUG, "Bottom: Negative upper edge back hall effect\n");
position->bottom.negedge_value =
- values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ values.lower_claw.back.upper_angle - initial_position_[BOTTOM_CLAW];
}
}
@@ -281,48 +322,35 @@
void Simulate() {
const frc971::constants::Values& v = constants::GetValues();
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
- Simulate(TOP_CLAW, top_claw_plant_.get(), v.upper_claw,
- claw_queue_group.output->top_claw_voltage);
- Simulate(BOTTOM_CLAW, bottom_claw_plant_.get(), v.lower_claw,
- claw_queue_group.output->bottom_claw_voltage);
+
+ claw_plant_->U << claw_queue_group.output->bottom_claw_voltage,
+ claw_queue_group.output->top_claw_voltage -
+ claw_queue_group.output->bottom_claw_voltage;
+ claw_plant_->Update();
+
+ // Check that the claw is within the limits.
+ EXPECT_GE(v.upper_claw.upper_limit, claw_plant_->Y(0, 0));
+ EXPECT_LE(v.upper_claw.lower_limit, claw_plant_->Y(0, 0));
+
+ EXPECT_GE(v.lower_claw.upper_limit, claw_plant_->Y(1, 0));
+ EXPECT_LE(v.lower_claw.lower_limit, claw_plant_->Y(1, 0));
+
+ EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ v.claw_max_seperation);
+ EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ v.claw_min_seperation);
}
- // Top of the claw, the one with rollers
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> top_claw_plant_;
- // Bottom of the claw, the one with tusks
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> bottom_claw_plant_;
+ // The whole claw.
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> 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];
- plant->Update();
-
- // check top claw inside limits
- EXPECT_GE(claw.upper_limit, plant->Y(0, 0));
- EXPECT_LE(claw.lower_limit, plant->Y(0, 0));
-
- // TODO(austin): Check that the claws aren't too close to eachother.
- last_voltage_[type] = nl_voltage;
}
ClawGroup claw_queue_group;
double initial_position_[CLAW_COUNT];
- double last_voltage_[CLAW_COUNT];
control_loops::ClawGroup::Position last_position_;
};
@@ -361,9 +389,11 @@
}
void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
+ ::aos::robot_state.MakeWithBuilder()
+ .enabled(enabled)
+ .autonomous(false)
+ .team_id(971)
+ .Send();
::aos::robot_state.FetchLatest();
}
@@ -416,14 +446,13 @@
VerifyNearGoal();
}
-/*
// Tests that missing positions are correctly handled.
TEST_F(ClawTest, HandleMissingPosition) {
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.1)
.seperation_angle(0.2)
.Send();
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0; i < 500; ++i) {
if (i % 23) {
claw_motor_plant_.SendPositionMessage();
}
@@ -435,6 +464,7 @@
VerifyNearGoal();
}
+/*
// Tests that loosing the encoder for a second triggers a re-zero.
TEST_F(ClawTest, RezeroWithMissingPos) {
claw_queue_group.goal.MakeWithBuilder()
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
new file mode 100644
index 0000000..5d6598e
--- /dev/null
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.0, 0.00904786878843, 0.0, 0.0, 1.0, 0.0, 0.00904786878843, 0.0, 0.0, 0.815818233346, 0.0, 0.0, 0.0, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000326582411818, 0.0, 0.0, 0.000326582411818, 0.0631746179893, 0.0, 0.0, 0.0631746179893;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 1, 1, 0, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 24.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -24.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeClawController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.71581823335, 5.38760974287e-16, -1.71581823335, 1.71581823335, 64.8264890043, 2.03572300346e-14, -64.8264890043, 64.8264890043;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 146.100132128, 0.0, 6.7282816813, 0.0, 0.0, 275.346049928, 0.0, 12.0408756114;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
+}
+
+StateFeedbackPlant<4, 2, 2> MakeClawPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClawPlantCoefficients());
+ return StateFeedbackPlant<4, 2, 2>(plants);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeClawLoop() {
+ ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClawController());
+ return StateFeedbackLoop<4, 2, 2>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
new file mode 100644
index 0000000..988cc20
--- /dev/null
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeClawController();
+
+StateFeedbackPlant<4, 2, 2> MakeClawPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.cc b/frc971/control_loops/claw/top_claw_motor_plant.cc
deleted file mode 100644
index 113ff77..0000000
--- a/frc971/control_loops/claw/top_claw_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/claw/top_claw_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeTopClawController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.81581823335, 78.6334534274, 142.868137351;
- Eigen::Matrix<double, 1, 3> K;
- K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeTopClawPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeTopClawPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeTopClawPlantCoefficients());
- return StateFeedbackPlant<3, 1, 1>(plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeTopClawLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeTopClawController());
- return StateFeedbackLoop<3, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.h b/frc971/control_loops/claw/top_claw_motor_plant.h
deleted file mode 100644
index c74c976..0000000
--- a/frc971/control_loops/claw/top_claw_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeTopClawController();
-
-StateFeedbackPlant<3, 1, 1> MakeTopClawPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeTopClawLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
deleted file mode 100644
index cda15c4..0000000
--- a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000326582411818, 0.0631746179893;
- 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeRawBottomClawController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71581823335, 64.8264890043;
- Eigen::Matrix<double, 1, 2> K;
- K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawBottomClawPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawBottomClawPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawBottomClawController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
deleted file mode 100644
index 8f59925..0000000
--- a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawBottomClawController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
deleted file mode 100644
index 8ab4bbf..0000000
--- a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000326582411818, 0.0631746179893;
- 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeRawTopClawController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71581823335, 64.8264890043;
- Eigen::Matrix<double, 1, 2> K;
- K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawTopClawPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawTopClawPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawTopClawController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
deleted file mode 100644
index c87d3ca..0000000
--- a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawTopClawController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b1f378e..98d79ea 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -51,7 +51,7 @@
[[0, 0],
[0, 0],
[self.motor_feedforward, 0],
- [-self.motor_feedforward, self.motor_feedforward]])
+ [0.0, self.motor_feedforward]])
self.C = numpy.matrix([[1, 0, 0, 0],
[1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
@@ -63,7 +63,17 @@
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
- self.PlaceControllerPoles([0.85, 0.45, 0.45, 0.85])
+ self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.03 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.2, 0.0],
+ [0.0, 0.0, 0.0, 2.00]])
+
+ self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (20.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ print "K unaugmented"
+ print self.K
self.rpl = .05
self.ipl = 0.008
@@ -72,8 +82,8 @@
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.U_max = numpy.matrix([[12.0], [24.0]])
+ self.U_min = numpy.matrix([[-12.0], [-24.0]])
self.InitializeState()
@@ -152,6 +162,52 @@
self.InitializeState()
+def FullSeparationPriority(claw, U):
+ bottom_u = U[0, 0]
+ top_u = U[1, 0] + bottom_u
+
+ #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+ if bottom_u > claw.U_max[0, 0]:
+ #print "Bottom is too big. Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
+ top_u -= bottom_u - claw.U_max[0, 0]
+ if top_u < claw.U_min[1, 0]:
+ top_u = claw.U_min[1, 0]
+
+ bottom_u = claw.U_max[0, 0]
+ if top_u > claw.U_max[1, 0]:
+ bottom_u -= top_u - claw.U_max[1, 0]
+ if bottom_u < claw.U_min[0, 0]:
+ bottom_u = claw.U_min[0, 0]
+
+ top_u = claw.U_max[1, 0]
+ if top_u < claw.U_min[1, 0]:
+ bottom_u -= top_u - claw.U_min[1, 0]
+ if bottom_u > claw.U_max[0, 0]:
+ bottom_u = claw.U_max[0, 0]
+
+ top_u = claw.U_min[1, 0]
+ if bottom_u < claw.U_min[0, 0]:
+ top_u -= bottom_u - claw.U_min[0, 0]
+ if top_u > claw.U_max[1, 0]:
+ top_u = claw.U_max[1, 0]
+
+ bottom_u = claw.U_min[0, 0]
+
+ return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+
+def AverageUFix(claw, U):
+ bottom_u = U[0, 0]
+ top_u = U[1, 0] + bottom_u
+
+ #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+ if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[1, 0] or
+ top_u < claw.U_min[1, 0] or bottom_u < claw.U_min[0, 0]):
+ scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ top_u *= scalar
+ bottom_u *= scalar
+
+ return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
top_u = U[1, 0]
@@ -192,65 +248,47 @@
#pylab.show()
# Simulate the closed loop response of the system to a step input.
- top_claw = ClawDeltaU("TopClaw")
+ claw = Claw("TopClaw")
+ t = []
close_loop_x_bottom = []
close_loop_x_sep = []
close_loop_u_bottom = []
close_loop_u_top = []
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0], [0.0]])
- top_claw.X[0, 0] = 0
- for _ in xrange(50):
- #print "Error is", (R - top_claw.X_hat)
- U = top_claw.K * (R - top_claw.X_hat)
- U = ClipDeltaU(top_claw, U)
- top_claw.UpdateObserver(U)
- top_claw.Update(U)
- close_loop_x_bottom.append(top_claw.X[0, 0] * 10)
- close_loop_u_bottom.append(top_claw.X[4, 0])
- close_loop_x_sep.append(top_claw.X[1, 0] * 10)
- close_loop_u_top.append(U[1, 0])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ claw.X[0, 0] = 0
+ for i in xrange(100):
+ #print "Error is", (R - claw.X_hat)
+ U = claw.K * (R - claw.X_hat)
+ #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
+ #U = FullSeparationPriority(claw, U)
+ U = AverageUFix(claw, U)
+ #U = claw.K * (R - claw.X_hat)
+ #U = ClipDeltaU(claw, U)
+ claw.UpdateObserver(U)
+ claw.Update(U)
+ close_loop_x_bottom.append(claw.X[0, 0] * 10)
+ close_loop_u_bottom.append(U[0, 0])
+ close_loop_x_sep.append(claw.X[1, 0] * 10)
+ close_loop_u_top.append(U[1, 0] + U[0, 0])
+ t.append(0.01 * i)
- pylab.plot(range(50), close_loop_x_bottom, label='x bottom')
- pylab.plot(range(50), close_loop_u_bottom, label='u bottom')
- pylab.plot(range(50), close_loop_x_sep, label='seperation')
- pylab.plot(range(50), close_loop_u_top, label='u top')
+ pylab.plot(t, close_loop_x_bottom, label='x bottom')
+ pylab.plot(t, close_loop_x_sep, label='seperation')
+ pylab.plot(t, close_loop_u_bottom, label='u bottom')
+ pylab.plot(t, close_loop_u_top, label='u top')
pylab.legend()
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 9:
- print "Expected .h file name and .cc file name for"
- print "both the plant and unaugmented plant"
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the claw."
else:
- top_unaug_claw = Claw("RawTopClaw")
- top_unaug_loop_writer = control_loop.ControlLoopWriter("RawTopClaw",
- [top_unaug_claw])
+ claw = Claw("Claw")
+ loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
if argv[1][-3:] == '.cc':
- top_unaug_loop_writer.Write(argv[2], argv[1])
+ loop_writer.Write(argv[2], argv[1])
else:
- top_unaug_loop_writer.Write(argv[1], argv[2])
-
- top_loop_writer = control_loop.ControlLoopWriter("TopClaw", [top_claw])
- if argv[3][-3:] == '.cc':
- top_loop_writer.Write(argv[4], argv[3])
- else:
- top_loop_writer.Write(argv[3], argv[4])
-
- bottom_claw = ClawDeltaU("BottomClaw")
- bottom_unaug_claw = Claw("RawBottomClaw")
- bottom_unaug_loop_writer = control_loop.ControlLoopWriter(
- "RawBottomClaw", [bottom_unaug_claw])
- if argv[5][-3:] == '.cc':
- bottom_unaug_loop_writer.Write(argv[6], argv[5])
- else:
- bottom_unaug_loop_writer.Write(argv[5], argv[6])
-
- bottom_loop_writer = control_loop.ControlLoopWriter("BottomClaw",
- [bottom_claw])
- if argv[7][-3:] == '.cc':
- bottom_loop_writer.Write(argv[8], argv[7])
- else:
- bottom_loop_writer.Write(argv[7], argv[8])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 420a0e7..0384747 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -4,9 +4,12 @@
#include <assert.h>
#include <vector>
+#include <iostream>
#include "Eigen/Dense"
+#include "aos/common/logging/logging.h"
+
template <int number_of_states, int number_of_inputs, int number_of_outputs>
class StateFeedbackPlantCoefficients {
public:
@@ -129,8 +132,8 @@
// Assert that U is within the hardware range.
virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0));
- assert(U(i, 0) >= U_min(i, 0));
+ assert(U(i, 0) <= U_max(i, 0) + 0.00001);
+ assert(U(i, 0) >= U_min(i, 0) - 0.00001);
}
}
@@ -286,6 +289,23 @@
// Corrects X_hat given the observation in Y.
void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ /*
+ auto eye =
+ Eigen::Matrix<double, number_of_states, number_of_states>::Identity();
+ //LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ ::std::cout << "Identity " << eye << ::std::endl;
+ ::std::cout << "X_hat " << X_hat << ::std::endl;
+ ::std::cout << "LC " << L() * C() << ::std::endl;
+ ::std::cout << "L " << L() << ::std::endl;
+ ::std::cout << "C " << C() << ::std::endl;
+ ::std::cout << "y " << Y << ::std::endl;
+ ::std::cout << "z " << (Y - C() * X_hat) << ::std::endl;
+ ::std::cout << "correction " << L() * (Y - C() * X_hat) << ::std::endl;
+ X_hat = (eye - L() * C()) * X_hat + L() * Y;
+ ::std::cout << "X_hat after " << X_hat << ::std::endl;
+ ::std::cout << ::std::endl;
+ */
+ //LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
Y_ = Y;
new_y_ = true;
}
@@ -299,12 +319,16 @@
CapU();
}
+ //::std::cout << "Predict xhat before " << X_hat;
+ //::std::cout << "Measurement error is " << Y_ - C() * X_hat;
+ //X_hat = A() * X_hat + 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;
}
+ //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of
diff --git a/frc971/control_loops/update_claw.sh b/frc971/control_loops/update_claw.sh
index 083850a..2800d2a 100755
--- a/frc971/control_loops/update_claw.sh
+++ b/frc971/control_loops/update_claw.sh
@@ -4,11 +4,5 @@
cd $(dirname $0)
-./python/claw.py claw/unaugmented_top_claw_motor_plant.h \
- claw/unaugmented_top_claw_motor_plant.cc \
- claw/top_claw_motor_plant.h \
- claw/top_claw_motor_plant.cc \
- claw/unaugmented_bottom_claw_motor_plant.h \
- claw/unaugmented_bottom_claw_motor_plant.cc \
- claw/bottom_claw_motor_plant.h \
- claw/bottom_claw_motor_plant.cc
+./python/claw.py claw/claw_motor_plant.h \
+ claw/claw_motor_plant.cc