Merge branch 'claw' into ben_shooter
Conflicts:
frc971/control_loops/claw/claw.cc
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 50076a7..dc8e6ba 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -23,10 +23,7 @@
class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
public:
- ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
- : StateFeedbackLoop<4, 2, 2>(loop),
- uncapped_average_voltage_(0.0),
- is_zeroing_(true) {}
+ ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop);
virtual void CapU();
void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
@@ -49,15 +46,7 @@
// controller.
class ZeroedStateFeedbackLoop {
public:
- ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor)
- : offset_(0.0),
- name_(name),
- motor_(motor),
- zeroing_state_(UNKNOWN_POSITION),
- posedge_value_(0.0),
- negedge_value_(0.0),
- encoder_(0.0),
- last_encoder_(0.0) {}
+ ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor);
const static int kZeroingMaxVoltage = 5;
@@ -78,26 +67,12 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- void SetPositionValues(const HalfClawPosition &claw) {
- front_.Update(claw.front);
- calibration_.Update(claw.calibration);
- back_.Update(claw.back);
+ void SetPositionValues(const HalfClawPosition &claw);
- if (any_hall_effect_changed()) {
- // 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_) {
- min_current_hall_effect_edge_ = claw.position;
- }
+ void Reset(const HalfClawPosition &claw);
- posedge_value_ = claw.posedge_value;
- negedge_value_ = claw.negedge_value;
- last_encoder_ = encoder_;
- encoder_ = claw.position;
+ bool ready() {
+ return front_.ready() && calibration_.ready() && back_.ready();
}
double absolute_position() const { return encoder() + offset(); }
@@ -114,6 +89,9 @@
bool front_or_back_triggered() const {
return front().value() || back().value();
}
+ bool any_triggered() const {
+ return calibration().value() || front().value() || back().value();
+ }
double encoder() const { return encoder_; }
double last_encoder() const { return last_encoder_; }
@@ -140,10 +118,15 @@
double posedge_value_;
double negedge_value_;
double last_edge_value_;
- double min_current_hall_effect_edge_;
- double max_current_hall_effect_edge_;
+ double min_hall_effect_on_angle_;
+ double max_hall_effect_on_angle_;
+ double min_hall_effect_off_angle_;
+ double max_hall_effect_off_angle_;
double encoder_;
double last_encoder_;
+ double last_on_encoder_;
+ double last_off_encoder_;
+ bool any_triggered_last_;
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
@@ -162,17 +145,7 @@
void SetCalibration(double edge_encoder, double edge_angle);
bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double edge_angle;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ JointZeroingState zeroing_state);
};
class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
@@ -184,17 +157,7 @@
void SetCalibration(double edge_encoder, double edge_angle);
bool SetCalibrationOnEdge(const constants::Values::Claws::Claw &claw_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double edge_angle;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ JointZeroingState zeroing_state);
};
class ClawMotor