converted the claw to separate structures for each hall effect
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index cfe61a0..abff616 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -87,10 +87,43 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
+ const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
+ double *edge_angle, const HallEffectTracker &sensor,
+ const char *hall_effect_name) {
+ if (sensor.posedge_count_changed()) {
+ if (posedge_value_ < last_encoder()) {
+ *edge_angle = angles.upper_angle;
+ LOG(INFO, "%s Posedge upper of %s -> %f\n", name_,
+ hall_effect_name, *edge_angle);
+ } else {
+ *edge_angle = angles.lower_angle;
+ LOG(INFO, "%s Posedge lower of %s -> %f\n", name_,
+ hall_effect_name, *edge_angle);
+ }
+ *edge_encoder = posedge_value_;
+ return true;
+ }
+ if (sensor.negedge_count_changed()) {
+ if (negedge_value_ > last_encoder()) {
+ *edge_angle = angles.upper_angle;
+ LOG(INFO, "%s Negedge upper of %s -> %f\n", name_,
+ hall_effect_name, *edge_angle);
+ } else {
+ *edge_angle = angles.lower_angle;
+ LOG(INFO, "%s Negedge lower of %s -> %f\n", name_,
+ hall_effect_name, *edge_angle);
+ }
+ *edge_encoder = negedge_value_;
+ return true;
+ }
+
+ return false;
+}
+
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.
@@ -98,72 +131,16 @@
// 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 (front_hall_effect_posedge_count_changed()) {
- if (posedge_value_ - last_encoder() < 0) {
- *edge_angle = claw_values.front.upper_angle;
- LOG(INFO, "%s Posedge front upper edge -> %f\n", name_, *edge_angle);
- } else {
- *edge_angle = claw_values.front.lower_angle;
- LOG(INFO, "%s Posedge front lower edge -> %f\n", name_, *edge_angle);
- }
- *edge_encoder = posedge_value_;
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
+ front_, "front")) {
return true;
}
- if (front_hall_effect_negedge_count_changed()) {
- if (negedge_value_ - last_encoder() > 0) {
- *edge_angle = claw_values.front.upper_angle;
- LOG(INFO, "%s Negedge front upper edge -> %f\n", name_, *edge_angle);
- } else {
- *edge_angle = claw_values.front.lower_angle;
- LOG(INFO, "%s Negedge front lower edge -> %f\n", name_, *edge_angle);
- }
- *edge_encoder = negedge_value_;
+ if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
+ calibration_, "calibration")) {
return true;
}
- if (calibration_hall_effect_posedge_count_changed()) {
- if (posedge_value_ - last_encoder() < 0) {
- *edge_angle = claw_values.calibration.upper_angle;
- LOG(INFO, "%s Posedge calibration upper edge -> %f\n", name_,
- *edge_angle);
- } else {
- *edge_angle = claw_values.calibration.lower_angle;
- LOG(INFO, "%s Posedge calibration lower edge -> %f\n", name_,
- *edge_angle);
- }
- *edge_encoder = posedge_value_;
- return true;
- }
- if (calibration_hall_effect_negedge_count_changed()) {
- if (negedge_value_ - last_encoder() > 0) {
- *edge_angle = claw_values.calibration.upper_angle;
- LOG(INFO, "%s Negedge calibration upper edge -> %f\n", name_, *edge_angle);
- } else {
- *edge_angle = claw_values.calibration.lower_angle;
- LOG(INFO, "%s Negedge calibration lower edge -> %f\n", name_, *edge_angle);
- }
- *edge_encoder = negedge_value_;
- return true;
- }
- if (back_hall_effect_posedge_count_changed()) {
- if (posedge_value_ - last_encoder() < 0) {
- *edge_angle = claw_values.back.upper_angle;
- LOG(INFO, "%s Posedge back upper edge -> %f\n", name_, *edge_angle);
- } else {
- *edge_angle = claw_values.back.lower_angle;
- LOG(INFO, "%s Posedge back lower edge -> %f\n", name_, *edge_angle);
- }
- *edge_encoder = posedge_value_;
- return true;
- }
- if (back_hall_effect_negedge_count_changed()) {
- if (negedge_value_ - last_encoder() > 0) {
- *edge_angle = claw_values.back.upper_angle;
- LOG(INFO, "%s Negedge back upper edge -> %f\n", name_, *edge_angle);
- } else {
- *edge_angle = claw_values.back.lower_angle;
- LOG(INFO, "%s Negedge back lower edge -> %f\n", name_, *edge_angle);
- }
- *edge_encoder = negedge_value_;
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
+ back_, "back")) {
return true;
}
return false;
@@ -314,9 +291,8 @@
}
} else {
bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
- if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
- bottom_claw_.front_hall_effect() ||
- bottom_claw_.back_hall_effect()) {
+ if (top_claw_.front_or_back_triggered() ||
+ bottom_claw_.front_or_back_triggered()) {
// We shouldn't hit a limit, but if we do, go back to the zeroing
// point and try again.
doing_calibration_fine_tune_ = false;
@@ -324,8 +300,8 @@
LOG(DEBUG, "Found a limit, starting over.\n");
}
- if (bottom_claw_.calibration_hall_effect()) {
- if (bottom_claw_.calibration_hall_effect_posedge_count_changed() &&
+ if (bottom_claw_.calibration().value()) {
+ if (bottom_claw_.calibration().posedge_count_changed() &&
position) {
// do calibration
bottom_claw_.SetCalibration(
@@ -362,16 +338,15 @@
}
} else {
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
- if (top_claw_.front_hall_effect() || top_claw_.back_hall_effect() ||
- bottom_claw_.front_hall_effect() ||
- bottom_claw_.back_hall_effect()) {
+ if (top_claw_.front_or_back_triggered() ||
+ bottom_claw_.front_or_back_triggered()) {
// this should not happen, but now we know it won't
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
LOG(DEBUG, "Found a limit, starting over.\n");
}
- if (top_claw_.calibration_hall_effect()) {
- if (top_claw_.calibration_hall_effect_posedge_count_changed() &&
+ if (top_claw_.calibration().value()) {
+ if (top_claw_.calibration().posedge_count_changed() &&
position) {
// do calibration
top_claw_.SetCalibration(
@@ -409,8 +384,8 @@
if ((bottom_claw_.zeroing_state() !=
ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
- bottom_claw_.front_hall_effect() || top_claw_.front_hall_effect()) &&
- !top_claw_.back_hall_effect() && !bottom_claw_.back_hall_effect()) {
+ bottom_claw_.front().value() || top_claw_.front().value()) &&
+ !top_claw_.back().value() && !bottom_claw_.back().value()) {
if (enabled) {
// Time to slowly move back up to find any position to narrow down the
// zero.
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 5741896..52712d5 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -9,11 +9,11 @@
},
'dependencies': [
'<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
'includes': ['../../../aos/build/queues.gypi'],
},
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 20b30b6..9ff406e 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -8,6 +8,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
+#include "frc971/control_loops/hall_effect_tracker.h"
namespace frc971 {
namespace control_loops {
@@ -53,21 +54,6 @@
: offset_(0.0),
name_(name),
motor_(motor),
- front_hall_effect_posedge_count_(0.0),
- previous_front_hall_effect_posedge_count_(0.0),
- front_hall_effect_negedge_count_(0.0),
- previous_front_hall_effect_negedge_count_(0.0),
- calibration_hall_effect_posedge_count_(0.0),
- previous_calibration_hall_effect_posedge_count_(0.0),
- calibration_hall_effect_negedge_count_(0.0),
- previous_calibration_hall_effect_negedge_count_(0.0),
- back_hall_effect_posedge_count_(0.0),
- previous_back_hall_effect_posedge_count_(0.0),
- back_hall_effect_negedge_count_(0.0),
- previous_back_hall_effect_negedge_count_(0.0),
- front_hall_effect_(false),
- calibration_hall_effect_(false),
- back_hall_effect_(false),
zeroing_state_(UNKNOWN_POSITION),
posedge_value_(0.0),
negedge_value_(0.0),
@@ -94,56 +80,29 @@
JointZeroingState zeroing_state() const { return zeroing_state_; }
void SetPositionValues(const HalfClawPosition &claw) {
- set_front_hall_effect_posedge_count(claw.front_hall_effect_posedge_count);
- set_front_hall_effect_negedge_count(claw.front_hall_effect_negedge_count);
- set_calibration_hall_effect_posedge_count(
- claw.calibration_hall_effect_posedge_count);
- set_calibration_hall_effect_negedge_count(
- claw.calibration_hall_effect_negedge_count);
- set_back_hall_effect_posedge_count(claw.back_hall_effect_posedge_count);
- set_back_hall_effect_negedge_count(claw.back_hall_effect_negedge_count);
+ front_.Update(claw.front);
+ calibration_.Update(claw.calibration);
+ back_.Update(claw.back);
posedge_value_ = claw.posedge_value;
negedge_value_ = claw.negedge_value;
last_encoder_ = encoder_;
encoder_ = claw.position;
-
- front_hall_effect_ = claw.front_hall_effect;
- calibration_hall_effect_ = claw.calibration_hall_effect;
- back_hall_effect_ = claw.back_hall_effect;
}
double absolute_position() const { return encoder() + offset(); }
- bool front_hall_effect() const { return front_hall_effect_; }
- bool calibration_hall_effect() const { return calibration_hall_effect_; }
- bool back_hall_effect() const { return back_hall_effect_; }
-
-#define COUNT_SETTER_GETTER(variable) \
- void set_##variable(int32_t value) { \
- previous_##variable##_ = variable##_; \
- variable##_ = value; \
- } \
- int32_t variable() const { return variable##_; } \
- bool variable##_changed() const { \
- return previous_##variable##_ != variable##_; \
- }
-
- // TODO(austin): Pull all this out of the new sub structure.
- COUNT_SETTER_GETTER(front_hall_effect_posedge_count);
- COUNT_SETTER_GETTER(front_hall_effect_negedge_count);
- COUNT_SETTER_GETTER(calibration_hall_effect_posedge_count);
- COUNT_SETTER_GETTER(calibration_hall_effect_negedge_count);
- COUNT_SETTER_GETTER(back_hall_effect_posedge_count);
- COUNT_SETTER_GETTER(back_hall_effect_negedge_count);
+ const HallEffectTracker &front() const { return front_; }
+ const HallEffectTracker &calibration() const { return calibration_; }
+ const HallEffectTracker &back() const { return back_; }
bool any_hall_effect_changed() const {
- return front_hall_effect_posedge_count_changed() ||
- front_hall_effect_negedge_count_changed() ||
- calibration_hall_effect_posedge_count_changed() ||
- calibration_hall_effect_negedge_count_changed() ||
- back_hall_effect_posedge_count_changed() ||
- back_hall_effect_negedge_count_changed();
+ return front().either_count_changed() ||
+ calibration().either_count_changed() ||
+ back().either_count_changed();
+ }
+ bool front_or_back_triggered() const {
+ return front().value() || back().value();
}
double encoder() const { return encoder_; }
@@ -165,27 +124,20 @@
ClawMotor *motor_;
- int32_t front_hall_effect_posedge_count_;
- int32_t previous_front_hall_effect_posedge_count_;
- int32_t front_hall_effect_negedge_count_;
- int32_t previous_front_hall_effect_negedge_count_;
- int32_t calibration_hall_effect_posedge_count_;
- int32_t previous_calibration_hall_effect_posedge_count_;
- int32_t calibration_hall_effect_negedge_count_;
- int32_t previous_calibration_hall_effect_negedge_count_;
- int32_t back_hall_effect_posedge_count_;
- int32_t previous_back_hall_effect_posedge_count_;
- int32_t back_hall_effect_negedge_count_;
- int32_t previous_back_hall_effect_negedge_count_;
- bool front_hall_effect_;
- bool calibration_hall_effect_;
- bool back_hall_effect_;
+ HallEffectTracker front_, calibration_, back_;
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
double encoder_;
double last_encoder_;
+
+ 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 char *hall_effect_name);
};
class TopZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 66e3036..f78dee6 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -1,28 +1,18 @@
package frc971.control_loops;
import "aos/common/control_loop/control_loops.q";
+import "frc971/control_loops/control_loops.q";
struct HalfClawPosition {
// The current position of this half of the claw.
double position;
- // The value of the front hall effect sensor.
- bool front_hall_effect;
- // The number of positive and negative edges that have been captured on the
- // front hall effect sensor.
- int32_t front_hall_effect_posedge_count;
- int32_t front_hall_effect_negedge_count;
- // The value of the calibration hall effect sensor.
- bool calibration_hall_effect;
- // The number of positive and negative edges that have been captured on the
- // calibration hall effect sensor.
- int32_t calibration_hall_effect_posedge_count;
- int32_t calibration_hall_effect_negedge_count;
- // The value of the back hall effect sensor.
- bool back_hall_effect;
- // The number of positive and negative edges that have been captured on the
- // back hall effect sensor.
- int32_t back_hall_effect_posedge_count;
- int32_t back_hall_effect_negedge_count;
+
+ // The hall effect sensor at the front limit.
+ HallEffectStruct front;
+ // The hall effect sensor in the middle to use for real calibration.
+ 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).
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 0d4a7be..e903e79 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -83,10 +83,24 @@
}
// Makes sure pos is inside range (inclusive)
- bool CheckRange(double pos, struct constants::Values::Claws::AnglePair pair) {
+ bool CheckRange(double pos, const constants::Values::Claws::AnglePair &pair) {
return (pos >= pair.lower_angle && pos <= pair.upper_angle);
}
+ void SetHallEffect(double pos,
+ const constants::Values::Claws::AnglePair &pair,
+ HallEffectStruct *hall_effect) {
+ hall_effect->current = CheckRange(pos, pair);
+ }
+
+ void SetClawHallEffects(double pos,
+ const constants::Values::Claws::Claw &claw,
+ control_loops::HalfClawPosition *half_claw) {
+ SetHallEffect(pos, claw.front, &half_claw->front);
+ SetHallEffect(pos, claw.calibration, &half_claw->calibration);
+ SetHallEffect(pos, claw.back, &half_claw->back);
+ }
+
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ClawGroup::Position *position) {
@@ -95,25 +109,76 @@
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
- LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n", pos[TOP_CLAW], pos[BOTTOM_CLAW]);
+ LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n", pos[TOP_CLAW],
+ pos[BOTTOM_CLAW]);
const frc971::constants::Values& values = constants::GetValues();
- // Signal that the hall effect sensor has been triggered if it is within
+ // Signal that each hall effect sensor has been triggered if it is within
// the correct range.
- position->top.front_hall_effect =
- CheckRange(pos[TOP_CLAW], values.claw.upper_claw.front);
- position->top.calibration_hall_effect =
- CheckRange(pos[TOP_CLAW], values.claw.upper_claw.calibration);
- position->top.back_hall_effect =
- CheckRange(pos[TOP_CLAW], values.claw.upper_claw.back);
+ SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw, &position->top);
+ SetClawHallEffects(pos[BOTTOM_CLAW], values.claw.lower_claw,
+ &position->bottom);
+ }
- position->bottom.front_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.front);
- position->bottom.calibration_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.calibration);
- position->bottom.back_hall_effect =
- CheckRange(pos[BOTTOM_CLAW], values.claw.lower_claw.back);
+ 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,
+ const char *claw_name, const char *hall_effect_name) {
+ if (position->current && !last_position.current) {
+ ++position->posedge_count;
+
+ 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;
+ } else {
+ LOG(DEBUG, "%s: Positive upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *posedge_value = pair.upper_angle - initial_position;
+ }
+ }
+ if (!position->current && last_position.current) {
+ ++position->negedge_count;
+
+ 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;
+ } else {
+ LOG(DEBUG, "%s: Negative upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
+ *negedge_value = pair.upper_angle - initial_position;
+ }
+ }
+ }
+
+ void UpdateClawHallEffects(
+ control_loops::HalfClawPosition *position,
+ const control_loops::HalfClawPosition &last_position,
+ const constants::Values::Claws::Claw &claw, double initial_position,
+ 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");
+ UpdateHallEffect(position->position + initial_position,
+ last_position.position + initial_position,
+ initial_position, &position->posedge_value,
+ &position->negedge_value, &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");
}
// Sends out the position queue messages.
@@ -127,212 +192,13 @@
SetPhysicalSensors(position.get());
const frc971::constants::Values& values = constants::GetValues();
- double last_top_position =
- last_position_.top.position + initial_position_[TOP_CLAW];
- double last_bottom_position =
- last_position_.bottom.position + initial_position_[BOTTOM_CLAW];
- double top_position =
- position->top.position + initial_position_[TOP_CLAW];
- double bottom_position =
- position->bottom.position + initial_position_[BOTTOM_CLAW];
- // Handle the front hall effect.
- if (position->top.front_hall_effect &&
- !last_position_.top.front_hall_effect) {
- ++position->top.front_hall_effect_posedge_count;
-
- if (last_top_position < values.claw.upper_claw.front.lower_angle) {
- LOG(DEBUG, "Top: Positive lower edge front hall effect\n");
- position->top.posedge_value = values.claw.upper_claw.front.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Positive upper edge front hall effect\n");
- position->top.posedge_value = values.claw.upper_claw.front.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
- if (!position->top.front_hall_effect &&
- last_position_.top.front_hall_effect) {
- ++position->top.front_hall_effect_negedge_count;
-
- if (top_position < values.claw.upper_claw.front.lower_angle) {
- LOG(DEBUG, "Top: Negative lower edge front hall effect\n");
- position->top.negedge_value = values.claw.upper_claw.front.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Negative upper edge front hall effect\n");
- position->top.negedge_value = values.claw.upper_claw.front.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
-
- // Handle the calibration hall effect.
- if (position->top.calibration_hall_effect &&
- !last_position_.top.calibration_hall_effect) {
- ++position->top.calibration_hall_effect_posedge_count;
-
- if (last_top_position < values.claw.upper_claw.calibration.lower_angle) {
- LOG(DEBUG, "Top: Positive lower edge calibration hall effect\n");
- position->top.posedge_value =
- values.claw.upper_claw.calibration.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Positive upper edge calibration hall effect\n");
- position->top.posedge_value =
- values.claw.upper_claw.calibration.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
- if (!position->top.calibration_hall_effect &&
- last_position_.top.calibration_hall_effect) {
- ++position->top.calibration_hall_effect_negedge_count;
-
- if (top_position < values.claw.upper_claw.calibration.lower_angle) {
- LOG(DEBUG, "Top: Negative lower edge calibration hall effect\n");
- position->top.negedge_value =
- values.claw.upper_claw.calibration.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Negative upper edge calibration hall effect\n");
- position->top.negedge_value =
- values.claw.upper_claw.calibration.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
-
- // Handle the back hall effect.
- if (position->top.back_hall_effect &&
- !last_position_.top.back_hall_effect) {
- ++position->top.back_hall_effect_posedge_count;
-
- if (last_top_position < values.claw.upper_claw.back.lower_angle) {
- LOG(DEBUG, "Top: Positive lower edge back hall effect\n");
- position->top.posedge_value = values.claw.upper_claw.back.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Positive upper edge back hall effect\n");
- position->top.posedge_value = values.claw.upper_claw.back.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
- if (!position->top.back_hall_effect &&
- last_position_.top.back_hall_effect) {
- ++position->top.back_hall_effect_negedge_count;
-
- if (top_position < values.claw.upper_claw.back.lower_angle) {
- LOG(DEBUG, "Top: Negative upper edge back hall effect\n");
- position->top.negedge_value = values.claw.upper_claw.back.lower_angle -
- initial_position_[TOP_CLAW];
- } else {
- LOG(DEBUG, "Top: Negative lower edge back hall effect\n");
- position->top.negedge_value = values.claw.upper_claw.back.upper_angle -
- initial_position_[TOP_CLAW];
- }
- }
-
- // Now deal with the bottom part of the claw.
- // Handle the front hall effect.
- if (position->bottom.front_hall_effect &&
- !last_position_.bottom.front_hall_effect) {
- ++position->bottom.front_hall_effect_posedge_count;
-
- if (last_bottom_position < values.claw.lower_claw.front.lower_angle) {
- LOG(DEBUG, "Bottom: Positive lower edge front hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.front.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Positive upper edge front hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.front.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
- if (!position->bottom.front_hall_effect &&
- last_position_.bottom.front_hall_effect) {
- ++position->bottom.front_hall_effect_negedge_count;
-
- if (bottom_position < values.claw.lower_claw.front.lower_angle) {
- LOG(DEBUG, "Bottom: Negative lower edge front hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.front.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Negative upper edge front hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.front.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
-
- // Handle the calibration hall effect.
- if (position->bottom.calibration_hall_effect &&
- !last_position_.bottom.calibration_hall_effect) {
- ++position->bottom.calibration_hall_effect_posedge_count;
-
- if (last_bottom_position <
- values.claw.lower_claw.calibration.lower_angle) {
- LOG(DEBUG, "Bottom: Positive lower edge calibration hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.calibration.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Positive upper edge calibration hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.calibration.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
- if (!position->bottom.calibration_hall_effect &&
- last_position_.bottom.calibration_hall_effect) {
- ++position->bottom.calibration_hall_effect_negedge_count;
-
- if (bottom_position < values.claw.lower_claw.calibration.lower_angle) {
- LOG(DEBUG, "Bottom: Negative lower edge calibration hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.calibration.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Negative upper edge calibration hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.calibration.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
-
- // Handle the back hall effect.
- if (position->bottom.back_hall_effect &&
- !last_position_.bottom.back_hall_effect) {
- ++position->bottom.back_hall_effect_posedge_count;
-
- if (last_bottom_position < values.claw.lower_claw.back.lower_angle) {
- LOG(DEBUG, "Bottom: Positive lower edge back hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.back.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Positive upper edge back hall effect\n");
- position->bottom.posedge_value =
- values.claw.lower_claw.back.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
- if (!position->bottom.back_hall_effect &&
- last_position_.bottom.back_hall_effect) {
- ++position->bottom.back_hall_effect_negedge_count;
-
- if (bottom_position < values.claw.lower_claw.back.lower_angle) {
- LOG(DEBUG, "Bottom: Negative lower edge back hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.back.lower_angle -
- initial_position_[BOTTOM_CLAW];
- } else {
- LOG(DEBUG, "Bottom: Negative upper edge back hall effect\n");
- position->bottom.negedge_value =
- values.claw.lower_claw.back.upper_angle -
- initial_position_[BOTTOM_CLAW];
- }
- }
+ UpdateClawHallEffects(&position->top, last_position_.top,
+ values.claw.upper_claw, initial_position_[TOP_CLAW],
+ "Top");
+ UpdateClawHallEffects(&position->bottom, last_position_.bottom,
+ values.claw.lower_claw,
+ initial_position_[BOTTOM_CLAW], "Bottom");
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 94c73d0..273063c 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -1,6 +1,30 @@
{
'targets': [
{
+ 'target_name': 'hall_effect_tracker',
+ 'type': 'static_library',
+ 'sources': [
+ #'hall_effect_tracker.h',
+ ],
+ 'dependencies': [
+ 'queues',
+ ],
+ 'export_dependent_settings': [
+ 'queues',
+ ],
+ },
+ {
+ 'target_name': 'queues',
+ 'type': 'static_library',
+ 'sources': [
+ 'control_loops.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/control_loops',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'state_feedback_loop',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
new file mode 100644
index 0000000..ccc8388
--- /dev/null
+++ b/frc971/control_loops/control_loops.q
@@ -0,0 +1,7 @@
+package frc971;
+
+struct HallEffectStruct {
+ bool current;
+ int32_t posedge_count;
+ int32_t negedge_count;
+};
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
new file mode 100644
index 0000000..79d14af
--- /dev/null
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -0,0 +1,52 @@
+#ifndef FRC971_CONTROL_LOOPS_HALL_EFFECT_H_
+#define FRC971_CONTROL_LOOPS_HALL_EFFECT_H_
+
+#include <stdint.h>
+
+#include "frc971/control_loops/control_loops.q.h"
+
+namespace frc971 {
+
+class HallEffectTracker {
+ public:
+ int32_t get_posedges() const { return posedges_.count(); }
+ int32_t get_negedges() const { return negedges_.count(); }
+
+ bool either_count_changed() const {
+ return posedges_.count_changed() || negedges_.count_changed();
+ }
+ bool posedge_count_changed() const { return posedges_.count_changed(); }
+ bool negedge_count_changed() const { return negedges_.count_changed(); }
+
+ bool value() const { return value_; }
+
+ void Update(const HallEffectStruct &position) {
+ value_ = position.current;
+ posedges_.update(position.posedge_count);
+ negedges_.update(position.negedge_count);
+ }
+
+ private:
+ class {
+ public:
+ void update(int32_t count) {
+ previous_count_ = count_;
+ count_ = count;
+ }
+
+ bool count_changed() const {
+ return previous_count_ != count_;
+ }
+
+ int32_t count() const { return count_; }
+
+ private:
+ int32_t count_ = 0;
+ int32_t previous_count_ = 0;
+ } posedges_, negedges_;
+ bool value_ = false;
+};
+
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_HALL_EFFECT_H_