Made test mode and the claw work on the robot.
Change-Id: I29a5677f616f70f9e8a8595f05cacbd3a85dcba6
diff --git a/frc971/constants.cc b/frc971/constants.cc
index e197f99..c68990f 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -252,7 +252,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.977913},
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.973311},
6.1663463999999992,
kClawPistonSwitchTime,
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 1bb4c10..4b47c02 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,9 +25,9 @@
StateFeedbackController<2, 1, 1> MakeClawController() {
Eigen::Matrix<double, 2, 1> L;
- L << 1.53065249533, 111.171516288;
+ L << 1.33065249533, 84.5098852645;
Eigen::Matrix<double, 1, 2> K;
- K << 284.338418915, 17.4107932965;
+ K << 115.895112435, 8.54319939792;
Eigen::Matrix<double, 2, 2> A_inv;
A_inv << 1.0, -0.00518405612386, 0.0, 1.07451492907;
return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeClawPlantCoefficients());
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 1a25f95..3395b1d 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -64,24 +64,21 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print self.A
-
controlability = controls.ctrb(self.A, self.B);
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
- q_pos = 0.03
- q_vel = 0.5
+ q_pos = 0.09
+ q_vel = 1.2
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ print 'K', self.K
+ print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
- self.rpl = 0.20
- self.ipl = 0.05
+ self.rpl = 0.30
+ self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index b67c160..09d815c 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -36,17 +36,17 @@
const ButtonLocation kQuickTurn(1, 5);
// TODO(danielp): Real buttons for all of these.
-const ButtonLocation kElevatorUp(99, 99);
-const ButtonLocation kElevatorDown(99, 99);
-const ButtonLocation kArmUp(99, 99);
-const ButtonLocation kArmDown(99, 99);
-const ButtonLocation kClawUp(99, 99);
-const ButtonLocation kClawDown(99, 99);
+const ButtonLocation kElevatorUp(3, 10);
+const ButtonLocation kElevatorDown(2, 6);
+const ButtonLocation kArmUp(3, 8);
+const ButtonLocation kArmDown(3, 3);
+const ButtonLocation kClawUp(3, 7);
+const ButtonLocation kClawDown(3, 6);
// Testing mode.
-const double kElevatorVelocity = 0.1;
-const double kArmVelocity = 0.2;
-const double kClawVelocity = 0.2;
+const double kElevatorVelocity = 0.5;
+const double kArmVelocity = 0.5;
+const double kClawVelocity = 2.0;
// TODO(danielp): Verify.
const double kJoystickDt = 0.01;
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 9c82ae5..b5c87b1 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -532,8 +532,8 @@
auto &queue = ::frc971::control_loops::claw_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
left_intake_talon_->Set(queue->intake_voltage / 12.0);
- right_intake_talon_->Set(queue->intake_voltage / 12.0);
- wrist_talon_->Set(queue->voltage / 12.0);
+ right_intake_talon_->Set(-queue->intake_voltage / 12.0);
+ wrist_talon_->Set(-queue->voltage / 12.0);
}
virtual void Stop() override {