tuned and redid the capping on the claw to prioritize separation error
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 58b2ca0..cbd4d2a 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -5,6 +5,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -44,6 +45,17 @@
namespace frc971 {
namespace control_loops {
+namespace {
+
+template <typename T> int sign(T val) {
+ if (val > T(0)) {
+ return 1;
+ } else {
+ return -1;
+ }
+}
+
+} // namespace
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -57,7 +69,14 @@
0, 1,
0, -1).finished(),
(Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
- kMaxVoltage, kMaxVoltage).finished()) {
+ kMaxVoltage, kMaxVoltage).finished()),
+ U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() <<
+ kZeroingVoltage, kZeroingVoltage,
+ kZeroingVoltage, kZeroingVoltage).finished()) {
::aos::controls::HPolytope<0>::Init();
}
@@ -79,6 +98,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+ LOG_MATRIX(DEBUG, "U at start", U);
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -100,22 +120,59 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
- Eigen::Matrix<double, 4, 1> pos_poly_k =
- U_Poly_.k() - U_Poly_.H() * velocity_K * velocity_error;
- Eigen::Matrix<double, 4, 2> pos_poly_H = U_Poly_.H() * position_K;
- ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = position_error;
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << sign(P(1, 0)), -sign(P(0, 0));
+ const double w45 = 0;
- Eigen::Matrix<double, 2, 1> adjusted_pos_error = CoerceGoal(
- pos_poly, (Eigen::Matrix<double, 1, 2>() << position_error(1, 0),
- -position_error(0, 0)).finished(),
- 0.0, position_error);
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
- LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, position_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
U = velocity_K * velocity_error + position_K * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U);
}
-
}
ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
@@ -741,13 +798,12 @@
case FINE_TUNE_BOTTOM:
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
- Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
- LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", U(1, 0), U(0, 0));
+ LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", claw_.U_uncapped(1, 0), claw_.U_uncapped(0, 0));
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (U(0, 0) -
+ double dx_bot = (claw_.U_uncapped(0, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (U(1, 0) -
+ double dx_top = (claw_.U_uncapped(1, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
@@ -755,7 +811,7 @@
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);
+ claw_.U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -765,10 +821,10 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (U(0, 0) +
+ double dx_bot = (claw_.U_uncapped(0, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (U(1, 0) +
+ double dx_top = (claw_.U_uncapped(1, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
@@ -776,7 +832,7 @@
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);
+ claw_.U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index bdce229..5982378 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -32,6 +32,7 @@
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index dab1dca..6dbc408 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -39,7 +39,7 @@
double uncapped_average_voltage_;
bool is_zeroing_;
- const ::aos::controls::HPolytope<2> U_Poly_;
+ const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
};
class ClawMotor;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 8704ad0..61813bc 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -304,7 +304,7 @@
claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
EXPECT_NEAR(claw_queue_group.goal->separation_angle, separation, 1e-4);
- EXPECT_TRUE(min_separation_ <= separation);
+ EXPECT_LE(min_separation_, separation);
}
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 8da2415..28151ad 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0, 0.00737284608086, 0.0, 0.0, 1.0, -0.00145272885484, 0.00592011722602, 0.0, 0.0, 0.525184383468, 0.0, 0.0, 0.0, -0.211450629042, 0.313733754426;
+ A << 1.0, 0.0, 0.00909331035298, 0.0, 0.0, 1.0, -6.04514323962e-05, 0.00903285892058, 0.0, 0.0, 0.824315642255, 0.0, 0.0, 0.0, -0.0112975266368, 0.813018115618;
Eigen::Matrix<double, 4, 2> B;
- B << 0.00102145540588, 0.0, -0.00102145540588, 0.00158628631709, 0.184611558069, 0.0, -0.184611558069, 0.26682500835;
+ B << 0.000352527133889, 0.0, -0.000352527133889, 0.000376031064118, 0.0683072794628, 0.0, -0.0683072794628, 0.0726998350615;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 1, 1, 0, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.48518438347, -2.35513868803e-16, -1.48518438347, 1.27373375443, 34.6171964667, -5.41681898246e-15, -34.6171964667, 14.5766570483;
+ L << 1.72431564225, 1.53329341668e-19, -1.72431564225, 1.71301811562, 65.9456997026, -0, -65.9456997026, 64.4642687194;
Eigen::Matrix<double, 2, 4> K;
- K << 104.272994613, 0.0, 1.72618753001, 0.0, 67.1443817466, 107.935909674, 0.195736876688, 0.983852673373;
+ K << 106.138028875, 0.0, 4.20012492658, 0.0, 99.5038407367, 99.7251230882, 3.77683310096, 3.78980738032;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
index 95d0fee..76e3fe7 100644
--- a/frc971/control_loops/claw/claw_motor_plant.h
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -14,7 +14,7 @@
StateFeedbackLoop<4, 2, 2> MakeClawLoop();
-const double kClawMomentOfInertiaRatio = 0.555556;
+const double kClawMomentOfInertiaRatio = 0.933333;
} // namespace control_loops
} // namespace frc971