Worked on getting claw to work with newer python code.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
old mode 100755
new mode 100644
index be21efe..1910989
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -46,28 +46,54 @@
namespace control_loops {
void ClawLimitedLoop::CapU() {
- uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
+ uncapped_average_voltage_ = (U(0, 0) + U(1, 0)) / 2.0;
if (is_zeroing_) {
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;
} 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;
}
}
double max_value =
- ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
+ ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
+ double scalar = 12.0 / max_value;
+ 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 u_top = U(1, 0);
+ double u_bottom = 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));
+ 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;
+ // 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;
}
+ 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;
+ if (u_bottom > 12.0) u_bottom = 12.0;
+ else if (u_bottom < -12.0) u_bottom = -12.0;
+ }
+
+ U(0, 0) = u_bottom;
+ U(1, 0) = u_top;
+
+ 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);
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
@@ -391,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) {
+ values.claw.claw_unimportant_epsilon) {//HERE
doing_calibration_fine_tune_ = true;
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
top_claw_velocity_ = bottom_claw_velocity_ =
@@ -429,7 +455,7 @@
// calibrated so we are done fine tuning top
doing_calibration_fine_tune_ = false;
LOG(DEBUG, "Calibrated the top correctly!\n");
- } else {
+ } else { //HERE
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
@@ -553,7 +579,7 @@
}
if (output) {
- output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
+ output->top_claw_voltage = claw_.U(1, 0);
output->bottom_claw_voltage = claw_.U(0, 0);
}
status->done = false;
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 5d6598e..babbb04 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,25 +9,25 @@
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;
+ A << 1.0, 0.0, 0.00737284608086, 0.0, 0.0, 1.0, -0.00294667339472, 0.00442617268614, 0.0, 0.0, 0.525184383468, 0.0, 0.0, 0.0, -0.380328742836, 0.144855640632;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000326582411818, 0.0, 0.0, 0.000326582411818, 0.0631746179893, 0.0, 0.0, 0.0631746179893;
+ B << 0.00102145540588, 0.0, -0.00102145540588, 0.00216714216844, 0.184611558069, 0.0, -0.184611558069, 0.332485973629;
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;
+ U_max << 12.0, 12.0;
Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -24.0;
+ U_min << -12.0, -12.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;
+ L << 1.42518438347, 4.71027737605e-16, -1.42518438347, 1.04485564063, 30.6346010502, 1.00485917356e-14, -30.6346010502, 2.04727497147;
Eigen::Matrix<double, 2, 4> K;
- K << 146.100132128, 0.0, 6.7282816813, 0.0, 0.0, 275.346049928, 0.0, 12.0408756114;
+ K << 50.0, 0.0, 1.0, 0.0, 23.5668757858, 300.0, -0.88836718554, 1.1;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b0767fb..f0a522a 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -165,8 +165,8 @@
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [24.0]])
- self.U_min = numpy.matrix([[-12.0], [-24.0]])
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()