Added stuff for properly capping voltage on claw.
Compiles, tests pass, generally good :).
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 4d7442b..b804399 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -1,7 +1,5 @@
#include "frc971/control_loops/claw/claw.h"
-#include <stdio.h>
-
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
@@ -51,44 +49,71 @@
ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
: StateFeedbackLoop<4, 2, 2>(loop),
uncapped_average_voltage_(0.0),
- is_zeroing_(true) {}
+ is_zeroing_(true),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
+ kMaxVoltage, kMaxVoltage).finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+}
+// Caps the voltage prioritizing reducing velocity error over reducing
+// positional error.
+// Uses the polytope libararies which we used to just use for the drivetrain.
+// Uses a region representing the maximum voltage and then transforms it such
+// that the points represent different amounts of positional error and
+// constrains the region such that, if at all possible, it will maintain its
+// current efforts to reduce velocity error.
void ClawLimitedLoop::CapU() {
- uncapped_average_voltage_ = (U(0, 0) + U(1, 0)) / 2.0;
+ Eigen::Matrix<double, 4, 1> error = R - X_hat;
- const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
- double max_value =
- ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
- double scalar = k_max_voltage / max_value;
- bool bottom_big = (::std::abs(U(0, 0)) > k_max_voltage) &&
- (::std::abs(U(0, 0)) > ::std::abs(U(1, 0)));
- bool top_big = (::std::abs(U(1, 0)) > k_max_voltage) && (!bottom_big);
- 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 * kClawMomentOfInertiaRatio;
- // If we can't maintain the separation, just clip it.
- if (u_top > k_max_voltage) u_top = k_max_voltage;
- else if (u_top < -k_max_voltage) u_top = -k_max_voltage;
- }
- else if (top_big) {
- LOG(DEBUG, "Capping U because top is %f\n", max_value);
- u_top *= scalar;
- u_bottom = (u_top - separation_voltage) / kClawMomentOfInertiaRatio;
- if (u_bottom > k_max_voltage) u_bottom = k_max_voltage;
- else if (u_bottom < -k_max_voltage) u_bottom = -k_max_voltage;
+ uncapped_average_voltage_ = (u_top + u_bottom) / 2;
+
+ double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
+
+ if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+ // H * U <= k
+ // U = UPos + UVel
+ // H * (UPos + UVel) <= k
+ // H * UPos <= k - H * UVel
+
+ // Now, we can do a coordinate transformation and say the following.
+
+ // UPos = position_K * position_error
+ // (H * position_K) * position_error <= k - H * UVel
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 1),
+ K(1, 0), K(1, 1);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 2), K(0, 3),
+ K(1, 2), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(1, 0);
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(2, 0), error(3, 0);
+
+ 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);
+
+
+ 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);
+
+ LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+ U = velocity_K * velocity_error + position_K * adjusted_pos_error;
}
- 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) * kClawMomentOfInertiaRatio);
}
ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
@@ -669,6 +694,7 @@
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));
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
double dx_bot = (U(0, 0) -
values.claw.max_zeroing_voltage) /
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 8bb94c0..f71bf6b 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -29,11 +29,15 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
],
'export_dependent_settings': [
'claw_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
],
},
{
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
old mode 100755
new mode 100644
index dc8e6ba..48650b4
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -4,8 +4,10 @@
#include <memory>
#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/controls/polytope.h"
#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
#include "frc971/control_loops/hall_effect_tracker.h"
@@ -36,6 +38,8 @@
private:
double uncapped_average_voltage_;
bool is_zeroing_;
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
};
class ClawMotor;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 43eacd5..e35cd7f 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -547,7 +547,6 @@
protected:
void TestWindup(ClawMotor::CalibrationMode mode, int start_time, double offset) {
int capped_count = 0;
- double saved_zeroing_position[2] = {0, 0};
const frc971::constants::Values& values = constants::GetValues();
bool kicked = false;
bool measured = false;
@@ -557,8 +556,6 @@
EXPECT_EQ(mode, claw_motor_.mode());
// Move the zeroing position far away and verify that it gets moved
// back.
- saved_zeroing_position[TOP_CLAW] = claw_motor_.top_claw_goal_;
- saved_zeroing_position[BOTTOM_CLAW] = claw_motor_.bottom_claw_goal_;
claw_motor_.top_claw_goal_ += offset;
claw_motor_.bottom_claw_goal_ += offset;
kicked = true;
@@ -567,10 +564,19 @@
measured = true;
EXPECT_EQ(mode, claw_motor_.mode());
- EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
- claw_motor_.top_claw_goal_, 0.1);
- EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
- claw_motor_.bottom_claw_goal_, 0.1);
+ Eigen::Matrix<double, 4, 1> R;
+ R << claw_motor_.bottom_claw_goal_,
+ claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
+ 0.0;
+ Eigen::Matrix<double, 2, 1> uncapped_voltage =
+ claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat);
+ // Use a factor of 1.8 because so long as it isn't actually running
+ // away, the CapU function will deal with getting the actual output
+ // down.
+ EXPECT_LT(::std::abs(uncapped_voltage(0, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
+ EXPECT_LT(::std::abs(uncapped_voltage(1, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
}
}
if (claw_motor_.mode() == mode) {