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/constants.cc b/frc971/constants.cc
old mode 100644
new mode 100755
index 14745b5..466fe60
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -60,6 +60,8 @@
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.02, // claw_unimportant_epsilon
+ 50505.05, // start_fine_tune_pos
};
break;
case kPracticeTeamNumber:
@@ -78,6 +80,8 @@
1.57,
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.02, // claw_unimportant_epsilon
+ 50505.05, //start_fine_tune_pos
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
old mode 100644
new mode 100755
index 9de3a28..4a6949e
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -61,8 +61,12 @@
AnglePair back;
};
+
Claw upper_claw;
Claw lower_claw;
+
+ double claw_unimportant_epsilon;
+ double start_fine_tune_pos;
};
// Creates (once) a Values instance and returns a reference to it.
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);
};