Merge branch 'claw' into ben_shooter
Conflicts:
frc971/control_loops/claw/claw.cc
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/constants.cc b/frc971/constants.cc
index 4b181eb..dd47975 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -69,8 +69,8 @@
1.57,
0,
0,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
+ {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index dd81ecb..00dca6f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -48,6 +48,11 @@
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
+ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
+ : StateFeedbackLoop<4, 2, 2>(loop),
+ uncapped_average_voltage_(0.0),
+ is_zeroing_(true) {}
+
void ClawLimitedLoop::CapU() {
uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
if (is_zeroing_) {
@@ -75,6 +80,100 @@
}
}
+ZeroedStateFeedbackLoop::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) {}
+
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition &claw) {
+ front_.Update(claw.front);
+ calibration_.Update(claw.calibration);
+ back_.Update(claw.back);
+
+ bool any_sensor_triggered = any_triggered();
+ if (any_sensor_triggered && any_triggered_last_) {
+ // We are still on the hall effect and nothing has changed.
+ min_hall_effect_on_angle_ =
+ ::std::min(min_hall_effect_on_angle_, claw.position);
+ max_hall_effect_on_angle_ =
+ ::std::max(max_hall_effect_on_angle_, claw.position);
+ } else if (!any_sensor_triggered && !any_triggered_last_) {
+ // We are still off the hall effect and nothing has changed.
+ min_hall_effect_off_angle_ =
+ ::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;
+ last_encoder_ = encoder_;
+ if (front().value() || calibration().value() || back().value()) {
+ last_on_encoder_ = encoder_;
+ } else {
+ last_off_encoder_ = encoder_;
+ }
+ encoder_ = claw.position;
+ any_triggered_last_ = any_sensor_triggered;
+}
+
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
+ set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+
+ front_.Reset();
+ calibration_.Reset();
+ back_.Reset();
+ // 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;
+ max_hall_effect_on_angle_ = claw.position;
+ min_hall_effect_off_angle_ = claw.position;
+ max_hall_effect_off_angle_ = claw.position;
+ any_triggered_last_ = any_triggered();
+}
+
+bool TopZeroedStateFeedbackLoop::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 edge should be %f.\n", edge_angle);
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+}
+
+bool BottomZeroedStateFeedbackLoop::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;
+}
+
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
: aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
has_top_claw_goal_(false),
@@ -96,33 +195,57 @@
double *edge_angle, const HallEffectTracker &sensor,
const char *hall_effect_name) {
bool found_edge = false;
+
if (sensor.posedge_count_changed()) {
- if (posedge_value_ < last_off_encoder_) {
- *edge_angle = angles.upper_angle;
- LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f last_encoder: %f\n", name_,
- hall_effect_name, *edge_angle, posedge_value_, last_off_encoder_);
+ 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;
} else {
- *edge_angle = angles.lower_angle;
- LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f last_encoder: %f\n", name_,
- hall_effect_name, *edge_angle, posedge_value_, last_off_encoder_);
+ const double average_last_encoder =
+ (min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
+ if (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_,
+ 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_,
+ average_last_encoder);
+ }
}
*edge_encoder = posedge_value_;
found_edge = true;
}
if (sensor.negedge_count_changed()) {
- if (negedge_value_ > last_on_encoder_) {
- *edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f last_encoder: %f\n", name_,
- hall_effect_name, *edge_angle, negedge_value_, last_on_encoder_);
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ *edge_angle = last_edge_value_;
+ found_edge = true;
} else {
- *edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f last_encoder: %f\n", name_,
- hall_effect_name, *edge_angle, negedge_value_, last_on_encoder_);
+ const double average_last_encoder =
+ (min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
+ if (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_,
+ 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_,
+ average_last_encoder);
+ }
+ *edge_encoder = negedge_value_;
}
- *edge_encoder = negedge_value_;
found_edge = true;
}
+ if (found_edge) {
+ last_edge_value_ = *edge_angle;
+ }
+
return found_edge;
}
@@ -264,10 +387,8 @@
}
if (reset()) {
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- top_claw_.Reset();
- bottom_claw_.Reset();
+ top_claw_.Reset(position->top);
+ bottom_claw_.Reset(position->bottom);
}
if (::aos::robot_state.get() == nullptr) {
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index bff533d..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,27 +67,9 @@
}
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);
- posedge_value_ = claw.posedge_value;
- negedge_value_ = claw.negedge_value;
- last_encoder_ = encoder_;
- if (front().value() || calibration().value() || back().value()) {
- last_on_encoder_ = encoder_;
- } else {
- last_off_encoder_ = encoder_;
- }
- encoder_ = claw.position;
- }
-
- void Reset() {
- front_.Reset();
- calibration_.Reset();
- back_.Reset();
- }
+ void Reset(const HalfClawPosition &claw);
bool ready() {
return front_.ready() && calibration_.ready() && back_.ready();
@@ -118,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_; }
@@ -143,10 +117,16 @@
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_;
+ 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.
@@ -165,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 edge should be %f.\n", edge_angle);
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
+ JointZeroingState zeroing_state);
};
class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
@@ -187,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
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 3fa9079..99194bd 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -29,7 +29,7 @@
class TeamNumberEnvironment : public ::testing::Environment {
public:
// Override this to define how to set up the environment.
- virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
+ virtual void SetUp() { aos::network::OverrideTeamNumber(1); }
};
::testing::Environment* const team_number_env =