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) {
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 8ef26e7..b32b590 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -50,7 +50,8 @@
min_distance = length;
}
}
- return region_vertices.col(closest_i);
+ return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
+ region_vertices(1, closest_i)).finished();
}
}
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index fe5ccff..aacf31e 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -417,66 +417,90 @@
return numpy.matrix([[new_bottom_u - old_bottom_u], [new_top_u]])
-def main(argv):
- # Simulate the response of the system to a step input.
- #claw = ClawDeltaU()
- #simulated_x = []
- #for _ in xrange(100):
- # claw.Update(numpy.matrix([[12.0]]))
- # simulated_x.append(claw.X[0, 0])
+def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=True, iterations=200):
+ """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
- #pylab.plot(range(100), simulated_x)
- #pylab.show()
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more then max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ claw: claw object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for."""
- # Simulate the closed loop response of the system.
- claw = Claw("TopClaw")
+ claw.X = initial_X
+
+ # Various lists for graphing things.
t = []
- close_loop_x_bottom = []
- close_loop_x_sep = []
- actual_sep = []
- actual_x_bottom = []
- close_loop_x_top = []
- close_loop_u_bottom = []
- close_loop_u_top = []
- R = numpy.matrix([[0.0], [0.00], [0.0], [0.0]])
- claw.X[0, 0] = 1.0
- claw.X[1, 0] = 0.0
- claw.X[2, 0] = 0.0
- claw.X[3, 0] = 0.0
- claw.X_hat = claw.X
- #X_actual = claw.X
- for i in xrange(100):
- #print "Error is", (R - claw.X_hat)
- U = claw.K * (R - claw.X)
- #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
- #U = FullSeparationPriority(claw, U)
- #U = AverageUFix(claw, U, preserve_v3=False)
- #U = claw.K * (R - claw.X_hat)
- #U = ClipDeltaU(claw, U)
- # TODO(austin): This scales the velocity power as well, which is really bad. Need to just scale the position power.
- U = ScaleU(claw, U, claw.K, R - claw.X)
- claw.UpdateObserver(U)
- claw.Update(U)
- #X_actual = claw.A_actual * X_actual + claw.B_actual * U
- #claw.Y = claw.C * X_actual
- close_loop_x_bottom.append(claw.X[0, 0] * 10)
- close_loop_u_bottom.append(U[0, 0])
- #actual_sep.append(X_actual[2, 0] * 100)
- #actual_x_bottom.append(X_actual[0, 0] * 10)
- close_loop_x_sep.append(claw.X[1, 0] * 10)
- close_loop_x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10)
- close_loop_u_top.append(U[1, 0])
- t.append(0.01 * i)
+ x_bottom = []
+ x_top = []
+ u_bottom = []
+ u_top = []
+ x_separation = []
- pylab.plot(t, close_loop_x_bottom, label='x bottom * 10')
- pylab.plot(t, close_loop_x_sep, label='separation * 10')
- #pylab.plot(t, actual_x_bottom, label='true x bottom')
- #pylab.plot(t, actual_sep, label='true separation')
- pylab.plot(t, close_loop_x_top, label='x top * 10')
- pylab.plot(t, close_loop_u_bottom, label='u bottom')
- pylab.plot(t, close_loop_u_top, label='u top')
- pylab.legend()
- pylab.show()
+ tests_passed = True
+
+ # Bounds which separation should not exceed.
+ lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
+ else goal[1, 0]) - max_separation_error
+ upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
+ else goal[1, 0]) + max_separation_error
+
+ for i in xrange(iterations):
+ U = claw.K * (goal - claw.X)
+ U = ScaleU(claw, U, claw.K, goal - claw.X)
+ claw.Update(U)
+
+ if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
+ tests_passed = False
+ print "Claw separation was", claw.X[1, 0]
+ print "Should have been between", lower_bound, "and", upper_bound
+
+ t.append(i * claw.dt)
+ x_bottom.append(claw.X[0, 0] * 10.0)
+ x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
+ u_bottom.append(U[0, 0])
+ u_top.append(U[1, 0])
+ x_separation.append(claw.X[1, 0] * 10.0)
+
+ if show_graph:
+ pylab.plot(t, x_bottom, label='x bottom * 10')
+ pylab.plot(t, x_top, label='x top * 10')
+ pylab.plot(t, u_bottom, label='u bottom')
+ pylab.plot(t, u_top, label='u top')
+ pylab.plot(t, x_separation, label='separation * 10')
+ pylab.legend()
+ pylab.show()
+
+ # Test to make sure that we are near the goal.
+ if numpy.max(abs(claw.X - goal)) > 1e-4:
+ tests_passed = False
+ print "X was", claw.X, "Expected", goal
+
+ return tests_passed
+
+def main(argv):
+ claw = Claw()
+
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
+ # Test just changing separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
+ # Test changing both separation and position at once..
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
# Write the generated constants out to a file.
if len(argv) != 3: