frc971/control_loops/angle_adjust.h

#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_

#include <memory>
#include <array>

#include "aos/common/control_loop/ControlLoop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/angle_adjust_motor.q.h"
#include "frc971/control_loops/angle_adjust_motor_plant.h"
#include "frc971/control_loops/hall_effect_loop.h"

namespace frc971 {
namespace control_loops {

// Allows the control loop to add the tests to access private members.
namespace testing {
class AngleAdjustTest_RezeroWithMissingPos_Test;
class AngleAdjustTest_DisableGoesUninitialized_Test;
}

class AngleAdjustMotor
  : public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
 public:
  explicit AngleAdjustMotor(
      control_loops::AngleAdjustLoop *my_angle_adjust = 
                                      &control_loops::angle_adjust);
 protected:
  virtual void RunIteration(
    const ::aos::control_loops::Goal *goal,
    const control_loops::AngleAdjustLoop::Position *position,
    ::aos::control_loops::Output *output,
    ::aos::control_loops::Status *status);
 
 private:
  // Allows the testing code to access some of private members.
  friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
  friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;

  // Whether or not we are testing it currently;
  // turns on/off recording data.
  static const bool testing = true;

  static constexpr double dt = 0.01;

  // Fetches and locally caches the latest set of constants. 
  // Returns whether it succeeded or not.
  bool FetchConstants();

  // Clips the goal to be inside the limits and returns the clipped goal.
  // Requires the constants to have already been fetched.
  double ClipGoal(double goal) const;
  // Limits the voltage depending whether the angle adjust has been zeroed or
  // is out of range to make it safer to use. May or may not be necessary.
  double LimitVoltage(double absolute_position, double voltage) const;

  // Hall Effect class for dealing with hall effect sensors.
  HallEffectLoop<2> hall_effect_;

  // Missed position packet count.
  int error_count_;

  // Local cache of angle adjust geometry constants.
  double horizontal_lower_limit_;
  double horizontal_upper_limit_;
  ::std::array<double, 2> horizontal_hall_effect_stop_angle_;
  double horizontal_zeroing_speed_;

  // Stores information from the queue about the hall effect sensors.
  // Because we don't have arrays in the queue, we needs these to convert
  // to a more convenient format.
  ::std::array<bool, 2> hall_effect_sensors_;
  ::std::array<double, 2> calibration_values_;

  // Time for recording data.
  double time_;

  DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
};

}  // namespace control_loops
}  // namespace frc971

#endif  // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_