Third robot commit.
All tests pass!
Change-Id: I086248537f075fd06afdfb3e94670eb7646aaf6c
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
new file mode 100644
index 0000000..fed17ab
--- /dev/null
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -0,0 +1,153 @@
+#ifndef Y2016_BOT3_CONTROL_LOOPS_INTAKE_INTAKE_H_
+#define Y2016_BOT3_CONTROL_LOOPS_INTAKE_INTAKE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "frc971/zeroing/zeroing.h"
+#include "y2016_bot3/control_loops/intake/intake.q.h"
+#include "y2016_bot3/control_loops/intake/intake_controls.h"
+
+namespace y2016_bot3 {
+namespace constants {
+static const int kZeroingSampleSize = 200;
+
+// Ratios for our subsystems.
+// TODO(constants): Update these.
+static constexpr double kIntakeEncoderRatio = 18.0 / 72.0 * 15.0 / 48.0;
+
+static constexpr double kIntakePotRatio = 15.0 / 48.0;
+
+// Difference in radians between index pulses.
+static constexpr double kIntakeEncoderIndexDifference =
+ 2.0 * M_PI * kIntakeEncoderRatio;
+
+// Subsystem motion ranges, in whatever units that their respective queues say
+// the use.
+// TODO(constants): Update these.
+static constexpr ::frc971::constants::Range kIntakeRange{// Lower hard stop
+ -0.5,
+ // Upper hard stop
+ 2.90,
+ // Lower soft stop
+ -0.300,
+ // Uppper soft stop
+ 2.725};
+
+struct IntakeZero {
+ double pot_offset = 0.0;
+ ::frc971::constants::ZeroingConstants zeroing{
+ kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.0, 0.3};
+};
+} // namespace constants
+namespace control_loops {
+namespace intake {
+namespace testing {
+class IntakeTest_RespectsRange_Test;
+class IntakeTest_DisabledGoalTest_Test;
+class IntakeTest_IntakeZeroingErrorTest_Test;
+class IntakeTest_UpperHardstopStartup_Test;
+class IntakeTest_DisabledWhileZeroingHigh_Test;
+class IntakeTest_DisabledWhileZeroingLow_Test;
+}
+
+class LimitChecker {
+ public:
+ LimitChecker(IntakeArm *intake) : intake_(intake) {}
+ void UpdateGoal(double intake_angle_goal);
+ private:
+ IntakeArm *intake_;
+};
+
+class Intake : public ::aos::controls::ControlLoop<control_loops::IntakeQueue> {
+ public:
+ explicit Intake(
+ control_loops::IntakeQueue *my_intake = &control_loops::intake_queue);
+
+ static constexpr double kZeroingVoltage = 6.0;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ // This is the large scale movement tolerance.
+ static constexpr double kLooseTolerance = 0.05;
+
+ // This is the small scale movement tolerance.
+ static constexpr double kTightTolerance = 0.03;
+
+ static constexpr double kIntakeUpAngle = M_PI / 2;
+
+ static constexpr double kIntakeDownAngle = 0.0;
+
+ static constexpr double kIntakeMiddleAngle =
+ (kIntakeUpAngle + kIntakeDownAngle) / 2;
+
+ enum State {
+ // Wait for all the filters to be ready before starting the initialization
+ // process.
+ UNINITIALIZED = 0,
+
+ // We now are ready to decide how to zero. Decide what to do once we are
+ // enabled.
+ DISABLED_INITIALIZED = 1,
+
+ ZERO_LOWER_INTAKE = 2,
+
+ ZERO_LIFT_INTAKE = 3,
+ // Run, but limit power to zeroing voltages.
+ SLOW_RUNNING = 12,
+ // Run with full power.
+ RUNNING = 13,
+ // Internal error caused the intake to abort.
+ ESTOP = 16,
+ };
+
+ bool IsRunning() const {
+ return (state_ == SLOW_RUNNING || state_ == RUNNING);
+ }
+
+ 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,
+ control_loops::IntakeQueue::Output *output,
+ control_loops::IntakeQueue::Status *status) override;
+
+ private:
+ friend class testing::IntakeTest_DisabledGoalTest_Test;
+ friend class testing::IntakeTest_IntakeZeroingErrorTest_Test;
+ friend class testing::IntakeTest_RespectsRange_Test;
+ friend class testing::IntakeTest_UpperHardstopStartup_Test;
+ friend class testing::IntakeTest_DisabledWhileZeroingHigh_Test;
+ friend class testing::IntakeTest_DisabledWhileZeroingLow_Test;
+ IntakeArm intake_;
+
+ State state_ = UNINITIALIZED;
+ State last_state_ = UNINITIALIZED;
+
+ float last_intake_angle_ = 0.0;
+ LimitChecker limit_checker_;
+ // Returns true if the profile has finished, and the joint is within the
+ // specified tolerance.
+ bool IsIntakeNear(double tolerance);
+
+ DISALLOW_COPY_AND_ASSIGN(Intake);
+};
+
+
+} // namespace intake
+} // namespace control_loops
+} // namespace y2016_bot3
+
+#endif // Y2016_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_