Merge remote-tracking branch 'austin/claw' into claw
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 1934032..11719ee 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -45,29 +45,19 @@
namespace frc971 {
namespace control_loops {
+static const double kZeroingVoltage = 4.0;
+static const double kMaxVoltage = 12.0;
+
void ClawLimitedLoop::CapU() {
uncapped_average_voltage_ = (U(0, 0) + U(1, 0)) / 2.0;
- if (is_zeroing_) {
- const frc971::constants::Values &values = constants::GetValues();
- if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
- const double difference =
- 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
- U(0, 0) *= difference;
- U(1, 0) *= difference;
- } else if (uncapped_average_voltage_ < -values.claw.max_zeroing_voltage) {
- const double difference =
- 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
- U(0, 0) *= difference;
- U(1, 0) *= difference;
- }
- }
+ const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
double max_value =
::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
- double scalar = 12.0 / max_value;
- bool bottom_big = (::std::abs(U(0, 0)) > 12.0) &&
+ double scalar = k_max_voltage / max_value;
+ bool bottom_big = (::std::abs(U(0, 0)) > k_max_voltage) &&
(::std::abs(U(0, 0)) > ::std::abs(U(1, 0)));
- bool top_big = (::std::abs(U(1, 0)) > 12.0) && (!bottom_big);
+ bool top_big = (::std::abs(U(1, 0)) > k_max_voltage) && (!bottom_big);
double separation_voltage = U(1, 0) - U(0, 0) * kClawMomentOfInertiaRatio;
double u_top = U(1, 0);
double u_bottom = U(0, 0);
@@ -77,15 +67,15 @@
u_bottom *= scalar;
u_top = separation_voltage + u_bottom * kClawMomentOfInertiaRatio;
// If we can't maintain the separation, just clip it.
- if (u_top > 12.0) u_top = 12.0;
- else if (u_top < -12.0) u_top = -12.0;
+ if (u_top > k_max_voltage) u_top = k_max_voltage;
+ else if (u_top < -k_max_voltage) u_top = -k_max_voltage;
}
else if (top_big) {
LOG(DEBUG, "Capping U because top is %f\n", max_value);
u_top *= scalar;
u_bottom = (u_top - separation_voltage) / kClawMomentOfInertiaRatio;
- if (u_bottom > 12.0) u_bottom = 12.0;
- else if (u_bottom < -12.0) u_bottom = -12.0;
+ if (u_bottom > k_max_voltage) u_bottom = k_max_voltage;
+ else if (u_bottom < -k_max_voltage) u_bottom = -k_max_voltage;
}
U(0, 0) = u_bottom;
@@ -116,34 +106,35 @@
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
double *edge_angle, const HallEffectTracker &sensor,
const char *hall_effect_name) {
+ bool found_edge = false;
if (sensor.posedge_count_changed()) {
- if (posedge_value_ < last_encoder()) {
+ if (posedge_value_ < last_off_encoder_) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Posedge upper of %s -> %f\n", name_,
- hall_effect_name, *edge_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_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Posedge lower of %s -> %f\n", name_,
- hall_effect_name, *edge_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_);
}
*edge_encoder = posedge_value_;
- return true;
+ found_edge = true;
}
if (sensor.negedge_count_changed()) {
- if (negedge_value_ > last_encoder()) {
+ if (negedge_value_ > last_on_encoder_) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
- hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
+ 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_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
- hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
+ 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_);
}
*edge_encoder = negedge_value_;
- return true;
+ found_edge = true;
}
- return false;
+ return found_edge;
}
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
@@ -286,6 +277,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();
}
if (::aos::robot_state.get() == nullptr) {
@@ -536,7 +529,8 @@
LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
claw_.R(1, 0), separation);
- // Only cap power when one of the halves of the claw is unknown.
+ // Only cap power when one of the halves of the claw is moving slowly and
+ // could wind up.
claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
mode_ == FINE_TUNE_BOTTOM);
claw_.Update(output == nullptr);
@@ -596,6 +590,18 @@
if (output) {
output->top_claw_voltage = claw_.U(1, 0);
output->bottom_claw_voltage = claw_.U(0, 0);
+
+ if (output->top_claw_voltage > kMaxVoltage) {
+ output->top_claw_voltage = kMaxVoltage;
+ } else if (output->top_claw_voltage < -kMaxVoltage) {
+ output->top_claw_voltage = -kMaxVoltage;
+ }
+
+ if (output->bottom_claw_voltage > kMaxVoltage) {
+ output->bottom_claw_voltage = kMaxVoltage;
+ } else if (output->bottom_claw_voltage < -kMaxVoltage) {
+ output->bottom_claw_voltage = -kMaxVoltage;
+ }
}
status->done = false;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 2d13d37..bff533d 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -86,9 +86,24 @@
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();
+ }
+
+ bool ready() {
+ return front_.ready() && calibration_.ready() && back_.ready();
+ }
+
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -130,6 +145,8 @@
double negedge_value_;
double encoder_;
double last_encoder_;
+ double last_on_encoder_;
+ double last_off_encoder_;
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
@@ -152,7 +169,7 @@
double edge_encoder;
double edge_angle;
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
+ LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(zeroing_state);
return true;
diff --git a/frc971/control_loops/claw/claw_calibration.cc b/frc971/control_loops/claw/claw_calibration.cc
index 0167c08..608c72d 100644
--- a/frc971/control_loops/claw/claw_calibration.cc
+++ b/frc971/control_loops/claw/claw_calibration.cc
@@ -8,69 +8,144 @@
typedef constants::Values::Claws Claws;
-bool DoGetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position,
- const HallEffectStruct &last_hall_effect,
- const HallEffectStruct &hall_effect,
- Claws::AnglePair *limits) {
+class Sensor {
+ public:
+ Sensor(const double start_position,
+ const HallEffectStruct &initial_hall_effect)
+ : start_position_(start_position),
+ last_hall_effect_(initial_hall_effect),
+ last_posedge_count_(initial_hall_effect.posedge_count),
+ last_negedge_count_(initial_hall_effect.negedge_count) {
+ last_on_min_position_ = start_position;
+ last_on_max_position_ = start_position;
+ last_off_min_position_ = start_position;
+ last_off_max_position_ = start_position;
+ }
- if (hall_effect.posedge_count != last_hall_effect.posedge_count) {
- if (claw_position.posedge_value < last_claw_position.position) {
- limits->upper_angle = claw_position.posedge_value - start_position;
- } else {
- limits->lower_decreasing_angle =
- claw_position.posedge_value - start_position;
+ bool DoGetPositionOfEdge(
+ const control_loops::HalfClawPosition &claw_position,
+ const HallEffectStruct &hall_effect, Claws::AnglePair *limits) {
+ 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) {
+ printf("Posedge upper current %f posedge %f avg_off %f [%f, %f]\n",
+ claw_position.position, claw_position.posedge_value,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->upper_decreasing_angle = claw_position.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,
+ avg_off_position, last_off_min_position_,
+ last_off_max_position_);
+ limits->lower_angle =
+ claw_position.posedge_value - start_position_;
+ }
+ print = true;
}
- return true;
- }
- if (hall_effect.negedge_count != last_hall_effect.negedge_count) {
- if (claw_position.negedge_value > last_claw_position.position) {
- limits->upper_decreasing_angle =
- claw_position.negedge_value - start_position;
- } else {
- limits->lower_angle = claw_position.negedge_value - start_position;
+ 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) {
+ printf("Negedge upper current %f negedge %f last_on %f [%f, %f]\n",
+ claw_position.position, claw_position.negedge_value,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->upper_angle =
+ claw_position.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,
+ avg_on_position, last_on_min_position_,
+ last_on_max_position_);
+ limits->lower_decreasing_angle = claw_position.negedge_value - start_position_;
+ }
+ print = true;
}
- return true;
+
+ if (hall_effect.current) {
+ if (!last_hall_effect_.current) {
+ last_on_min_position_ = last_on_max_position_ = claw_position.position;
+ } else {
+ last_on_min_position_ =
+ ::std::min(claw_position.position, last_on_min_position_);
+ last_on_max_position_ =
+ ::std::max(claw_position.position, last_on_max_position_);
+ }
+ } else {
+ if (last_hall_effect_.current) {
+ 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_);
+ last_off_max_position_ =
+ ::std::max(claw_position.position, last_off_max_position_);
+ }
+ }
+
+ last_hall_effect_ = hall_effect;
+ last_posedge_count_ = hall_effect.posedge_count;
+ last_negedge_count_ = hall_effect.negedge_count;
+
+ return print;
}
- return false;
-}
+ private:
+ const double start_position_;
+ HallEffectStruct last_hall_effect_;
+ int32_t last_posedge_count_;
+ int32_t last_negedge_count_;
+ double last_on_min_position_;
+ double last_off_min_position_;
+ double last_on_max_position_;
+ double last_off_max_position_;
+};
-bool GetPositionOfEdge(
- const double start_position,
- const control_loops::HalfClawPosition &last_claw_position,
- const control_loops::HalfClawPosition &claw_position, Claws::Claw *claw) {
+class ClawSensors {
+ public:
+ ClawSensors(const double start_position,
+ const control_loops::HalfClawPosition &initial_claw_position)
+ : start_position_(start_position),
+ front_(start_position, initial_claw_position.front),
+ calibration_(start_position, initial_claw_position.calibration),
+ back_(start_position, initial_claw_position.back) {}
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.front, claw_position.front,
- &claw->front)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.calibration,
- claw_position.calibration, &claw->calibration)) {
- return true;
- }
- if (DoGetPositionOfEdge(start_position, last_claw_position, claw_position,
- last_claw_position.back, claw_position.back,
- &claw->back)) {
- return true;
+ bool GetPositionOfEdge(const control_loops::HalfClawPosition &claw_position,
+ Claws::Claw *claw) {
+
+ bool print = false;
+ if (front_.DoGetPositionOfEdge(claw_position,
+ claw_position.front, &claw->front)) {
+ print = true;
+ } else if (calibration_.DoGetPositionOfEdge(claw_position,
+ claw_position.calibration,
+ &claw->calibration)) {
+ print = true;
+ } else if (back_.DoGetPositionOfEdge(claw_position,
+ claw_position.back, &claw->back)) {
+ print = true;
+ }
+
+ double position = claw_position.position - start_position_;
+
+ if (position > claw->upper_limit) {
+ claw->upper_hard_limit = claw->upper_limit = position;
+ print = true;
+ }
+ if (position < claw->lower_limit) {
+ claw->lower_hard_limit = claw->lower_limit = position;
+ print = true;
+ }
+ return print;
}
- double position = claw_position.position - start_position;
-
- if (position > claw->upper_limit) {
- claw->upper_hard_limit = claw->upper_limit = position;
- return true;
- }
- if (position < claw->lower_limit) {
- claw->lower_hard_limit = claw->lower_limit = position;
- return true;
- }
- return false;
-}
+ private:
+ const double start_position_;
+ Sensor front_;
+ Sensor calibration_;
+ Sensor back_;
+};
int Main() {
while (!control_loops::claw_queue_group.position.FetchNextBlocking());
@@ -80,6 +155,11 @@
const double bottom_start_position =
control_loops::claw_queue_group.position->bottom.position;
+ ClawSensors top(top_start_position,
+ control_loops::claw_queue_group.position->top);
+ ClawSensors bottom(bottom_start_position,
+ control_loops::claw_queue_group.position->bottom);
+
Claws limits;
limits.claw_zeroing_off_speed = 0.5;
@@ -138,17 +218,16 @@
while (true) {
if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
bool print = false;
- if (GetPositionOfEdge(top_start_position, last_position.top,
- control_loops::claw_queue_group.position->top,
- &limits.upper_claw)) {
+ if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the upper claw\n");
+ printf("Got an edge on the upper claw\n");
}
- if (GetPositionOfEdge(bottom_start_position, last_position.bottom,
- control_loops::claw_queue_group.position->bottom,
- &limits.lower_claw)) {
+ if (bottom.GetPositionOfEdge(
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
print = true;
- LOG(DEBUG, "Got an edge on the lower claw\n");
+ printf("Got an edge on the lower claw\n");
}
const double top_position =
control_loops::claw_queue_group.position->top.position -