Cleaned stuff up and made claw tests actually pass.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 1910989..1934032 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -51,14 +51,14 @@
const frc971::constants::Values &values = constants::GetValues();
if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
const double difference =
- uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
- U(0, 0) -= difference;
- U(1, 0) -= difference;
+ 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
+ U(0, 0) *= difference;
+ U(1, 0) *= difference;
} else if (uncapped_average_voltage_ < -values.claw.max_zeroing_voltage) {
const double difference =
- -uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
- U(0, 0) += difference;
- U(1, 0) += difference;
+ 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
+ U(0, 0) *= difference;
+ U(1, 0) *= difference;
}
}
@@ -68,14 +68,14 @@
bool bottom_big = (::std::abs(U(0, 0)) > 12.0) &&
(::std::abs(U(0, 0)) > ::std::abs(U(1, 0)));
bool top_big = (::std::abs(U(1, 0)) > 12.0) && (!bottom_big);
- double separation_voltage = U(1, 0) - U(0, 0) / 3.0000;
+ double separation_voltage = U(1, 0) - U(0, 0) * kClawMomentOfInertiaRatio;
double u_top = U(1, 0);
double u_bottom = U(0, 0);
if (bottom_big) {
LOG(DEBUG, "Capping U because bottom is %f\n", max_value);
u_bottom *= scalar;
- u_top = separation_voltage + u_bottom / 3.0000;
+ u_top = separation_voltage + u_bottom * kClawMomentOfInertiaRatio;
// If we can't maintain the separation, just clip it.
if (u_top > 12.0) u_top = 12.0;
else if (u_top < -12.0) u_top = -12.0;
@@ -83,7 +83,7 @@
else if (top_big) {
LOG(DEBUG, "Capping U because top is %f\n", max_value);
u_top *= scalar;
- u_bottom = (u_top - separation_voltage) * 3.0000;
+ u_bottom = (u_top - separation_voltage) / kClawMomentOfInertiaRatio;
if (u_bottom > 12.0) u_bottom = 12.0;
else if (u_bottom < -12.0) u_bottom = -12.0;
}
@@ -93,7 +93,7 @@
LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
LOG(DEBUG, "Separation Voltage was %f, is now %f\n", separation_voltage,
- U(1, 0) - U(0, 0) / 3.0000);
+ U(1, 0) - U(0, 0) * kClawMomentOfInertiaRatio);
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
@@ -132,12 +132,12 @@
if (sensor.negedge_count_changed()) {
if (negedge_value_ > last_encoder()) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge upper of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
+ hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
+ hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
}
*edge_encoder = negedge_value_;
return true;
@@ -417,7 +417,7 @@
if (!doing_calibration_fine_tune_) {
if (::std::abs(top_absolute_position() -
values.claw.start_fine_tune_pos) <
- values.claw.claw_unimportant_epsilon) {//HERE
+ values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
top_claw_velocity_ = bottom_claw_velocity_ =
@@ -455,7 +455,7 @@
// calibrated so we are done fine tuning top
doing_calibration_fine_tune_ = false;
LOG(DEBUG, "Calibrated the top correctly!\n");
- } else { //HERE
+ } else {
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
@@ -553,12 +553,20 @@
case FINE_TUNE_BOTTOM:
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
+ Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx = (claw_.uncapped_average_voltage() -
+ double dx_bot = (U(0, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
+ double dx_top = (U(1, 0) -
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
+ U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
@@ -567,11 +575,18 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx = (claw_.uncapped_average_voltage() +
+ double dx_bot = (U(0, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
+ double dx_top = (U(1, 0) +
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
+ U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}