Claw now zeros, though slowly. Not sure about high speeds yet and the constants seem wrong.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 971c1a6..f9f19a6 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -136,17 +136,11 @@
0.90675,
-0.39110,
0.843349,
-#if 0
- separations (top, bottom)
- hard min position:-0.253845, position:-0.001136,
- soft min position:-0.244528, position:-0.047269,
- soft max position:0.526326, position:-0.510872,
- hard max position:0.517917, position:-0.582685,
-#endif
{-1.62102, 1.039699, -1.606248, 0.989702, {-1.65, -1.546252, -1.65, -1.548752}, {-0.13249, -0.02113, -0.134763, -0.021589}, {0.934024, 1.05, 0.92970, 1.05}},
{-1.420352, 1.348313, -1.161281, 1.264001, {-1.45, -1.283771, -1.45, -1.28468}, {-0.332476, -0.214984, -0.334294, -0.217029}, {1.248547, 1.37, 1.245366, 1.37}},
0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
+ -0.4, // start_fine_tune_pos
+ // TODO(austin): Different for the two halfs.
4.0,
}
};
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index be21efe..5924731 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -45,9 +45,12 @@
namespace frc971 {
namespace control_loops {
+static const double kMaxVoltage = 1.5;
+
void ClawLimitedLoop::CapU() {
uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
if (is_zeroing_) {
+ LOG(DEBUG, "zeroing\n");
const frc971::constants::Values &values = constants::GetValues();
if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
const double difference =
@@ -63,9 +66,9 @@
double max_value =
::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
- if (max_value > 12.0) {
+ if (max_value > kMaxVoltage) {
LOG(DEBUG, "Capping U because max is %f\n", max_value);
- U = U * 12.0 / max_value;
+ U = U * kMaxVoltage / max_value;
LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
}
}
@@ -90,34 +93,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 upper of %s -> %f\n", name_,
- hall_effect_name, *edge_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_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f\n", name_,
- hall_effect_name, *edge_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_);
}
*edge_encoder = negedge_value_;
- return true;
+ found_edge = true;
}
- return false;
+ return found_edge;
}
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
@@ -260,6 +264,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) {
@@ -555,6 +561,18 @@
if (output) {
output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 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/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 79d14af..7e6617c 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -7,6 +7,7 @@
namespace frc971 {
+// TODO(brians): Have a Reset() for when the cape resets.
class HallEffectTracker {
public:
int32_t get_posedges() const { return posedges_.count(); }
@@ -26,23 +27,38 @@
negedges_.update(position.negedge_count);
}
+ void Reset() {
+ posedges_.Reset();
+ negedges_.Reset();
+ }
+
+ bool ready() { return posedges_.ready() && negedges_.ready(); }
+
private:
class {
public:
void update(int32_t count) {
+ if (first_) {
+ count_ = count;
+ LOG(DEBUG, "First time through the hall effect, resetting\n");
+ }
previous_count_ = count_;
count_ = count;
+ first_ = false;
}
- bool count_changed() const {
- return previous_count_ != count_;
- }
+ void Reset() { first_ = true; }
+
+ bool count_changed() const { return !first_ && previous_count_ != count_; }
int32_t count() const { return count_; }
+ bool ready() { return !first_; }
+
private:
int32_t count_ = 0;
int32_t previous_count_ = 0;
+ bool first_ = true;
} posedges_, negedges_;
bool value_ = false;
};
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 8f435fb..763f005 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -463,7 +463,14 @@
// If it is latched and the plunger is back, move the pusher back to catch
// the plunger.
- if (position->plunger && position->latch) {
+ bool all_back;
+ if (position) {
+ all_back = position->plunger && position->latch;
+ } else {
+ all_back = last_position_.plunger && last_position_.latch;
+ }
+
+ if (all_back) {
// Pull back to 0, 0.
shooter_.SetGoalPosition(0.0, 0.0);
if (shooter_.absolute_position() < 0.005) {
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index ef23d21..e17d6bc 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -37,7 +37,7 @@
if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(8, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
SetSolenoid(7, drivetrain.output->left_high);
SetSolenoid(8, drivetrain.output->right_high);
} else {
@@ -53,7 +53,7 @@
shooter.FetchLatest();
if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *shooter.get());
- SetPWMOutput(9, shooter->voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, shooter->voltage / 12.0, kTalonBounds);
SetSolenoid(6, !shooter->latch_piston);
SetSolenoid(5, !shooter->brake_piston);
} else {
@@ -68,8 +68,8 @@
claw.FetchLatest();
if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *claw.get());
- SetPWMOutput(6, claw->intake_voltage / 12.0, kTalonBounds);
- SetPWMOutput(7, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(9, claw->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(8, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(1, -claw->bottom_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(2, claw->top_claw_voltage / 12.0, kTalonBounds);
SetPWMOutput(5, claw->tusk_voltage / 12.0, kTalonBounds); // left