Merge remote-tracking branch 'ben/shooter' into ben_shooter
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 5877226..e82710b 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -134,9 +134,9 @@
void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset();
- calibration_.Reset();
- back_.Reset();
+ front_.Reset(claw.front);
+ calibration_.Reset(claw.calibration);
+ back_.Reset(claw.back);
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
min_hall_effect_on_angle_ = claw.position;
@@ -190,17 +190,54 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
+ this_sensor.value() && !this_sensor.last_value()) {
+ posedge_filter_ = &this_sensor;
+ } else if (posedge_filter_ == &this_sensor &&
+ !this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() &&
+ !sensorB.posedge_count_changed() && this_sensor.value()) {
+ posedge_filter_ = nullptr;
+ return true;
+ } else if (posedge_filter_ == &this_sensor) {
+ posedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
+ !this_sensor.value() && this_sensor.last_value()) {
+ negedge_filter_ = &this_sensor;
+ } else if (negedge_filter_ == &this_sensor &&
+ !this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() &&
+ !sensorB.negedge_count_changed() && !this_sensor.value()) {
+ negedge_filter_ = nullptr;
+ return true;
+ } else if (negedge_filter_ == &this_sensor) {
+ negedge_filter_ = nullptr;
+ }
+ return false;
+}
+
bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
- double *edge_angle, const HallEffectTracker &sensor,
+ double *edge_angle, const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
const char *hall_effect_name) {
bool found_edge = false;
- if (sensor.posedge_count_changed()) {
+ if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- // we oddly got two of the same edge.
- *edge_angle = last_edge_value_;
- found_edge = true;
+ LOG(INFO, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -215,14 +252,14 @@
name_, hall_effect_name, *edge_angle, posedge_value_,
average_last_encoder);
}
- }
- *edge_encoder = posedge_value_;
- found_edge = true;
- }
- if (sensor.negedge_count_changed()) {
- if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- *edge_angle = last_edge_value_;
+ *edge_encoder = posedge_value_;
found_edge = true;
+ }
+ }
+
+ if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ LOG(INFO, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -238,12 +275,8 @@
average_last_encoder);
}
*edge_encoder = negedge_value_;
+ found_edge = true;
}
- found_edge = true;
- }
-
- if (found_edge) {
- last_edge_value_ = *edge_angle;
}
return found_edge;
@@ -252,23 +285,16 @@
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
- // TODO(austin): Validate that the hall effect edge makes sense.
- // We must now be on the side of the edge that we expect to be, and the
- // encoder must have been on either side of the edge before and after.
-
- // TODO(austin): Compute the last off range min and max and compare the edge
- // value to the middle of the range. This will be quite a bit more reliable.
-
if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
- front_, "front")) {
+ front_, calibration_, back_, "front")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
- calibration_, "calibration")) {
+ calibration_, front_, back_, "calibration")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
- back_, "back")) {
+ back_, calibration_, front_, "back")) {
return true;
}
return false;
@@ -322,14 +348,12 @@
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.claw_max_separation) {
- LOG(DEBUG, "Greater than\n");
const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
}
if (separation < values.claw.claw_min_separation) {
- LOG(DEBUG, "Less than\n");
const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
@@ -491,23 +515,23 @@
mode_ = PREP_FINE_TUNE_BOTTOM;
}
- if (bottom_claw_.calibration().value()) {
- if (bottom_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
- values.claw.lower_claw.calibration.lower_angle);
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning bottom
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the bottom correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_BOTTOM;
- }
+ if (position &&
+ bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(),
+ bottom_claw_.front(), bottom_claw_.back())) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.claw.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else if (bottom_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_BOTTOM;
} else {
LOG(DEBUG, "Fine tuning\n");
}
@@ -547,24 +571,24 @@
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
- if (top_claw_.calibration().value()) {
- if (top_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- top_claw_.SetCalibration(
- position->top.posedge_value,
- values.claw.upper_claw.calibration.lower_angle);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning top
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the top correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_TOP;
- }
- }
+
+ if (position &&
+ top_claw_.SawFilteredPosedge(top_claw_.calibration(),
+ top_claw_.front(), top_claw_.back())) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ } else if (top_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_TOP;
+ }
}
// now set the bottom claw to track
bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
@@ -608,18 +632,26 @@
}
}
- if (enabled) {
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- } else {
- // TODO(austin): Only calibrate on the predetermined edge.
- // We might be able to just ignore this since the backlash is soooo low. :)
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ if (position) {
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo
+ // low.
+ // :)
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
}
mode_ = UNKNOWN_LOCATION;
}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index dc8e6ba..8a4678e 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -71,10 +71,6 @@
void Reset(const HalfClawPosition &claw);
- bool ready() {
- return front_.ready() && calibration_.ready() && back_.ready();
- }
-
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -103,6 +99,14 @@
bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
double *edge_encoder, double *edge_angle);
+ bool SawFilteredPosedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+ bool SawFilteredNegedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
#undef COUNT_SETTER_GETTER
protected:
@@ -117,7 +121,6 @@
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
- double last_edge_value_;
double min_hall_effect_on_angle_;
double max_hall_effect_on_angle_;
double min_hall_effect_off_angle_;
@@ -128,11 +131,16 @@
double last_off_encoder_;
bool any_triggered_last_;
+ const HallEffectTracker* posedge_filter_ = nullptr;
+ const HallEffectTracker* negedge_filter_ = nullptr;
+
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
bool DoGetPositionOfEdge(const constants::Values::Claws::AnglePair &angles,
double *edge_encoder, double *edge_angle,
const HallEffectTracker &sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB,
const char *hall_effect_name);
};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 99194bd..47f5bef 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -227,6 +227,7 @@
v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -279,6 +280,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
void SendDSPacket(bool enabled) {
@@ -305,6 +307,7 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
::bbb::sensor_generation.Clear();
+ ::aos::time::Time::DisableMockTime();
}
};
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 7e6617c..b63a34b 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -20,47 +20,42 @@
bool negedge_count_changed() const { return negedges_.count_changed(); }
bool value() const { return value_; }
+ bool last_value() const { return last_value_; }
void Update(const HallEffectStruct &position) {
+ last_value_ = value_;
value_ = position.current;
posedges_.update(position.posedge_count);
negedges_.update(position.negedge_count);
}
- void Reset() {
- posedges_.Reset();
- negedges_.Reset();
+ void Reset(const HallEffectStruct &position) {
+ posedges_.Reset(position.posedge_count);
+ negedges_.Reset(position.negedge_count);
+ value_ = position.current;
+ last_value_ = position.current;
}
- 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;
}
- void Reset() { first_ = true; }
+ void Reset(int32_t count) { count_ = count; }
- bool count_changed() const { return !first_ && previous_count_ != count_; }
+ bool count_changed() const { return 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;
+ bool last_value_ = false;
};
} // namespace frc971