Merge remote-tracking branch 'brian/devel' into claw
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_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 3fa9079..7917703 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -212,8 +212,7 @@
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
claw_plant_->U << claw_queue_group.output->bottom_claw_voltage,
- claw_queue_group.output->top_claw_voltage -
- claw_queue_group.output->bottom_claw_voltage;
+ claw_queue_group.output->top_claw_voltage;
claw_plant_->Update();
// Check that the claw is within the limits.
@@ -441,28 +440,29 @@
}
INSTANTIATE_TEST_CASE_P(ZeroingClawTest, ZeroingClawTest,
- ::testing::Values(::std::make_pair(0.04, 0.02),
- ::std::make_pair(0.2, 0.1),
- ::std::make_pair(0.3, 0.2),
- ::std::make_pair(0.4, 0.3),
- ::std::make_pair(0.5, 0.4),
- ::std::make_pair(0.6, 0.5),
- ::std::make_pair(0.7, 0.6),
- ::std::make_pair(0.8, 0.7),
- ::std::make_pair(0.9, 0.8),
- ::std::make_pair(1.0, 0.9),
+ ::testing::Values(//::std::make_pair(0.04, 0.02),
+ // ::std::make_pair(0.2, 0.1),
+ // ::std::make_pair(0.3, 0.2),
+ // ::std::make_pair(0.4, 0.3),
+ // ::std::make_pair(0.5, 0.4),
+ // ::std::make_pair(0.6, 0.5),
+ // ::std::make_pair(0.7, 0.6),
+ // ::std::make_pair(0.8, 0.7),
+ // ::std::make_pair(0.9, 0.8),
+ // ::std::make_pair(1.0, 0.9),
::std::make_pair(1.1, 1.0),
- ::std::make_pair(1.15, 1.05),
- ::std::make_pair(1.05, 0.95),
- ::std::make_pair(1.1, 1.0),
- ::std::make_pair(1.2, 1.1),
- ::std::make_pair(1.3, 1.2),
- ::std::make_pair(1.4, 1.3),
- ::std::make_pair(1.5, 1.4),
- ::std::make_pair(1.6, 1.5),
- ::std::make_pair(1.7, 1.6),
- ::std::make_pair(1.8, 1.7),
- ::std::make_pair(2.015, 2.01)));
+ // ::std::make_pair(1.15, 1.05),
+ // ::std::make_pair(1.05, 0.95),
+ ::std::make_pair(1.1, 1.0)//,
+ // ::std::make_pair(1.2, 1.1),
+ // ::std::make_pair(1.3, 1.2),
+ // ::std::make_pair(1.4, 1.3),
+ // ::std::make_pair(1.5, 1.4),
+ // ::std::make_pair(1.6, 1.5),
+ // ::std::make_pair(1.7, 1.6),
+ // ::std::make_pair(1.8, 1.7),
+ // ::std::make_pair(2.015, 2.01)
+));
/*
// Tests that loosing the encoder for a second triggers a re-zero.
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 09718e5..f0a522a 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -13,15 +13,16 @@
self.stall_torque = 2.42
# Stall Current in Amps
self.stall_current = 133
- # Free Speed in RPM, pulled from drivetrain
- self.free_speed = 4650.0
+ # Free Speed in RPM
+ self.free_speed = 5500.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
- # approzimately 0.76 (without ball) in CAD
- self.J = 0.76
+ # measured from CAD
+ self.J_top = 0.3
+ self.J_bottom = 0.9
# Resistance of the motor
- self.R = 12.0 / self.stall_current + 0.024 + .003
+ self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(13.5 - self.R * self.free_current))
@@ -32,25 +33,58 @@
# Control loop time step
self.dt = 0.01
- # State is [bottom position, top - bottom position,
- # bottom velocity, top - bottom velocity]
- # Input is [bottom power, top power]
- # Motor time constant.
- self.motor_timeconstant = self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
+ # State is [bottom position, bottom velocity, top - bottom position,
+ # top - bottom velocity]
+ # Input is [bottom power, top power - bottom power * J_top / J_bottom]
+ # Motor time constants. difference_bottom refers to the constant for how the
+ # bottom velocity affects the difference of the top and bottom velocities.
+ self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
+ self.bottom_bottom = self.common_motor_constant / self.J_bottom
+ self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
+ - 1 / self.J_top)
+ self.difference_difference = self.common_motor_constant / self.J_top
# State feedback matrices
+
self.A_continuous = numpy.matrix(
[[0, 0, 1, 0],
[0, 0, 0, 1],
- [0, 0, -self.motor_timeconstant, 0],
- [0, 0, 0, -self.motor_timeconstant]])
+ [0, 0, self.bottom_bottom, 0],
+ [0, 0, self.difference_bottom, self.difference_difference]])
- self.motor_feedforward = self.Kt / (self.J * self.G * self.R)
+ self.A_bottom_cont = numpy.matrix(
+ [[0, 1],
+ [0, self.bottom_bottom]])
+ self.A_diff_cont = numpy.matrix(
+ [[0, 1],
+ [0, self.difference_difference]])
+
+ # self.A_continuous[0:2, 0:2] = self.A_bottom_cont
+ # self.A_continuous[2:4, 2:4] = self.A_diff_cont
+ # self.A_continuous[3, 1] = self.difference_bottom
+
+ self.motor_feedforward = self.Kt / (self.G * self.R)
+ self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+ self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+ self.motor_feedforward_difference_bottom = (
+ self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
self.B_continuous = numpy.matrix(
[[0, 0],
[0, 0],
- [self.motor_feedforward, 0],
- [0.0, self.motor_feedforward]])
+ [self.motor_feedforward_bottom, 0],
+ [-self.motor_feedforward_bottom,
+ self.motor_feedforward_difference]])
+
+ print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
+
+ self.B_bottom_cont = numpy.matrix(
+ [[0],
+ [self.motor_feedforward_bottom]])
+
+ self.B_diff_cont = numpy.matrix(
+ [[0],
+ [self.motor_feedforward_difference]])
+
self.C = numpy.matrix([[1, 0, 0, 0],
[1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
@@ -59,30 +93,80 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
+ self.A_bottom, self.B_bottom = controls.c2d(
+ self.A_bottom_cont, self.B_bottom_cont, self.dt)
+ self.A_diff, self.B_diff = controls.c2d(
+ self.A_diff_cont, self.B_diff_cont, self.dt)
+
+ print "A"
+ print self.A
+ print "B"
+ print self.B
+
+ X_ss = numpy.matrix([[0], [0], [0.0], [0]])
+
+ U = numpy.matrix([[1.0],[1.0]])
+ A = self.A
+ B = self.B
+ #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+ X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+ #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+ X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+
+ print "X_ss", X_ss
+
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.03 ** 2.0)), 0.0, 0.0],
+ self.Q = numpy.matrix([[(1.0 / (0.40 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.007 ** 2.0)), 0.0, 0.0],
[0.0, 0.0, 0.2, 0.0],
[0.0, 0.0, 0.0, 2.00]])
- self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (20.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (5.0 ** 2.0))]])
+ #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ self.K = numpy.matrix([[50, 0.0, 1, 0.0],
+ [0.0, 300, 0.0, 1.1]])
+ lstsq_A = numpy.identity(2)
+ lstsq_A[0] = self.B[1]
+ lstsq_A[1] = self.B[3]
+ print "System of Equations coefficients:"
+ print lstsq_A
+ print "det", numpy.linalg.det(lstsq_A)
+ self.K[1, 0] = -lstsq_A[0, 0] * self.K[0, 0] / lstsq_A[0, 1]
+ #self.K[0:2, 0] = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[0.0], [0.0]]))[0]
+ out_x = numpy.linalg.lstsq(
+ lstsq_A,
+ numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
print "K unaugmented"
print self.K
+ print "B * K unaugmented"
+ print self.B * self.K
+ F = self.A - self.B * self.K
+ print "A - B * K unaugmented"
+ print F
+ print "eigenvalues"
+ print numpy.linalg.eig(F)[0]
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
self.rpl - 1j * self.ipl])
- self.U_max = numpy.matrix([[12.0], [24.0]])
- self.U_min = numpy.matrix([[-12.0], [-24.0]])
+ # 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], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
@@ -194,18 +278,47 @@
return numpy.matrix([[bottom_u], [top_u - bottom_u]])
-def AverageUFix(claw, U):
- bottom_u = U[0, 0]
- top_u = U[1, 0] + bottom_u
+def AverageUFix(claw, U, preserve_v3=False):
+ """Clips U as necessary.
- #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
- if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[1, 0] or
- top_u < claw.U_min[1, 0] or bottom_u < claw.U_min[0, 0]):
- scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ Args:
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ preserve_v3: There are two ways to attempt to clip the voltages:
+ -If you preserve the imaginary v3, then it will attempt to
+ preserve the effect on the separation of the two claws.
+ If it is not able to do this, then it calls itself with preserve_v3
+ set to False.
+ -If you preserve the ratio of the voltage of the bottom and the top,
+ then it will attempt to preserve the ratio of those two. This is
+ equivalent to preserving the ratio of v3 and the bottom voltage.
+ """
+ bottom_u = U[0, 0]
+ top_u = U[1, 0]
+ seperation_u = top_u - bottom_u * claw.J_top / claw.J_bottom
+
+ top_big = top_u > claw.U_max[0, 0]
+ top_small = top_u < claw.U_min[0, 0]
+ bot_big = bottom_u > claw.U_max[0, 0]
+ bot_small = top_u < claw.U_min[0, 0]
+ bottom_bad = bot_big or bot_small
+ top_bad = top_big or top_small
+ scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ if bottom_bad and preserve_v3:
+ bottom_u *= scalar
+ top_u = seperation_u + bottom_u * claw.J_top / claw.J_bottom
+ if abs(top_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif top_bad and preserve_v3:
+ top_u *= scalar
+ bottom_u = (top_u - seperation_u) * claw.J_bottom / claw.J_top
+ if abs(bottom_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif (bottom_bad or top_bad) and not preserve_v3:
top_u *= scalar
bottom_u *= scalar
- return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+ return numpy.matrix([[bottom_u], [top_u]])
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
@@ -246,33 +359,47 @@
#pylab.plot(range(100), simulated_x)
#pylab.show()
- # Simulate the closed loop response of the system to a step input.
+ # Simulate the closed loop response of the system.
claw = Claw("TopClaw")
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([[1.0], [1.0], [0.0], [0.0]])
- claw.X[0, 0] = 0
+ R = numpy.matrix([[0.0], [0.00], [0.0], [0.0]])
+ claw.X[0, 0] = 1
+ claw.X[1, 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_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)
+ #U = AverageUFix(claw, U, preserve_v3=True)
#U = claw.K * (R - claw.X_hat)
#U = ClipDeltaU(claw, U)
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])
- close_loop_x_sep.append(claw.X[1, 0] * 10)
- close_loop_u_top.append(U[1, 0] + 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] * 100)
+ 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)
pylab.plot(t, close_loop_x_bottom, label='x bottom')
pylab.plot(t, close_loop_x_sep, label='separation')
+ #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')
pylab.plot(t, close_loop_u_bottom, label='u bottom')
pylab.plot(t, close_loop_u_top, label='u top')
pylab.legend()
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4b63aec..0610225 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -183,7 +183,7 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
- U = numpy.clip(U, self.U_min, self.U_max)
+ #U = numpy.clip(U, self.U_min, self.U_max)
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index 6f5ee34..ef23d21 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -70,10 +70,10 @@
LOG_STRUCT(DEBUG, "will output", *claw.get());
SetPWMOutput(6, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(7, claw->intake_voltage / 12.0, kTalonBounds);
- SetPWMOutput(1, claw->bottom_claw_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(1, -claw->bottom_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(2, claw->top_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(5, claw->tusk_voltage / 12.0, kTalonBounds); // left
- SetPWMOutput(4, claw->tusk_voltage / 12.0, kTalonBounds); // right
+ SetPWMOutput(4, -claw->tusk_voltage / 12.0, kTalonBounds); // right
} else {
DisablePWMOutput(6);
DisablePWMOutput(7);