Merge remote-tracking branch 'austin/claw' into claw
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 971c1a6..4b181eb 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -39,7 +39,7 @@
const ShifterHallEffect kPracticeRightDriveShifter{5, 0, 0.62,
0.55};
const double shooter_zeroing_off_speed = 0.0;
-const double shooter_zeroing_speed = 0.1;
+const double shooter_zeroing_speed = 0.05;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
@@ -121,33 +121,25 @@
control_loops::MakeDogDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {-0.000446, 0.300038, -0.001, 0.304354,
- 0.014436,
- {-2, 0.001786, 0.001786, -2},
- {-2, -0.000446, -2, 0.026938},
- {0.005358, 0.014436, 0.014436, 0.026491},
+ {-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
+ {-0.002, 0.000446, -0.002, 0.000446},
+ {-0.002, 0.009078, -0.002, 0.009078},
+ {0.003869, 0.026194, 0.003869, 0.026194},
shooter_zeroing_off_speed,
shooter_zeroing_speed
},
- {0.5,
- 0.2,
- 0.1,
- -0.446558,
- 0.90675,
- -0.39110,
- 0.843349,
-#if 0
- separations (top, bottom)
- hard min position:-0.253845, position:-0.001136,
- soft min position:-0.244528, position:-0.047269,
- soft max position:0.526326, position:-0.510872,
- hard max position:0.517917, position:-0.582685,
-#endif
- {-1.62102, 1.039699, -1.606248, 0.989702, {-1.65, -1.546252, -1.65, -1.548752}, {-0.13249, -0.02113, -0.134763, -0.021589}, {0.934024, 1.05, 0.92970, 1.05}},
- {-1.420352, 1.348313, -1.161281, 1.264001, {-1.45, -1.283771, -1.45, -1.28468}, {-0.332476, -0.214984, -0.334294, -0.217029}, {1.248547, 1.37, 1.245366, 1.37}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
+ {0.400000,
+ 0.200000,
+ 0.000000,
+ -0.762218,
+ 0.912207,
+ -0.362218,
+ 0.512207,
+ {-1.682379, 1.043334, -1.282379, 0.643334, {-1.7, -1.544662, -1.7, -1.547616}, {-0.130218, -0.019771, -0.132036, -0.018862}, {0.935842, 1.1, 0.932660, 1.1}},
+ {-1.225821, 1.553752, -0.825821, 1.153752, {-1.3, -1.088331, -1.3, -1.088331}, {-0.134536, -0.018408, -0.136127, -0.019771}, {1.447396, 1.6, 1.443987, 1.6}},
+ 0.020000, // claw_unimportant_epsilon
+ -0.200000, // start_fine_tune_pos
+ 4.000000,
}
};
break;
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 1934032..11719ee 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -45,29 +45,19 @@
namespace frc971 {
namespace control_loops {
+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;
- if (is_zeroing_) {
- const frc971::constants::Values &values = constants::GetValues();
- if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
- const double difference =
- 1 / 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 =
- 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
- U(0, 0) *= difference;
- U(1, 0) *= difference;
- }
- }
+ 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 = 12.0 / max_value;
- bool bottom_big = (::std::abs(U(0, 0)) > 12.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)) > 12.0) && (!bottom_big);
+ 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);
@@ -77,15 +67,15 @@
u_bottom *= scalar;
u_top = separation_voltage + u_bottom * kClawMomentOfInertiaRatio;
// 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;
+ 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 > 12.0) u_bottom = 12.0;
- else if (u_bottom < -12.0) u_bottom = -12.0;
+ if (u_bottom > k_max_voltage) u_bottom = k_max_voltage;
+ else if (u_bottom < -k_max_voltage) u_bottom = -k_max_voltage;
}
U(0, 0) = u_bottom;
@@ -116,34 +106,35 @@
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
double *edge_angle, const HallEffectTracker &sensor,
const char *hall_effect_name) {
+ bool found_edge = false;
if (sensor.posedge_count_changed()) {
- if (posedge_value_ < last_encoder()) {
+ if (posedge_value_ < last_off_encoder_) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Posedge upper of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f last_encoder: %f\n", name_,
+ hall_effect_name, *edge_angle, posedge_value_, last_off_encoder_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Posedge lower of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f last_encoder: %f\n", name_,
+ hall_effect_name, *edge_angle, posedge_value_, last_off_encoder_);
}
*edge_encoder = posedge_value_;
- return true;
+ found_edge = true;
}
if (sensor.negedge_count_changed()) {
- if (negedge_value_ > last_encoder()) {
+ if (negedge_value_ > last_on_encoder_) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
- hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
+ LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f last_encoder: %f\n", name_,
+ hall_effect_name, *edge_angle, negedge_value_, last_on_encoder_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
- hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
+ LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f last_encoder: %f\n", name_,
+ hall_effect_name, *edge_angle, negedge_value_, last_on_encoder_);
}
*edge_encoder = negedge_value_;
- return true;
+ found_edge = true;
}
- return false;
+ return found_edge;
}
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
@@ -286,6 +277,8 @@
if (reset()) {
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ top_claw_.Reset();
+ bottom_claw_.Reset();
}
if (::aos::robot_state.get() == nullptr) {
@@ -536,7 +529,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);
@@ -596,6 +590,18 @@
if (output) {
output->top_claw_voltage = claw_.U(1, 0);
output->bottom_claw_voltage = claw_.U(0, 0);
+
+ if (output->top_claw_voltage > kMaxVoltage) {
+ output->top_claw_voltage = kMaxVoltage;
+ } else if (output->top_claw_voltage < -kMaxVoltage) {
+ output->top_claw_voltage = -kMaxVoltage;
+ }
+
+ if (output->bottom_claw_voltage > kMaxVoltage) {
+ output->bottom_claw_voltage = kMaxVoltage;
+ } else if (output->bottom_claw_voltage < -kMaxVoltage) {
+ output->bottom_claw_voltage = -kMaxVoltage;
+ }
}
status->done = false;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 2d13d37..bff533d 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -86,9 +86,24 @@
posedge_value_ = claw.posedge_value;
negedge_value_ = claw.negedge_value;
last_encoder_ = encoder_;
+ if (front().value() || calibration().value() || back().value()) {
+ last_on_encoder_ = encoder_;
+ } else {
+ last_off_encoder_ = encoder_;
+ }
encoder_ = claw.position;
}
+ void Reset() {
+ front_.Reset();
+ calibration_.Reset();
+ back_.Reset();
+ }
+
+ bool ready() {
+ return front_.ready() && calibration_.ready() && back_.ready();
+ }
+
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -130,6 +145,8 @@
double negedge_value_;
double encoder_;
double last_encoder_;
+ double last_on_encoder_;
+ double last_off_encoder_;
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
@@ -152,7 +169,7 @@
double edge_encoder;
double edge_angle;
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
+ LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(zeroing_state);
return true;
diff --git a/frc971/control_loops/claw/claw_calibration.cc b/frc971/control_loops/claw/claw_calibration.cc
index 0167c08..608c72d 100644
--- a/frc971/control_loops/claw/claw_calibration.cc
+++ b/frc971/control_loops/claw/claw_calibration.cc
@@ -8,69 +8,144 @@
typedef constants::Values::Claws Claws;
-bool DoGetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position,
- const HallEffectStruct &last_hall_effect,
- const HallEffectStruct &hall_effect,
- Claws::AnglePair *limits) {
+class Sensor {
+ public:
+ Sensor(const double start_position,
+ const HallEffectStruct &initial_hall_effect)
+ : start_position_(start_position),
+ last_hall_effect_(initial_hall_effect),
+ last_posedge_count_(initial_hall_effect.posedge_count),
+ last_negedge_count_(initial_hall_effect.negedge_count) {
+ last_on_min_position_ = start_position;
+ last_on_max_position_ = start_position;
+ last_off_min_position_ = start_position;
+ last_off_max_position_ = start_position;
+ }
- if (hall_effect.posedge_count != last_hall_effect.posedge_count) {
- if (claw_position.posedge_value < last_claw_position.position) {
- limits->upper_angle = claw_position.posedge_value - start_position;
- } else {
- limits->lower_decreasing_angle =
- claw_position.posedge_value - start_position;
+ bool DoGetPositionOfEdge(
+ const control_loops::HalfClawPosition &claw_position,
+ const HallEffectStruct &hall_effect, Claws::AnglePair *limits) {
+ bool print = false;
+
+ if (hall_effect.posedge_count != last_posedge_count_) {
+ const double avg_off_position = (last_off_min_position_ + last_off_max_position_) / 2.0;
+ if (claw_position.posedge_value < avg_off_position) {
+ printf("Posedge upper current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->upper_decreasing_angle = claw_position.posedge_value - start_position_;
+ } else {
+ printf("Posedge lower current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->lower_angle =
+ claw_position.posedge_value - start_position_;
+ }
+ print = true;
}
- return true;
- }
- if (hall_effect.negedge_count != last_hall_effect.negedge_count) {
- if (claw_position.negedge_value > last_claw_position.position) {
- limits->upper_decreasing_angle =
- claw_position.negedge_value - start_position;
- } else {
- limits->lower_angle = claw_position.negedge_value - start_position;
+ if (hall_effect.negedge_count != last_negedge_count_) {
+ const double avg_on_position = (last_on_min_position_ + last_on_max_position_) / 2.0;
+ if (claw_position.negedge_value > avg_on_position) {
+ printf("Negedge upper current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->upper_angle =
+ claw_position.negedge_value - start_position_;
+ } else {
+ printf("Negedge lower current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->lower_decreasing_angle = claw_position.negedge_value - start_position_;
+ }
+ print = true;
}
- return true;
+
+ if (hall_effect.current) {
+ if (!last_hall_effect_.current) {
+ last_on_min_position_ = last_on_max_position_ = claw_position.position;
+ } else {
+ last_on_min_position_ =
+ ::std::min(claw_position.position, last_on_min_position_);
+ last_on_max_position_ =
+ ::std::max(claw_position.position, last_on_max_position_);
+ }
+ } else {
+ if (last_hall_effect_.current) {
+ last_off_min_position_ = last_off_max_position_ = claw_position.position;
+ } else {
+ last_off_min_position_ =
+ ::std::min(claw_position.position, last_off_min_position_);
+ last_off_max_position_ =
+ ::std::max(claw_position.position, last_off_max_position_);
+ }
+ }
+
+ last_hall_effect_ = hall_effect;
+ last_posedge_count_ = hall_effect.posedge_count;
+ last_negedge_count_ = hall_effect.negedge_count;
+
+ return print;
}
- return false;
-}
+ private:
+ const double start_position_;
+ HallEffectStruct last_hall_effect_;
+ int32_t last_posedge_count_;
+ int32_t last_negedge_count_;
+ double last_on_min_position_;
+ double last_off_min_position_;
+ double last_on_max_position_;
+ double last_off_max_position_;
+};
-bool GetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position, Claws::Claw *claw) {
+class ClawSensors {
+ public:
+ ClawSensors(const double start_position,
+ const control_loops::HalfClawPosition &initial_claw_position)
+ : start_position_(start_position),
+ front_(start_position, initial_claw_position.front),
+ calibration_(start_position, initial_claw_position.calibration),
+ back_(start_position, initial_claw_position.back) {}
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.front, claw_position.front,
- &claw->front)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.calibration,
- claw_position.calibration, &claw->calibration)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.back, claw_position.back,
- &claw->back)) {
- return true;
+ bool GetPositionOfEdge(const control_loops::HalfClawPosition &claw_position,
+ Claws::Claw *claw) {
+
+ bool print = false;
+ if (front_.DoGetPositionOfEdge(claw_position,
+ claw_position.front, &claw->front)) {
+ print = true;
+ } else if (calibration_.DoGetPositionOfEdge(claw_position,
+ claw_position.calibration,
+ &claw->calibration)) {
+ print = true;
+ } else if (back_.DoGetPositionOfEdge(claw_position,
+ claw_position.back, &claw->back)) {
+ print = true;
+ }
+
+ double position = claw_position.position - start_position_;
+
+ if (position > claw->upper_limit) {
+ claw->upper_hard_limit = claw->upper_limit = position;
+ print = true;
+ }
+ if (position < claw->lower_limit) {
+ claw->lower_hard_limit = claw->lower_limit = position;
+ print = true;
+ }
+ return print;
}
- double position = claw_position.position - start_position;
-
- if (position > claw->upper_limit) {
- claw->upper_hard_limit = claw->upper_limit = position;
- return true;
- }
- if (position < claw->lower_limit) {
- claw->lower_hard_limit = claw->lower_limit = position;
- return true;
- }
- return false;
-}
+ private:
+ const double start_position_;
+ Sensor front_;
+ Sensor calibration_;
+ Sensor back_;
+};
int Main() {
while (!control_loops::claw_queue_group.position.FetchNextBlocking());
@@ -80,6 +155,11 @@
const double bottom_start_position =
control_loops::claw_queue_group.position->bottom.position;
+ ClawSensors top(top_start_position,
+ control_loops::claw_queue_group.position->top);
+ ClawSensors bottom(bottom_start_position,
+ control_loops::claw_queue_group.position->bottom);
+
Claws limits;
limits.claw_zeroing_off_speed = 0.5;
@@ -138,17 +218,16 @@
while (true) {
if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
bool print = false;
- if (GetPositionOfEdge(top_start_position, last_position.top,
- control_loops::claw_queue_group.position->top,
- &limits.upper_claw)) {
+ if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the upper claw\n");
+ printf("Got an edge on the upper claw\n");
}
- if (GetPositionOfEdge(bottom_start_position, last_position.bottom,
- control_loops::claw_queue_group.position->bottom,
- &limits.lower_claw)) {
+ if (bottom.GetPositionOfEdge(
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the lower claw\n");
+ printf("Got an edge on the lower claw\n");
}
const double top_position =
control_loops::claw_queue_group.position->top.position -
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 79d14af..7e6617c 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -7,6 +7,7 @@
namespace frc971 {
+// TODO(brians): Have a Reset() for when the cape resets.
class HallEffectTracker {
public:
int32_t get_posedges() const { return posedges_.count(); }
@@ -26,23 +27,38 @@
negedges_.update(position.negedge_count);
}
+ void Reset() {
+ posedges_.Reset();
+ negedges_.Reset();
+ }
+
+ bool ready() { return posedges_.ready() && negedges_.ready(); }
+
private:
class {
public:
void update(int32_t count) {
+ if (first_) {
+ count_ = count;
+ LOG(DEBUG, "First time through the hall effect, resetting\n");
+ }
previous_count_ = count_;
count_ = count;
+ first_ = false;
}
- bool count_changed() const {
- return previous_count_ != count_;
- }
+ void Reset() { first_ = true; }
+
+ bool count_changed() const { return !first_ && previous_count_ != count_; }
int32_t count() const { return count_; }
+ bool ready() { return !first_; }
+
private:
int32_t count_ = 0;
int32_t previous_count_ = 0;
+ bool first_ = true;
} posedges_, negedges_;
bool value_ = false;
};
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index f0a522a..f059b45 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -122,10 +122,10 @@
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
- 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.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
+ [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 / (40.0 ** 2.0)), 0.0],
[0.0, (1.0 / (5.0 ** 2.0))]])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index c2b9dbb..89f682a 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -20,7 +20,7 @@
# This rough estimate should about include the effect of the masses
# of the gears. If this number is too low, the eigen values of self.A
# will start to become extremely small.
- self.J = 20
+ self.J = 200
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2.0
# Motor velocity constant
@@ -110,7 +110,7 @@
self.C = numpy.matrix([[1.0, 0.0, 0.0]])
self.D = numpy.matrix([[0.0]])
- self.PlaceControllerPoles([0.55, 0.45, 0.80])
+ self.PlaceControllerPoles([0.50, 0.35, 0.80])
print "K"
print self.K
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 8f435fb..4789e1d 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -174,7 +174,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_controller_index(1);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -216,16 +216,24 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->plunger && position->latch) {
- // The plunger is back and we are latched, get ready to shoot.
- state_ = STATE_PREPARE_SHOT;
- latch_piston_ = true;
- } else if (position->pusher_distal.current) {
+ if (position->pusher_distal.current) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
state_ = STATE_LOAD_BACKTRACK;
- latch_piston_ = false;
+
+ // The plunger is already back and latched. Don't release it.
+ if (position->plunger && position->latch) {
+ latch_piston_ = true;
+ } else {
+ latch_piston_ = false;
+ }
+ } else if (position->plunger && position->latch) {
+ // The plunger is back and we are latched. We most likely got here
+ // from Initialize, in which case we want to 'load' again anyways to
+ // zero.
+ Load();
+ latch_piston_ = true;
} else {
// Off the sensor, start loading.
Load();
@@ -248,7 +256,7 @@
values.shooter.zeroing_speed);
}
cap_goal = true;
- shooter_.set_max_voltage(12.0);
+ shooter_.set_max_voltage(4.0);
if (position) {
if (!position->pusher_distal.current) {
@@ -289,17 +297,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current;
+ latch_piston_ = position->pusher_distal.current || position->plunger;
- // Check if we are latched and back.
- if (position->plunger && position->latch) {
+ // Check if we are latched and back. Make sure the plunger is all the
+ // way back as well.
+ if (position->plunger && position->latch &&
+ position->pusher_distal.current) {
state_ = STATE_PREPARE_SHOT;
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = Time::Now() + Time::InSeconds(0.5);
+ loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
}
}
if (load_timeout_ < Time::Now()) {
@@ -354,7 +364,7 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = Time::Now() + Time::InSeconds(0.05);
+ shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
@@ -384,8 +394,7 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
apply_some_voltage = true;
state_ = STATE_PREPARE_FIRE;
}
@@ -408,14 +417,13 @@
shooter_loop_disable = true;
if (disabled) {
// If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ =
- Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+ prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
break;
}
if (Time::Now() > prepare_fire_end_time_) {
cycles_not_moved_ = 0;
firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
state_ = STATE_FIRE;
latch_piston_ = false;
} else {
@@ -432,7 +440,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = Time::Now() + Time::InSeconds(1);
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
}
}
}
@@ -463,7 +471,14 @@
// If it is latched and the plunger is back, move the pusher back to catch
// the plunger.
- if (position->plunger && position->latch) {
+ bool all_back;
+ if (position) {
+ all_back = position->plunger && position->latch;
+ } else {
+ all_back = last_position_.plunger && last_position_.latch;
+ }
+
+ if (all_back) {
// Pull back to 0, 0.
shooter_.SetGoalPosition(0.0, 0.0);
if (shooter_.absolute_position() < 0.005) {
@@ -478,7 +493,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
if (Time::Now() > unload_timeout_) {
@@ -491,11 +506,11 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(6.0);
+ shooter_.set_max_voltage(5.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 322a556..195894b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -107,6 +107,13 @@
bool capped_goal_;
};
+const Time kUnloadTimeout = Time::InSeconds(10);
+const Time kLoadTimeout = Time::InSeconds(10);
+const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+const Time kShotEndTimeout = Time::InSeconds(1.0);
+const Time kPrepareFireEndTime = Time::InMS(40);
+
class ShooterMotor
: public aos::control_loops::ControlLoop<control_loops::ShooterGroup> {
public:
@@ -150,12 +157,12 @@
// Enter state STATE_UNLOAD
void Unload() {
state_ = STATE_UNLOAD;
- unload_timeout_ = Time::Now() + Time::InSeconds(1);
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
}
// Enter state STATE_LOAD
void Load() {
state_ = STATE_LOAD;
- load_timeout_ = Time::Now() + Time::InSeconds(1);
+ load_timeout_ = Time::Now() + kLoadTimeout;
}
control_loops::ShooterGroup::Position last_position_;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 74cb0ec..8aa4c27 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -606,6 +606,9 @@
// TODO(austin): Test all the timeouts...
+// TODO(austin): Test starting latched and with the plunger back.
+// TODO(austin): Verify that we zeroed if we started latched and all the way back.
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 50642b9..546387d 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -9,7 +9,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 0.997145287595, 0.00115072867987, 0.000356210952805, -0.322204030364, -0.000199174994385, 0.0402046120149, 0.0, 0.0, 1.0;
+ A << 0.999391114909, 0.00811316740387, 7.59766686183e-05, -0.113584343654, 0.64780421498, 0.0141730519709, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
B << 0.0, 0.0, 1.0;
Eigen::Matrix<double, 1, 3> C;
@@ -25,7 +25,7 @@
StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00115359397892, 0.000356613321821, 0.0, 0.000172163011452, 0.0403047209622, 0.0, 0.0, 1.0;
+ A << 1.0, 0.00811505488455, 7.59852687598e-05, 0.0, 0.648331305446, 0.0141763492481, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
B << 0.0, 0.0, 1.0;
Eigen::Matrix<double, 1, 3> C;
@@ -41,17 +41,17 @@
StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 0.996946112601, 10.71141318, 224.213599484;
+ L << 1.64719532989, 57.0572680832, 636.74290365;
Eigen::Matrix<double, 1, 3> K;
- K << 121.388812879, 5.06126911425, 0.196946112601;
+ K << 450.571849185, 11.8404918938, 0.997195329889;
return StateFeedbackController<3, 1, 1>(L, K, MakeSprungShooterPlantCoefficients());
}
StateFeedbackController<3, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 3, 1> L;
- L << 1.00017216301, 11.0141047888, 223.935057347;
+ L << 1.64833130545, 57.2417604572, 636.668851906;
Eigen::Matrix<double, 1, 3> K;
- K << 122.81439697, 5.05065025388, 0.200172163011;
+ K << 349.173113146, 8.65077618169, 0.848331305446;
return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index 4a0e6f2..42b166f 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.997145287595, 0.00115072867987, -0.322204030364, -0.000199174994385;
+ A << 0.999391114909, 0.00811316740387, -0.113584343654, 0.64780421498;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356210952805, 0.0402046120149;
+ B << 7.59766686183e-05, 0.0141730519709;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00115359397892, 0.0, 0.000172163011452;
+ A << 1.0, 0.00811505488455, 0.0, 0.648331305446;
Eigen::Matrix<double, 2, 1> B;
- B << 0.000356613321821, 0.0403047209622;
+ B << 7.59852687598e-05, 0.0141763492481;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -41,17 +41,17 @@
StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.896946112601, 1.86767549049;
+ L << 1.54719532989, 43.9345489758;
Eigen::Matrix<double, 1, 2> K;
- K << 743.451871215, -4.17563006819;
+ K << 2126.06977433, 41.3223370936;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawSprungShooterPlantCoefficients());
}
StateFeedbackController<2, 1, 1> MakeRawShooterController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.900172163011, 2.15224193635;
+ L << 1.54833130545, 44.1155797675;
Eigen::Matrix<double, 1, 2> K;
- K << 750.532425926, -4.15528738406;
+ K << 2133.83569145, 41.3499425476;
return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
}
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 1150304..d751a43 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -5,6 +5,7 @@
#include "aos/linux_code/init.h"
#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -29,10 +30,14 @@
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);
+const ButtonLocation kFire(3, 11);
+const ButtonLocation kUnload(3, 12);
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,22 +110,34 @@
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)
- .intake(false).Send()) {
+ .bottom_angle(goal_angle)
+ .separation_angle(separation_angle)
+ .intake(false)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(0)
- .shot_requested(false)
- .unload_requested(true)
- .Send()) {
+ .shot_power(0.25)
+ .shot_requested(data.IsPressed(kFire))
+ .unload_requested(data.IsPressed(kUnload))
+ .Send()) {
LOG(WARNING, "sending shooter goal failed\n");
}
}
}
+
+ private:
+ bool closed_;
};
} // namespace joysticks
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index ef23d21..e17d6bc 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -37,7 +37,7 @@
if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(8, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
SetSolenoid(7, drivetrain.output->left_high);
SetSolenoid(8, drivetrain.output->right_high);
} else {
@@ -53,7 +53,7 @@
shooter.FetchLatest();
if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *shooter.get());
- SetPWMOutput(9, shooter->voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, shooter->voltage / 12.0, kTalonBounds);
SetSolenoid(6, !shooter->latch_piston);
SetSolenoid(5, !shooter->brake_piston);
} else {
@@ -68,8 +68,8 @@
claw.FetchLatest();
if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *claw.get());
- SetPWMOutput(6, claw->intake_voltage / 12.0, kTalonBounds);
- SetPWMOutput(7, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(9, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(8, claw->intake_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