Fixed Austin's comments.
Squashed 'Changed fender shot to long without IMU' and
fixed Austin's comments.
Change-Id: Id81f4f8d35bb399d33bb74bd3d4e895603817711
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index 5320cd5..d3d288b 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -33,28 +33,6 @@
.lpNorm<Eigen::Infinity>() < tolerance);
}
-double Intake::MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance) {
- return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
-}
-
-double Intake::MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance) {
- // There are 3 interesting places to move to.
- const double small_negative_move = current_angle - move_distance;
- const double small_positive_move = current_angle + move_distance;
- // And the reference angle.
-
- // Move the the highest one that is below reference_angle.
- if (small_negative_move > reference_angle) {
- return reference_angle;
- } else if (small_positive_move > reference_angle) {
- return small_negative_move;
- } else {
- return small_positive_move;
- }
-}
-
void Intake::RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
control_loops::IntakeQueue::Output *output,
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index fed17ab..a5b4da6 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -54,6 +54,7 @@
class IntakeTest_DisabledWhileZeroingLow_Test;
}
+// TODO(Adam): Implement this class and delete it from here.
class LimitChecker {
public:
LimitChecker(IntakeArm *intake) : intake_(intake) {}
@@ -109,15 +110,6 @@
State state() const { return state_; }
- // Returns the value to move the joint to such that it will stay below
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance);
- // Returns the value to move the joint to such that it will stay above
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance);
-
protected:
void RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
diff --git a/y2016_bot3/control_loops/intake/intake.q b/y2016_bot3/control_loops/intake/intake.q
index da3edbb..b6b0bdc 100644
--- a/y2016_bot3/control_loops/intake/intake.q
+++ b/y2016_bot3/control_loops/intake/intake.q
@@ -80,9 +80,6 @@
// Estimated angle and angular velocitie of the intake.
JointState intake;
-
- // Is the intake collided?
- bool is_collided;
};
message Position {
diff --git a/y2016_bot3/control_loops/intake/intake_lib_test.cc b/y2016_bot3/control_loops/intake/intake_lib_test.cc
index 43232aa..4048a19 100644
--- a/y2016_bot3/control_loops/intake/intake_lib_test.cc
+++ b/y2016_bot3/control_loops/intake/intake_lib_test.cc
@@ -443,14 +443,6 @@
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
-// Tests that MoveButKeepBelow returns sane values.
-TEST_F(IntakeTest, MoveButKeepBelowTest) {
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 10.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 2.0, 1.0));
- EXPECT_EQ(0.0, Intake::MoveButKeepBelow(1.0, 1.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 0.0, 1.0));
-}
-
// Tests that the integrators works.
TEST_F(IntakeTest, IntegratorTest) {
intake_plant_.InitializeIntakePosition(