Fixed the reset for min max edge value. Not compiled as the build is broken.
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 032d5fc..22712f0 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -72,7 +72,7 @@
if (control_loop_->position.FetchLatest()) {
position = control_loop_->position.get();
} else {
- if (control_loop_->position.get()) {
+ if (control_loop_->position.get() && !reset_) {
int msec_age = control_loop_->position.Age().ToMSec();
if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
LOG_INTERVAL(very_stale_position_);
@@ -126,7 +126,7 @@
output.Send();
} else {
// The outputs are disabled, so pass NULL in for the output.
- RunIteration(goal, position, NULL, status.get());
+ RunIteration(goal, position, nullptr, status.get());
ZeroOutputs();
}
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index 9dde589..f1109e6 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -84,7 +84,9 @@
void ZeroOutputs();
// Returns true if the device reading the sensors reset and potentially lost
- // track of encoder counts. Calling this read method clears the flag.
+ // track of encoder counts. Calling this read method clears the flag. After
+ // a reset, RunIteration will not be called until there is a valid position
+ // message.
bool reset() {
bool ans = reset_;
reset_ = false;
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 410d6c6..19e9a18 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -272,6 +272,12 @@
if (reset()) {
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ // close up the min and max edge positions as they are no longer valid and
+ // will be expanded in future iterations
+ top_claw_.min_current_hall_effect_edge_ =
+ top_claw_.max_current_hall_effect_edge_ = position->top.position;
+ bottom_claw_.min_current_hall_effect_edge_ =
+ bottom_claw_.max_current_hall_effect_edge_ = position->bottom.position;
}
if (::aos::robot_state.get() == nullptr) {
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 949b0f5..50076a7 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -84,8 +84,10 @@
back_.Update(claw.back);
if (any_hall_effect_changed()) {
- min_current_hall_edge_ = claw.position;
- max_current_hall_edge_ = claw.position;
+ // if the hall effect has changed we are new edge
+ // so we zero out the interval this new edge has covered
+ min_current_hall_effect_edge_ = claw.position;
+ max_current_hall_effect_edge_ = claw.position;
} else if (claw.position > max_current_hall_effect_edge_) {
max_current_hall_effect_edge_ = claw.position;
} else if (claw.position < min_current_hall_effect_edge_) {
@@ -137,6 +139,9 @@
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
+ double last_edge_value_;
+ double min_current_hall_effect_edge_;
+ double max_current_hall_effect_edge_;
double encoder_;
double last_encoder_;