Added ability to fine tune calibration maybe after auto. Goes to set position then catches the up edge on the calib hall effect. Does bottom then the top claw.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
old mode 100644
new mode 100755
index 8c35733..e3e7e6d
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -82,7 +82,8 @@
has_bottom_claw_goal_(false),
bottom_claw_goal_(0.0),
bottom_claw_(MakeBottomClawLoop()),
- was_enabled_(false) {}
+ was_enabled_(false),
+ doing_calibration_fine_tune_(false) {}
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
@@ -219,7 +220,73 @@
// Time to fine tune the zero.
// Limit the goals here.
if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
+ // always get the bottom claw to calibrated first
+ if (!doing_calibration_fine_tune_) {
+ if (position->bottom.position > values.start_fine_tune_pos -
+ values.claw_unimportant_epsilon &&
+ position->bottom.position < values.start_fine_tune_pos +
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ } else {
+ // send bottom to zeroing start
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ }
+ } else {
+ if (position->bottom.front_hall_effect ||
+ position->bottom.back_hall_effect ||
+ position->top.front_hall_effect ||
+ position->top.back_hall_effect) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.start_fine_tune_pos;
+ }
+ if (position->bottom.calibration_hall_effect) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calinrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ }
+ }
+ // now set the top claw to track
+ top_claw_goal_ = bottom_claw_goal_ + goal->seperation_angle;
} else {
+ // bottom claw must be calibrated, start on the top
+ if (!doing_calibration_fine_tune_) {
+ if (position->top.position > values.start_fine_tune_pos -
+ values.claw_unimportant_epsilon &&
+ position->top.position < values.start_fine_tune_pos +
+ values.claw_unimportant_epsilon) {
+ doing_calibration_fine_tune_ = true;
+ top_claw_goal_ += values.claw_zeroing_off_speed * dt;
+ } else {
+ // send top to zeroing start
+ top_claw_goal_ = values.start_fine_tune_pos;
+ }
+ } else {
+ if (position->top.front_hall_effect ||
+ position->top.back_hall_effect ||
+ position->top.front_hall_effect ||
+ position->top.back_hall_effect) {
+ // this should not happen, but now we know it won't
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.start_fine_tune_pos;
+ }
+ if (position->top.calibration_hall_effect) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calinrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ }
+ }
+ // now set the bottom claw to track
+ bottom_claw_goal_ = top_claw_goal_ - goal->seperation_angle;
}
} else {
if (!was_enabled_ && enabled) {
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
old mode 100644
new mode 100755
index 4ee89c4..c045d6f
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -50,7 +50,7 @@
posedge_value_(0.0),
negedge_value_(0.0),
encoder_(0.0),
- last_encoder_(0.0) {}
+ last_encoder_(0.0){}
const static int kZeroingMaxVoltage = 5;
@@ -186,6 +186,7 @@
double last_encoder_;
};
+
class ClawMotor
: public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
public:
@@ -219,6 +220,7 @@
ZeroedStateFeedbackLoop bottom_claw_;
bool was_enabled_;
+ bool doing_calibration_fine_tune_;
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};