Claw now respects the limits.
diff --git a/frc971/constants.h b/frc971/constants.h
index 2a9c642..e9b2b05 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -69,8 +69,8 @@
double claw_zeroing_separation;
// claw seperation that would be considered a collision
- double claw_min_seperation;
- double claw_max_seperation;
+ double claw_min_separation;
+ double claw_max_separation;
// Three hall effects are known as front, calib and back
struct AnglePair {
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b445546..b6e3024 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -186,37 +186,46 @@
X_hat(1, 0) -= doffset;
LOG(INFO, "Changing bottom offset by %f\n", doffset);
}
-//void FixClawPos(double * bottom_goal, double * top_goal, frc971::constants::Values &values) {
-// //first update position based on angle limit
-//
-// double goal_angle = *top_goal - *bottom_goal;
-// if (goal_angle > values.lower_claw.front.upper_angle) {
-// *bottom_goal += goal_angle - values.lower_claw.front.upper_angle;
-// }
-// if (goal_angle < values.lower_claw.front.lower_angle) {
-// *bottom_goal += goal_angle - values.lower_claw.front.lower_angle;
-// }
-//
-// //now move both goals in unison
-// if (*bottom_goal < values.bottom.lower_limit) {
-// *bottom_goal += *bottom_goal - values.lower_claw.lower_limit;
-// *top_goal += *bottom_goal - values.bottom.lower_limit;
-// }
-// if (*bottom_goal > values.bottom.upper_limit) {
-// *bottom_goal -= *bottom_goal - values.bottom.upper_limit;
-// *top_goal -= *bottom_goal - values.bottom.upper_limit;
-// }
-//
-// if (*top_goal < values.bottom.lower_limit) {
-// *top_goal += *top_goal - values.bottom.lower_limit;
-// *bottom_goal += *top_goal - values.bottom.lower_limit;
-// }
-// if (*top_goal > values.top.upper_limit) {
-// *top_goal -= *top_goal - values.top.upper_limit;
-// *bottom_goal -= *top_goal - values.top.upper_limit;
-// }
-//}
+void LimitClawGoal(double *bottom_goal, double *top_goal,
+ const frc971::constants::Values &values) {
+ // first update position based on angle limit
+
+ const double separation = *top_goal - *bottom_goal;
+ if (separation > values.claw.claw_max_separation) {
+ LOG(DEBUG, "Greater than\n");
+ const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
+ *bottom_goal += dsep;
+ *top_goal -= dsep;
+ LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
+ }
+ if (separation < values.claw.claw_min_separation) {
+ LOG(DEBUG, "Less than\n");
+ const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
+ *bottom_goal += dsep;
+ *top_goal -= dsep;
+ LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
+ }
+
+ // now move both goals in unison
+ if (*bottom_goal < values.claw.lower_claw.lower_limit) {
+ *top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
+ *bottom_goal = values.claw.lower_claw.lower_limit;
+ }
+ if (*bottom_goal > values.claw.lower_claw.upper_limit) {
+ *top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
+ *bottom_goal = values.claw.lower_claw.upper_limit;
+ }
+
+ if (*top_goal < values.claw.upper_claw.lower_limit) {
+ *bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
+ *top_goal = values.claw.upper_claw.lower_limit;
+ }
+ if (*top_goal > values.claw.upper_claw.upper_limit) {
+ *bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
+ *top_goal = values.claw.upper_claw.upper_limit;
+ }
+}
bool ClawMotor::is_ready() const {
return (
@@ -248,9 +257,6 @@
output->intake_voltage = 0;
}
- // TODO(austin): Handle zeroing while disabled correctly (only use a single
- // edge and direction when zeroing.)
-
if (reset()) {
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
@@ -287,8 +293,11 @@
top_claw_.absolute_position(), bottom_claw_.absolute_position());
}
- bool autonomous = ::aos::robot_state->autonomous;
- bool enabled = ::aos::robot_state->enabled;
+ const bool autonomous = ::aos::robot_state->autonomous;
+ const bool enabled = ::aos::robot_state->enabled;
+
+ double bottom_claw_velocity_ = 0.0;
+ double top_claw_velocity_ = 0.0;
if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
@@ -327,6 +336,8 @@
values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
LOG(DEBUG, "Ready to fine tune the bottom\n");
mode_ = FINE_TUNE_BOTTOM;
} else {
@@ -338,12 +349,15 @@
} else {
mode_ = FINE_TUNE_BOTTOM;
bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
if (top_claw_.front_or_back_triggered() ||
bottom_claw_.front_or_back_triggered()) {
// We shouldn't hit a limit, but if we do, go back to the zeroing
// point and try again.
doing_calibration_fine_tune_ = false;
bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_BOTTOM;
}
@@ -362,6 +376,7 @@
} else {
doing_calibration_fine_tune_ = false;
bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
mode_ = PREP_FINE_TUNE_BOTTOM;
}
} else {
@@ -379,6 +394,8 @@
values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
LOG(DEBUG, "Ready to fine tune the top\n");
mode_ = FINE_TUNE_TOP;
} else {
@@ -390,11 +407,14 @@
} else {
mode_ = FINE_TUNE_TOP;
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_speed;
if (top_claw_.front_or_back_triggered() ||
bottom_claw_.front_or_back_triggered()) {
// this should not happen, but now we know it won't
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
@@ -412,6 +432,7 @@
} else {
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
mode_ = PREP_FINE_TUNE_TOP;
}
}
@@ -433,8 +454,6 @@
}
}
- // TODO(austin): Limit the goals here.
-
if ((bottom_claw_.zeroing_state() !=
ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
bottom_claw_.front().value() || top_claw_.front().value()) &&
@@ -444,7 +463,8 @@
// zero.
top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
- // TODO(austin): Goal velocity too!
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ values.claw.claw_zeroing_off_speed;
LOG(DEBUG, "Bottom is known.\n");
}
} else {
@@ -453,7 +473,8 @@
if (enabled) {
top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
- // TODO(austin): Goal velocity too!
+ top_claw_velocity_ = bottom_claw_velocity_ =
+ -values.claw.claw_zeroing_off_speed;
LOG(DEBUG, "Both are unknown.\n");
}
}
@@ -464,6 +485,8 @@
bottom_claw_.SetCalibrationOnEdge(
values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
} else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo low. :)
top_claw_.SetCalibrationOnEdge(
values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
bottom_claw_.SetCalibrationOnEdge(
@@ -472,14 +495,14 @@
mode_ = UNKNOWN_LOCATION;
}
- // TODO(Joe): Write this.
- // make sure this is possible phsically
- // fix goals to make it possible
- // break into function at some point?
- //
- //FixClawPos(&bottom_claw_goal_, &top_claw_goal_, values);
+ // Limit the goals if both claws have been (mostly) found.
+ if (mode_ != UNKNOWN_LOCATION) {
+ LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
+ }
+
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
- claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, 0, 0;
+ claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
+ bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
double separation = -971;
if (position != nullptr) {
separation = position->top.position - position->bottom.position;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index b2fec3d..344fd78 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -256,6 +256,11 @@
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};
+// Modifies the bottom and top goal such that they are within the limits and
+// their separation isn't too much or little.
+void LimitClawGoal(double *bottom_goal, double *top_goal,
+ const frc971::constants::Values &values);
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index db435ad..e16e3a4 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -224,9 +224,9 @@
EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1, 0));
EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
- v.claw.claw_max_seperation);
+ v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
- v.claw.claw_min_seperation);
+ v.claw.claw_min_separation);
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -269,7 +269,7 @@
".frc971.control_loops.claw_queue_group.status"),
claw_motor_(&claw_queue_group),
claw_motor_plant_(0.4, 0.2),
- min_seperation_(constants::GetValues().claw.claw_min_seperation) {
+ min_seperation_(constants::GetValues().claw.claw_min_separation) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -323,6 +323,81 @@
VerifyNearGoal();
}
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ClawTest, LimitClawGoal) {
+ frc971::constants::Values values;
+ values.claw.claw_min_separation = -2.0;
+ values.claw.claw_max_separation = 1.0;
+ values.claw.upper_claw.lower_limit = -5.0;
+ values.claw.upper_claw.upper_limit = 7.5;
+ values.claw.lower_claw.lower_limit = -5.5;
+ values.claw.lower_claw.upper_limit = 8.0;
+
+ double bottom_goal = 0.0;
+ double top_goal = 0.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(0.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(0.0, top_goal, 1e-4);
+
+ bottom_goal = 0.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-1.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(-3.0, top_goal, 1e-4);
+
+ bottom_goal = 0.0;
+ top_goal = 4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(1.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(2.5, top_goal, 1e-4);
+
+ bottom_goal = -10.0;
+ top_goal = -9.5;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-5.0, top_goal, 1e-4);
+
+ bottom_goal = -20.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-4.5, top_goal, 1e-4);
+
+ bottom_goal = -20.0;
+ top_goal = -4.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-5.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(-4.5, top_goal, 1e-4);
+
+ bottom_goal = -5.0;
+ top_goal = -10.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(-3.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(-5.0, top_goal, 1e-4);
+
+ bottom_goal = 10.0;
+ top_goal = 9.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(8.0, bottom_goal, 1e-4);
+ EXPECT_NEAR(7.0, top_goal, 1e-4);
+
+ bottom_goal = 8.0;
+ top_goal = 9.0;
+
+ LimitClawGoal(&bottom_goal, &top_goal, values);
+ EXPECT_NEAR(6.5, bottom_goal, 1e-4);
+ EXPECT_NEAR(7.5, top_goal, 1e-4);
+}
+
+
class ZeroingClawTest
: public ClawTest,
public ::testing::WithParamInterface< ::std::pair<double, double>> {};
@@ -513,7 +588,8 @@
}
EXPECT_TRUE(kicked);
EXPECT_TRUE(measured);
- EXPECT_EQ(1, capped_count);
+ EXPECT_LE(1, capped_count);
+ EXPECT_GE(3, capped_count);
}
};