text: /*----------------------------------------------------------------------------*/
  /* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
  /* Open Source Software - may be modified and shared by FRC teams. The code   */
  /* must be accompanied by the FIRST BSD license file in the root directory of */
  /* the project.                                                               */
  /*----------------------------------------------------------------------------*/
  
  #include <frc/Joystick.h>
  #include <frc/TimedRobot.h>
  #include <frc/SmartDashboard/SmartDashboard.h>
  #include "rev/CANSparkMax.h"
  
  /**
   * Before Running:
   * Open shuffleBoard, select File->Load Layout and select the 
   * shuffleboard.json that is in the root directory of this example
   */
  
  /**
   * REV Smart Motion Guide
   * 
   * The SPARK MAX includes a new control mode, REV Smart Motion which is used to 
   * control the position of the motor, and includes a max velocity and max 
   * acceleration parameter to ensure the motor moves in a smooth and predictable 
   * way. This is done by generating a motion profile on the fly in SPARK MAX and 
   * controlling the velocity of the motor to follow this profile.
   * 
   * Since REV Smart Motion uses the velocity to track a profile, there are only 
   * two steps required to configure this mode:
   *    1) Tune a velocity PID loop for the mechanism
   *    2) Configure the smart motion parameters
   * 
   * Tuning the Velocity PID Loop
   * 
   * The most important part of tuning any closed loop control such as the velocity 
   * PID, is to graph the inputs and outputs to understand exactly what is happening. 
   * For tuning the Velocity PID loop, at a minimum we recommend graphing:
   *
   *    1) The velocity of the mechanism (‘Process variable’)
   *    2) The commanded velocity value (‘Setpoint’)
   *    3) The applied output
   *
   * This example will use ShuffleBoard to graph the above parameters. Make sure to
   * load the shuffleboard.json file in the root of this directory to get the full
   * effect of the GUI layout.
   */
  
  class Robot : public frc::TimedRobot {
    static const int deviceID = 9;
    rev::CANSparkMax m_motor{deviceID, rev::CANSparkMax::MotorType::kBrushless};
    rev::SparkMaxPIDController m_pidController = m_motor.GetPIDController();
    rev::SparkMaxRelativeEncoder m_encoder = m_motor.GetEncoder();
  
    // default PID coefficients
    double kP = 5e-5, kI = 1e-6, kD = 0, kIz = 0, kFF = 0.000156, kMaxOutput = 1, kMinOutput = -1;
  
    // default smart motion coefficients
    double kMaxVel = 2000, kMinVel = 0, kMaxAcc = 1500, kAllErr = 0;
  
    // motor max RPM
    const double MaxRPM = 5700;
  
    frc::Joystick m_stick{0};
  
   public:
    void RobotInit() {
      /**
       * The RestoreFactoryDefaults method can be used to reset the configuration parameters
       * in the SPARK MAX to their factory default state. If no argument is passed, these
       * parameters will not persist between power cycles
       */
      m_motor.RestoreFactoryDefaults();
  
      // set PID coefficients
      m_pidController.SetP(kP);
      m_pidController.SetI(kI);
      m_pidController.SetD(kD);
      m_pidController.SetIZone(kIz);
      m_pidController.SetFF(kFF);
      m_pidController.SetOutputRange(kMinOutput, kMaxOutput);
  
      /**
       * Smart Motion coefficients are set on a SparkMaxPIDController object
       * 
       * - SetSmartMotionMaxVelocity() will limit the velocity in RPM of
       * the pid controller in Smart Motion mode
       * - SetSmartMotionMinOutputVelocity() will put a lower bound in
       * RPM of the pid controller in Smart Motion mode
       * - SetSmartMotionMaxAccel() will limit the acceleration in RPM^2
       * of the pid controller in Smart Motion mode
       * - SetSmartMotionAllowedClosedLoopError() will set the max allowed
       * error for the pid controller in Smart Motion mode
       */
      m_pidController.SetSmartMotionMaxVelocity(kMaxVel);
      m_pidController.SetSmartMotionMinOutputVelocity(kMinVel);
      m_pidController.SetSmartMotionMaxAccel(kMaxAcc);
      m_pidController.SetSmartMotionAllowedClosedLoopError(kAllErr);
  
      // display PID coefficients on SmartDashboard
      frc::SmartDashboard::PutNumber("P Gain", kP);
      frc::SmartDashboard::PutNumber("I Gain", kI);
      frc::SmartDashboard::PutNumber("D Gain", kD);
      frc::SmartDashboard::PutNumber("I Zone", kIz);
      frc::SmartDashboard::PutNumber("Feed Forward", kFF);
      frc::SmartDashboard::PutNumber("Max Output", kMaxOutput);
      frc::SmartDashboard::PutNumber("Min Output", kMinOutput);
  
      // display Smart Motion coefficients
      frc::SmartDashboard::PutNumber("Max Velocity", kMaxVel);
      frc::SmartDashboard::PutNumber("Min Velocity", kMinVel);
      frc::SmartDashboard::PutNumber("Max Acceleration", kMaxAcc);
      frc::SmartDashboard::PutNumber("Allowed Closed Loop Error", kAllErr);
      frc::SmartDashboard::PutNumber("Set Position", 0);
      frc::SmartDashboard::PutNumber("Set Velocity", 0);
  
      // button to toggle between velocity and smart motion modes
      frc::SmartDashboard::PutBoolean("Mode", true);
    }
  
    void TeleopPeriodic() {
      // read PID coefficients from SmartDashboard
      double p = frc::SmartDashboard::GetNumber("P Gain", 0);
      double i = frc::SmartDashboard::GetNumber("I Gain", 0);
      double d = frc::SmartDashboard::GetNumber("D Gain", 0);
      double iz = frc::SmartDashboard::GetNumber("I Zone", 0);
      double ff = frc::SmartDashboard::GetNumber("Feed Forward", 0);
      double max = frc::SmartDashboard::GetNumber("Max Output", 0);
      double min = frc::SmartDashboard::GetNumber("Min Output", 0);
      double maxV = frc::SmartDashboard::GetNumber("Max Velocity", 0);
      double minV = frc::SmartDashboard::GetNumber("Min Velocity", 0);
      double maxA = frc::SmartDashboard::GetNumber("Max Acceleration", 0);
      double allE = frc::SmartDashboard::GetNumber("Allowed Closed Loop Error", 0);
  
      // if PID coefficients on SmartDashboard have changed, write new values to controller
      if((p != kP))   { m_pidController.SetP(p); kP = p; }
      if((i != kI))   { m_pidController.SetI(i); kI = i; }
      if((d != kD))   { m_pidController.SetD(d); kD = d; }
      if((iz != kIz)) { m_pidController.SetIZone(iz); kIz = iz; }
      if((ff != kFF)) { m_pidController.SetFF(ff); kFF = ff; }
      if((max != kMaxOutput) || (min != kMinOutput)) { m_pidController.SetOutputRange(min, max); kMinOutput = min; kMaxOutput = max; }
      if((maxV != kMaxVel)) { m_pidController.SetSmartMotionMaxVelocity(maxV); kMaxVel = maxV; }
      if((minV != kMinVel)) { m_pidController.SetSmartMotionMinOutputVelocity(minV); kMinVel = minV; }
      if((maxA != kMaxAcc)) { m_pidController.SetSmartMotionMaxAccel(maxA); kMaxAcc = maxA; }
      if((allE != kAllErr)) { m_pidController.SetSmartMotionAllowedClosedLoopError(allE); allE = kAllErr; }
  
      double SetPoint, ProcessVariable;
      bool mode = frc::SmartDashboard::GetBoolean("Mode", false);
      if(mode) {
        SetPoint = frc::SmartDashboard::GetNumber("Set Velocity", 0);
        m_pidController.SetReference(SetPoint, rev::ControlType::kVelocity);
        ProcessVariable = m_encoder.GetVelocity();
      } else {
        SetPoint = frc::SmartDashboard::GetNumber("Set Position", 0);
        /**
         * As with other PID modes, Smart Motion is set by calling the
         * SetReference method on an existing pid object and setting
         * the control type to kSmartMotion
         */
        m_pidController.SetReference(SetPoint, rev::ControlType::kSmartMotion);
        ProcessVariable = m_encoder.GetPosition();
      }
      
      frc::SmartDashboard::PutNumber("Set Point", SetPoint);
      frc::SmartDashboard::PutNumber("Process Variable", ProcessVariable);
      frc::SmartDashboard::PutNumber("Output", m_motor.GetAppliedOutput());
    }
  };
  
  #ifndef RUNNING_FRC_TESTS
  int main() { return frc::StartRobot<Robot>(); }
  #endif
  