/**
   * ******************************
   *
   * <p>Configures motors: - Sets which side is inverted - Configures the secondary motors to be in
   * follow mode - Sets ramp rate
   *
   * <p>******************************
   */
  private void setupMotors() {
    leftMotor1.setInverted(invertLeft);
    leftMotor2.setInverted(invertLeft);
    rightMotor1.setInverted(invertRight);
    rightMotor1.setInverted(invertRight);

    leftMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
    leftMotor2.set(leftMotor1.getDeviceID());
    rightMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
    rightMotor2.set(rightMotor1.getDeviceID());

    leftMotor1.setVoltageRampRate(RAMP_RATE);
    rightMotor1.setVoltageRampRate(RAMP_RATE);
  }
  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    chooser = new SendableChooser();
    chooser.addDefault("Default Auto", defaultAuto);
    chooser.addObject("My Auto", customAuto);
    SmartDashboard.putData("Auto choices", chooser);

    controlMethod = new SendableChooser();
    controlMethod.addDefault("Tank Control", tankControl);
    controlMethod.addObject("Stick Control", stickControl);
    controlMethod.addObject("Clayton Control", claytonControl);
    SmartDashboard.putData("Control method", controlMethod);

    rightMotor = new CANTalon(RIGHT_INDEX);
    rightSlave = new CANTalon(RIGHT_INDEX + 1);
    rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
    rightSlave.set(RIGHT_INDEX);
    rightMotor.setInverted(true);

    leftMotor = new CANTalon(LEFT_INDEX);
    leftSlave = new CANTalon(LEFT_INDEX + 1);
    leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
    leftSlave.set(LEFT_INDEX);

    intakeMotor = new CANTalon(INTAKE_INDEX);
    // Arm motor is currently disabled
    // armMotor = new CANTalon(ARM_INDEX);

    compressor = new Compressor(0);
    compressor.setClosedLoopControl(true);

    intakePneumatic = new DoubleSolenoid(1, 2);
    intakePneumatic.set(DoubleSolenoid.Value.kOff);

    gamepad = new Gamepad(0);
  }
 /**
  * This method inverts the motor direction.
  *
  * @param inverted specifies true to invert motor direction, false otherwise.
  */
 @Override
 public void setInverted(boolean inverted) {
   super.setInverted(inverted);
 } // setInverted
  /** Initializes all actuators */
  public static void init() {
    // TODO: Change ID's
    // Motors
    rightDriveMotor = new CANTalon(3);
    rightDriveMotor =
        initCANTalon(
            rightDriveMotor,
            FeedbackDevice.QuadEncoder,
            RIGHT_DIVE_REVERSE_SENSOR,
            CODES_PER_REV,
            DRIVE_MOTOR_ACCEPTABLE_ERROR,
            RIGHT_DRIVE_KP,
            RIGHT_DRIVE_KI,
            RIGHT_DRIVE_KD);
    rightDriveMotor2 = new CANTalon(0);
    rightDriveMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
    rightDriveMotor2.setInverted(true);
    rightDriveMotor2.set(rightDriveMotor.getDeviceID());

    leftDriveMotor = new CANTalon(2);
    leftDriveMotor =
        initCANTalon(
            leftDriveMotor,
            FeedbackDevice.QuadEncoder,
            LEFT_DIVE_REVERSE_SENSOR,
            CODES_PER_REV,
            DRIVE_MOTOR_ACCEPTABLE_ERROR,
            LEFT_DRIVE_KP,
            LEFT_DRIVE_KI,
            LEFT_DRIVE_KD);
    leftDriveMotor.setInverted(true);
    leftDriveMotor2 = new CANTalon(1);
    leftDriveMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
    leftDriveMotor2.setInverted(true);
    leftDriveMotor2.set(leftDriveMotor.getDeviceID());

    armWinchMotor1 = new VictorSP(4);
    armWinchMotor2 = new VictorSP(5);
    armWinchMotor2.setInverted(true);

    armAngleMotor = new CANTalon(6);
    armAngleMotor =
        initCANTalon(
            armAngleMotor,
            FeedbackDevice.AnalogPot,
            ARM_REVERSE_SENSOR,
            ARM_POT_TURNS_PER_REV,
            ARM_ACCEPTABLE_EROR,
            ARM_ANGLE_KP,
            ARM_ANGLE_KI,
            ARM_ANGLE_KD);
    // TODO: Use string pot with CANTalon

    boulderIntakeMotor = new VictorSP(7);

    catapultMotor = new VictorSP(8);

    // Solenoids
    driveShiftPneumatic = new Solenoid(2);
    winchRatchetPneumatic = new Solenoid(1);
  }