Sanify the format posedge+negedge values are passed in.
Previously, everything which dealt with them either had to combine the
three separate sources or split them apart, which made no sense.
Change-Id: Ibcd9dc48f83ab7372fe46dc5d0a6483b281a08f0
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 3dacff8..37a6420 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -42,6 +42,8 @@
bool current;
int32_t posedge_count;
int32_t negedge_count;
+ double posedge_value;
+ double negedge_value;
};
// Records the positions for a mechanism with edge-capturing sensors on it.
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 4e3981e..d1b6efe 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -7,7 +7,6 @@
namespace frc971 {
-// TODO(brians): Have a Reset() for when the cape resets.
// TODO(aschuh): Can we filter for 2 cycles instead of just 1?
class HallEffectTracker {
public:
@@ -22,10 +21,17 @@
bool value() const { return value_; }
bool last_value() const { return last_value_; }
+ bool is_posedge() const { return value() && !last_value(); }
+ bool is_negedge() const { return !value() && last_value(); }
+
+ double posedge_value() const { return posedge_value_; }
+ double negedge_value() const { return negedge_value_; }
void Update(const HallEffectStruct &position) {
last_value_ = value_;
value_ = position.current;
+ posedge_value_ = position.posedge_value;
+ negedge_value_ = position.negedge_value;
posedges_.update(position.posedge_count);
negedges_.update(position.negedge_count);
}
@@ -35,6 +41,8 @@
negedges_.Reset(position.negedge_count);
value_ = position.current;
last_value_ = position.current;
+ posedge_value_ = position.posedge_value;
+ negedge_value_ = position.negedge_value;
}
private:
@@ -55,8 +63,11 @@
int32_t count_ = 0;
int32_t previous_count_ = 0;
} posedges_, negedges_;
+
bool value_ = false;
bool last_value_ = false;
+
+ double posedge_value_ = 0, negedge_value_ = 0;
};
} // namespace frc971
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 4f9fe2b..00c4341 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -211,8 +211,6 @@
name_(name),
motor_(motor),
zeroing_state_(UNKNOWN_POSITION),
- posedge_value_(0.0),
- negedge_value_(0.0),
encoder_(0.0),
last_encoder_(0.0) {}
@@ -234,18 +232,52 @@
::std::min(min_hall_effect_off_angle_, claw.position);
max_hall_effect_off_angle_ =
::std::max(max_hall_effect_off_angle_, claw.position);
- } else if (any_sensor_triggered && !any_triggered_last_) {
- // Saw a posedge on the hall effect. Reset the limits.
- min_hall_effect_on_angle_ = ::std::min(claw.posedge_value, claw.position);
- max_hall_effect_on_angle_ = ::std::max(claw.posedge_value, claw.position);
- } else if (!any_sensor_triggered && any_triggered_last_) {
- // Saw a negedge on the hall effect. Reset the limits.
- min_hall_effect_off_angle_ = ::std::min(claw.negedge_value, claw.position);
- max_hall_effect_off_angle_ = ::std::max(claw.negedge_value, claw.position);
}
- posedge_value_ = claw.posedge_value;
- negedge_value_ = claw.negedge_value;
+ if (front_.is_posedge()) {
+ // Saw a posedge on the hall effect. Reset the limits.
+ min_hall_effect_on_angle_ =
+ ::std::min(claw.front.posedge_value, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(claw.front.posedge_value, claw.position);
+ }
+ if (calibration_.is_posedge()) {
+ // Saw a posedge on the hall effect. Reset the limits.
+ min_hall_effect_on_angle_ =
+ ::std::min(claw.calibration.posedge_value, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(claw.calibration.posedge_value, claw.position);
+ }
+ if (back_.is_posedge()) {
+ // Saw a posedge on the hall effect. Reset the limits.
+ min_hall_effect_on_angle_ =
+ ::std::min(claw.back.posedge_value, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(claw.back.posedge_value, claw.position);
+ }
+
+ if (front_.is_negedge()) {
+ // Saw a negedge on the hall effect. Reset the limits.
+ min_hall_effect_off_angle_ =
+ ::std::min(claw.front.negedge_value, claw.position);
+ max_hall_effect_off_angle_ =
+ ::std::max(claw.front.negedge_value, claw.position);
+ }
+ if (calibration_.is_negedge()) {
+ // Saw a negedge on the hall effect. Reset the limits.
+ min_hall_effect_off_angle_ =
+ ::std::min(claw.calibration.negedge_value, claw.position);
+ max_hall_effect_off_angle_ =
+ ::std::max(claw.calibration.negedge_value, claw.position);
+ }
+ if (back_.is_negedge()) {
+ // Saw a negedge on the hall effect. Reset the limits.
+ min_hall_effect_off_angle_ =
+ ::std::min(claw.back.negedge_value, claw.position);
+ max_hall_effect_off_angle_ =
+ ::std::max(claw.back.negedge_value, claw.position);
+ }
+
last_encoder_ = encoder_;
if (front().value() || calibration().value() || back().value()) {
last_on_encoder_ = encoder_;
@@ -399,18 +431,18 @@
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
- if (posedge_value_ < average_last_encoder) {
+ if (this_sensor.posedge_value() < average_last_encoder) {
*edge_angle = angles.upper_decreasing_angle;
LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, posedge_value_,
+ name_, hall_effect_name, *edge_angle, this_sensor.posedge_value(),
average_last_encoder);
} else {
*edge_angle = angles.lower_angle;
LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, posedge_value_,
+ name_, hall_effect_name, *edge_angle, this_sensor.posedge_value(),
average_last_encoder);
}
- *edge_encoder = posedge_value_;
+ *edge_encoder = this_sensor.posedge_value();
found_edge = true;
}
}
@@ -421,18 +453,18 @@
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
- if (negedge_value_ > average_last_encoder) {
+ if (this_sensor.negedge_value() > average_last_encoder) {
*edge_angle = angles.upper_angle;
LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, negedge_value_,
+ name_, hall_effect_name, *edge_angle, this_sensor.negedge_value(),
average_last_encoder);
} else {
*edge_angle = angles.lower_decreasing_angle;
LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, negedge_value_,
+ name_, hall_effect_name, *edge_angle, this_sensor.negedge_value(),
average_last_encoder);
}
- *edge_encoder = negedge_value_;
+ *edge_encoder = this_sensor.negedge_value();
found_edge = true;
}
}
@@ -716,7 +748,7 @@
bottom_claw_.back())) {
// do calibration
bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
+ position->bottom.calibration.posedge_value,
values.claw.lower_claw.calibration.lower_angle);
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning bottom
@@ -772,7 +804,7 @@
top_claw_.front(), top_claw_.back())) {
// do calibration
top_claw_.SetCalibration(
- position->top.posedge_value,
+ position->top.calibration.posedge_value,
values.claw.upper_claw.calibration.lower_angle);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning top
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index c04a8fe..86b9b6d 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -129,8 +129,6 @@
HallEffectTracker front_, calibration_, back_;
JointZeroingState zeroing_state_;
- double posedge_value_;
- double negedge_value_;
double min_hall_effect_on_angle_;
double max_hall_effect_on_angle_;
double min_hall_effect_off_angle_;
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
index 860854b..6c7dada 100644
--- a/y2014/control_loops/claw/claw.q
+++ b/y2014/control_loops/claw/claw.q
@@ -13,13 +13,6 @@
HallEffectStruct calibration;
// The hall effect at the back limit.
HallEffectStruct back;
-
- // The encoder value at the last posedge of any of the claw hall effect
- // sensors (front, calibration, or back).
- double posedge_value;
- // The encoder value at the last negedge of any of the claw hall effect
- // sensors (front, calibration, or back).
- double negedge_value;
};
// All angles here are 0 vertical, positive "up" (aka backwards).
diff --git a/y2014/control_loops/claw/claw_calibration.cc b/y2014/control_loops/claw/claw_calibration.cc
index de91d91..8d806d7 100644
--- a/y2014/control_loops/claw/claw_calibration.cc
+++ b/y2014/control_loops/claw/claw_calibration.cc
@@ -28,38 +28,42 @@
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) {
+ const double avg_off_position =
+ (last_off_min_position_ + last_off_max_position_) / 2.0;
+ if (hall_effect.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,
+ claw_position.position, hall_effect.posedge_value,
avg_off_position, last_off_min_position_,
last_off_max_position_);
- limits->upper_decreasing_angle = claw_position.posedge_value - start_position_;
+ limits->upper_decreasing_angle =
+ hall_effect.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,
+ claw_position.position, hall_effect.posedge_value,
avg_off_position, last_off_min_position_,
last_off_max_position_);
limits->lower_angle =
- claw_position.posedge_value - start_position_;
+ hall_effect.posedge_value - start_position_;
}
print = true;
}
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) {
+ const double avg_on_position =
+ (last_on_min_position_ + last_on_max_position_) / 2.0;
+ if (hall_effect.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,
+ claw_position.position, hall_effect.negedge_value,
avg_on_position, last_on_min_position_,
last_on_max_position_);
limits->upper_angle =
- claw_position.negedge_value - start_position_;
+ hall_effect.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,
+ claw_position.position, hall_effect.negedge_value,
avg_on_position, last_on_min_position_,
last_on_max_position_);
- limits->lower_decreasing_angle = claw_position.negedge_value - start_position_;
+ limits->lower_decreasing_angle =
+ hall_effect.negedge_value - start_position_;
}
print = true;
}
@@ -75,7 +79,8 @@
}
} else {
if (last_hall_effect_.current) {
- last_off_min_position_ = last_off_max_position_ = claw_position.position;
+ 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_);
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index b98a1bb..2137316 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -127,8 +127,6 @@
void UpdateHallEffect(double angle,
double last_angle,
double initial_position,
- double *posedge_value,
- double *negedge_value,
HallEffectStruct *position,
const HallEffectStruct &last_position,
const constants::Values::Claws::AnglePair &pair,
@@ -139,11 +137,11 @@
if (last_angle < pair.lower_angle) {
LOG(DEBUG, "%s: Positive lower edge on %s hall effect\n", claw_name,
hall_effect_name);
- *posedge_value = pair.lower_angle - initial_position;
+ position->posedge_value = pair.lower_angle - initial_position;
} else {
LOG(DEBUG, "%s: Positive upper edge on %s hall effect\n", claw_name,
hall_effect_name);
- *posedge_value = pair.upper_angle - initial_position;
+ position->posedge_value = pair.upper_angle - initial_position;
}
}
if (!position->current && last_position.current) {
@@ -152,11 +150,11 @@
if (angle < pair.lower_angle) {
LOG(DEBUG, "%s: Negative lower edge on %s hall effect\n", claw_name,
hall_effect_name);
- *negedge_value = pair.lower_angle - initial_position;
+ position->negedge_value = pair.lower_angle - initial_position;
} else {
LOG(DEBUG, "%s: Negative upper edge on %s hall effect\n", claw_name,
hall_effect_name);
- *negedge_value = pair.upper_angle - initial_position;
+ position->negedge_value = pair.upper_angle - initial_position;
}
}
}
@@ -168,20 +166,17 @@
const char *claw_name) {
UpdateHallEffect(position->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->posedge_value,
- &position->negedge_value, &position->front,
- last_position.front, claw.front, claw_name, "front");
+ initial_position, &position->front, last_position.front,
+ claw.front, claw_name, "front");
UpdateHallEffect(position->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->posedge_value,
- &position->negedge_value, &position->calibration,
+ initial_position, &position->calibration,
last_position.calibration, claw.calibration, claw_name,
"calibration");
UpdateHallEffect(position->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->posedge_value,
- &position->negedge_value, &position->back,
- last_position.back, claw.back, claw_name, "back");
+ initial_position, &position->back, last_position.back,
+ claw.back, claw_name, "back");
}
// Sends out the position queue messages.