Claw now zeros quickly and quite stably.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index a2f5bb6..1b33a2a 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -129,8 +129,8 @@
shooter_zeroing_off_speed,
shooter_zeroing_speed
},
- {0.200000,
- 0.100000,
+ {0.400000,
+ 0.200000,
0.000000,
-0.762218,
0.912207,
diff --git a/frc971/constants.h b/frc971/constants.h
index 3416a39..f696e8b 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,8 +11,8 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-const uint16_t kCompTeamNumber = 971;
-const uint16_t kPracticeTeamNumber = 8971;
+const uint16_t kCompTeamNumber = 8971;
+const uint16_t kPracticeTeamNumber = 971;
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 5924731..dd81ecb 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -45,7 +45,8 @@
namespace frc971 {
namespace control_loops {
-static const double kMaxVoltage = 1.5;
+static const double kZeroingVoltage = 4.0;
+static const double kMaxVoltage = 12.0;
void ClawLimitedLoop::CapU() {
uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
@@ -66,9 +67,10 @@
double max_value =
::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
- if (max_value > kMaxVoltage) {
+ const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
+ if (max_value > k_max_voltage) {
LOG(DEBUG, "Capping U because max is %f\n", max_value);
- U = U * kMaxVoltage / max_value;
+ U = U * k_max_voltage / max_value;
LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
}
}
@@ -516,7 +518,8 @@
LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
claw_.R(1, 0), separation);
- // Only cap power when one of the halves of the claw is unknown.
+ // Only cap power when one of the halves of the claw is moving slowly and
+ // could wind up.
claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
mode_ == FINE_TUNE_BOTTOM);
claw_.Update(output == nullptr);
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 5d6598e..196b62c 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.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.00740659366663, 0.0, 0.0, 1.0, 0.0, 0.00740659366663, 0.0, 0.0, 0.530576083967, 0.0, 0.0, 0.0, 0.0, 0.530576083967;
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.00101390984157, 0.0, 0.0, 0.00101390984157, 0.183524472124, 0.0, 0.0, 0.183524472124;
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.71581823335, 5.38760974287e-16, -1.71581823335, 1.71581823335, 64.8264890043, 2.03572300346e-14, -64.8264890043, 64.8264890043;
+ L << 1.43057608397, -4.48948312405e-16, -1.43057608397, 1.43057608397, 31.1907717473, -9.79345171104e-15, -31.1907717473, 31.1907717473;
Eigen::Matrix<double, 2, 4> K;
- K << 146.100132128, 0.0, 6.7282816813, 0.0, 0.0, 275.346049928, 0.0, 12.0408756114;
+ K << 110.395400642, 0.0, 2.50425726274, 0.0, 0.0, 170.435941688, 0.0, 2.89797614353;
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..73039db 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -14,12 +14,12 @@
# Stall Current in Amps
self.stall_current = 133
# Free Speed in RPM, pulled from drivetrain
- self.free_speed = 4650.0
+ 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
+ self.J = 0.70
# Resistance of the motor
self.R = 12.0 / self.stall_current + 0.024 + .003
# Motor velocity constant
@@ -63,9 +63,9 @@
#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],
- [0.0, 0.0, 0.2, 0.0],
- [0.0, 0.0, 0.0, 2.00]])
+ [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.10, 0.0],
+ [0.0, 0.0, 0.0, 0.1]])
self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
[0.0, (1.0 / (20.0 ** 2.0))]])
@@ -73,6 +73,8 @@
print "K unaugmented"
print self.K
+ print "Placed controller poles"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.rpl = .05
self.ipl = 0.008
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 1150304..8dfec5a 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -29,10 +29,12 @@
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
+const ButtonLocation kClawClosed(3, 7);
+const ButtonLocation kClawOpen(3, 9);
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {}
+ Reader() : closed_(true) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
@@ -105,10 +107,18 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+ if (data.PosEdge(kClawClosed)) {
+ closed_ = true;
+ }
+ if (data.PosEdge(kClawOpen)) {
+ closed_ = false;
+ }
+ double separation_angle = closed_ ? 0.0 : 0.5;
+ double goal_angle = closed_ ? 0.0 : -0.2;
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(0)
- .separation_angle(0)
+ .bottom_angle(goal_angle)
+ .separation_angle(separation_angle)
.intake(false).Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
@@ -121,6 +131,9 @@
}
}
}
+
+ private:
+ bool closed_;
};
} // namespace joysticks