Java 类edu.wpi.first.wpilibj.CANJaguar 实例源码

private boolean initalizeCANJaguar() {
//      if (lastCANRetry != -1 && System.currentTimeMillis() - lastCANRetry < 200) {
//          canInitialized = false;
//          return false;
//      }
//      lastCANRetry = System.currentTimeMillis();
//      for (int i = 0; i < 3; i++) {
//          try {
                winch = new CANJaguarSafety(RobotMap.SHOOTER_WINCH_CAN_PORT);
                winch.configEncoderCodesPerRev(360);
                winch.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                winch.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
                winch.disableControl();
                initializeWinch();
                if (!winchSafety.isRunning())
                    winchSafety.start();
                canInitialized = true;
                return true;
//          } catch (CANTimeoutException e){
//              BlackBoxProtocol.log("CAN-Jaguar initilization failed: " + e.toString());
//          }
//      }
//      canInitialized = false;
//      return false;
    }

private void configSpeedControl(CANJaguar jag) throws CANTimeoutException {
        final int CPR = 360;
        final double ENCODER_FINAL_POS = 0;
        final double VOLTAGE_RAMP = 40;
//        jag.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
//        jag.setSpeedReference(CANJaguar.SpeedReference.kNone);
//        jag.enableControl();
//        jag.configMaxOutputVoltage(10);//ToDo: 
        // PIDs may be required.  Values here:
        //  http://www.chiefdelphi.com/forums/showthread.php?t=91384
        // and here:
        // http://www.chiefdelphi.com/forums/showthread.php?t=89721
        // neither seem correct.
//        jag.setPID(0.4, .005, 0);
        jag.setPID(0.3, 0.005, 0);
        jag.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        jag.configEncoderCodesPerRev(CPR);
//        jag.setVoltageRampRate(VOLTAGE_RAMP);
        jag.enableControl();

//        System.out.println("Control Mode = " + jag.getControlMode());
    }

public RoboDrive(){

    try {
        //creates the motors
        aLeft = new CANJaguar(RobotMap.LEFT_DRIVE_MOTOR_ALPHA);
        bLeft = new CANJaguar(RobotMap.LEFT_DRIVE_MOTOR_BETA);//, CANJaguar.ControlMode.kSpeed);
        aRight = new CANJaguar(RobotMap.RIGHT_DRIVE_MOTOR_ALPHA);
        bRight = new CANJaguar(RobotMap.RIGHT_DRIVE_MOTOR_BETA);//, CANJaguar.ControlMode.kSpeed);

        //creates the drive train
        roboDrive = new RobotDrive(aLeft, bLeft, aRight, bRight);
        roboDrive.setSafetyEnabled(false);  

    } catch(CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

private BTCanJaguar(int port, boolean isVoltage, BTDebugger debug)
{
    this.isVoltage = isVoltage;
    this.debug = debug;
    this.port = port;
    setVoltageMode(isVoltage);
    try {
        motor = new CANJaguar(port);
    } catch (CANTimeoutException ex) {
        debug.write(Constants.DebugLocation.BTMotor, Constants.Severity.SEVERE, "ERROR: Motor not initiated at port: "+port);
    }
    catch (Exception e)
    {
        debug.write(Constants.DebugLocation.BTMotor, Constants.Severity.SEVERE, "ERROR: Motor not initiated at port: "+port+" Exception: "+e.toString());
    }
}

public void init() {
    if(!hasInit) {
        left = new Victor(HardwareDefines.RIGHT_LOADER_VICTOR);
        try {
            right = new CANJaguar(HardwareDefines.LEFT_LOADER_JAG);
        }
        catch(CANTimeoutException e) {
            System.out.println("Could not instantiate left loader jag!");
        }
        hasInit = true;
    }
    else {
        System.out.println("The loader subsystem has already "
                                        + "been initialized!");
        return;
    }
}

private Puncher() 
{
    try 
    {
        winch = new CANJaguar(RobotMap.WINCH_JAG);
        winch.configPotentiometerTurns(1);
        winch.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
        winch.setSafetyEnabled(false);
        setWinchLimit(.95f);
    } 
    catch (CANTimeoutException ex) 
    {
        reportCANException(ex);
    }
    dogEar = new DoubleSolenoid(RobotMap.DOG_EAR_SOLENOID_DEPLOY, RobotMap.DOG_EAR_SOLENOID_UNDEPLOY);
    dogEar.set(Value.kReverse);
}

public SlingShot()
    {
        try
        {
            shooterPull = new CANJaguar(ReboundRumble.SHOOTER_PULL_CAN_ID);
            shooterPull.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
            shooterPull.configNeutralMode(CANJaguar.NeutralMode.kBrake);
            elevation = new CANJaguar(ReboundRumble.ELEVATION_CAN_ID);
            SetElevationPositionControl();
        } catch (CANTimeoutException ex)
        {
        }
        trigger = new Relay(ReboundRumble.SHOOTER_TRIGGER_RELAY_CHANNEL);
//        loadPosition = new DigitalInput(ReboundRumble.LOAD_POSITON_LIMIT_SWITCH);
//        slingMagnet = new Relay(ReboundRumble.SLING_ELECTROMAGNET_RELAY_CHANNEL);
        //  ballSensor = new DigitalInput(ReboundRumble.SHOOTER_BALL_SENSOR_GPIO_CHANNEL);
        settingForce = false;
    }

public BTMotor(int port, boolean isCan)
{
    isCANBus = isCan;
    if (isCANBus)
    {
        //int maxTries = 0;
        //while(CANMotor == null && maxTries < 10)
        //{
            try{
                CANMotor = new CANJaguar(port);
            }
            catch(Exception CANTimeoutException){
                Log.log("Error initialising CANJaguar");
            }
        //}
        //if (maxTries >= 10)
            //Log.log("CANJaguar(" + port + ") failed to initialize.");
    }
    else
    {
        PWMMotor = new Jaguar(port);
    }
}

public void instanciate(int port, boolean isCan, boolean isVoltage)
{
    isCANBus = isCan;
    portNum = port;
    voltage = isVoltage;
    if (isCANBus) {
        try{
            CANMotor = new CANJaguar(port);
            if (isVoltage)
            {
                setVoltageControlMode();
            }
            successJag = true;
        }
        catch(Exception CANTimeoutException){
            System.out.println("Error initialising CANJaguar at port: " +port);
        }
    }
    else
    {
        PWMMotor = new Jaguar(port);
    }
}

public ShooterMotorControl() {
    super("ShooterControl", Kp, Ki, Kd);
    Encoder topEncoder = new Encoder(RobotMap.topEncoderChannelA, RobotMap.topEncoderChannelB);
    Encoder bottomEncoder = new Encoder(RobotMap.bottomEncoderChannelA, RobotMap.bottomEncoderChannelB);

    try {
                    shooterJagTop = new CANJaguar(RobotMap.shooterJagTopID);
                    shooterJagBottom = new CANJaguar(RobotMap.shooterJagBottomID);
    } catch (CANTimeoutException e) {
        e.printStackTrace();
    }
    // Use these to get going:
    // setSetpoint() -  Sets where the PID controller should move the system
    //                  to
    // enable() - Enables the PID controller.
    enable();
}

private void configSpeedControl(CANJaguar jag) throws CANTimeoutException {
        final int CPR = 360;
        final double ENCODER_FINAL_POS = 0;
        final double VOLTAGE_RAMP = 40;
                jag.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
                        jag.setSpeedReference(CANJaguar.SpeedReference.kNone);
                        jag.enableControl();
                        jag.configMaxOutputVoltage(10);//ToDo: 
        // PIDs may be required.  Values here:
        //  http://www.chiefdelphi.com/forums/showthread.php?t=91384
        // and here:
        // http://www.chiefdelphi.com/forums/showthread.php?t=89721
        // neither seem correct.
//        jag.setPID(0.4, .005, 0);
        /*jag.setPID(1, 0, 0);
        jag.changeControlMode(CANJaguar.ControlMode.kSpeed);
        jag.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        jag.configEncoderCodesPerRev(CPR);
//        jag.setVoltageRampRate(VOLTAGE_RAMP);
        jag.enableControl();*/

//        System.out.println("Control Mode = " + jag.getControlMode());
    }

private void initEncoder(){
    try {
        jaguar.configEncoderCodesPerRev(ticsPerRev); // Config encoder tics per rev

        // Set speed or position reference
        switch(controlMode.value){
            case ControlMode.kPosition_val:
                jaguar.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                break;
            case ControlMode.kSpeed_val:
                jaguar.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
                break;
            default:
                break;
        }
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public DriveTrain(){
    try{
        leftFront = new CANJaguar(1);
        leftRear = new CANJaguar(2);
        rightFront = new CANJaguar(4);
        rightRear = new CANJaguar(3);
    }catch(Exception e){
        System.out.println(e);
        System.out.println(leftFront);
        System.out.println(leftRear);
        System.out.println(rightFront);
        System.out.println(rightRear);
    }

    drive = new RobotDrive(leftFront, leftRear, rightFront, rightRear);
    gyro.reset();
    gyro.setSensitivity(0.007);
}

public void setSpeed(double rpm) throws CANTimeoutException {

    //Right now, we're using voltage control mode 
    // guess voltage to rpm relationship
    double voltage = rpm / 0;

    //Check to see if 'rpm' works
    if ((firstShootingMotor.getControlMode() == CANJaguar.ControlMode.kVoltage)
            && (secondShootingMotor.getControlMode() == CANJaguar.ControlMode.kVoltage)) {
        firstShootingMotor.setX(voltage);
    } else {
        firstShootingMotor.setX(rpm);

        secondShootingMotor.setX(rpm);

    }

}

public Drive(){

    try {

        //Setup the drive motors
        leftFront = new CANJaguar(1);
        rightFront = new CANJaguar(2);
        leftRear = new CANJaguar(3);
        rightRear = new CANJaguar(4);

        //Setup the Drive
        robotDrive = new RobotDrive(leftFront, leftRear, rightFront, rightRear);

    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }

}

public void setPositionReference(CANJaguar.PositionReference ref) {
    if (!connected)
        return;
    for (int i = 0; i < 3; i++) {
        try { jaguar.setPositionReference(ref); connected = true; break; }
        catch (CANTimeoutException ex) { connected = false; }
    }
}

public void changeControlMode(CANJaguar.ControlMode mode) {
    if (!connected)
        return;
    for (int i = 0; i < 3; i++) {
        try { jaguar.changeControlMode(mode); connected = true; break; }
        catch (CANTimeoutException ex) { connected = false; }
    }
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    joystick = new Joystick(JOYSTICK_PORT);
    try {
        rightFront = new CANJaguar(JAG_MOTOR, CANJaguar.ControlMode.kSpeed);
        configSpeedControl(rightFront);
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
        //System.exit(-1);
    }
}

public Catapult() {
    try {
        //creates the motors
        Arm2 = new CANJaguar(RobotMap.CATAPULT_MOTOR);//, CANJaguar.ControlMode.kSpeed); 
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public Arm(){ 
    try {
        //creates the motors
        arm = new CANJaguar(RobotMap.ARM_MOTOR);//, CANJaguar.ControlMode.kSpeed);    

    } catch(CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

private void setVoltageMode(boolean isVoltage)
{
    if (isVoltage)
    {
        try {
            motor.changeControlMode(CANJaguar.ControlMode.kVoltage);
        } catch (Exception ex) {
            debug.write(Constants.DebugLocation.BTMotor, Constants.Severity.SEVERE, "Error Setting Voltage: "+isVoltage+" at port: "+port+" Exception: "+ex.toString());
        }
    }
}

public robot2015preseason()
{
    try
    {
        Drive = new drive();

        driver_left_joystick = new Joystick(1);
        driver_right_joystick = new Joystick(2);

        front_left_jaguar = new CANJaguar(10);
        front_left_jaguar.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
        front_left_jaguar.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        front_left_jaguar.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
        front_left_jaguar.configEncoderCodesPerRev(250);

        back_left_jaguar = new CANJaguar(11);
        back_left_jaguar.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
        back_left_jaguar.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        back_left_jaguar.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
        back_left_jaguar.configEncoderCodesPerRev(250);

        back_right_jaguar = new CANJaguar(12);
        back_right_jaguar.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
        back_right_jaguar.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        back_right_jaguar.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
        back_right_jaguar.configEncoderCodesPerRev(250);

        front_right_jaguar = new CANJaguar(13);
        front_right_jaguar.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
        front_right_jaguar.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        front_right_jaguar.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
        front_right_jaguar.configEncoderCodesPerRev(250);

    }
    catch (CANTimeoutException ex)
    {
        ex.printStackTrace();
    }
}

public void init() {
    try {
       winchJag = new CANJaguar(HardwareDefines.SHOOTER_WINCH_JAG);
       tiltJag = new CANJaguar(HardwareDefines.SHOOTER_TILT_JAG);
       if(HardwareDefines.ENC_COMMS_JAG == 1) {
           // Winch jag closed-loop control setup
           winchJag.changeControlMode(CANJaguar.ControlMode.kPosition);
           winchJag.setPositionReference(
                                 CANJaguar.PositionReference.kQuadEncoder
                                         );
           winchJag.configEncoderCodesPerRev(
                                    HardwareDefines.SHOOTER_LIN_ENC_CLICKS
                                            );

           // Tilt jag closed-loop control setup
           tiltJag.changeControlMode(CANJaguar.ControlMode.kPosition);
           tiltJag.setPositionReference(
                                  CANJaguar.PositionReference.kPotentiometer
                                       );
           tiltJag.configPotentiometerTurns(
                                       HardwareDefines.SHOOTER_POT_TURNS
                                            );
           dogGear = new Puncher();
       }
    }
    catch(CANTimeoutException e) {
       System.out.println(
                           "Cannot instantiate winch and tilt jags!"
                         );
    }
}

public RobotHardware() {
    try {
        // Initializes the Jaguar motor controllers for driving
        CANJaguar frontLeftJaguar = new CANJaguar(Constants.FRONT_LEFT_MOTOR_PORT);
        CANJaguar backLeftJaguar = new CANJaguar(Constants.BACK_LEFT_MOTOR_PORT);
        CANJaguar frontRightJaguar = new CANJaguar(Constants.FRONT_RIGHT_MOTOR_PORT);
        CANJaguar backRightJaguar = new CANJaguar(Constants.BACK_RIGHT_MOTOR_PORT);

        // Initializes the controller for the four driving motors and reverses two of them
        driveControl = new RobotDrive(frontLeftJaguar, backLeftJaguar, frontRightJaguar, backRightJaguar);
        driveControl.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        driveControl.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }

    // Initialize joysticks
    driverJoystick = new Joystick(Constants.DRIVER_JOYSTICK_PORT);
    secondJoystick = new Joystick(Constants.SECOND_JOYSTICK_PORT);

    sonar = new AnalogChannel(Constants.ANALOG_SONAR_PORT);

    compressor = new Compressor(Constants.COMPRESSOR_PRESSURE_SWITCH_CHANNEL, Constants.COMPRESSOR_RELAY_CHANNEL);
    doubleSol = new DoubleSolenoid(Constants.DOUBLE_SOLENOID_FORWARD_CHANNEL, Constants.DOUBLE_SOLENOID_REVERSE_CHANNEL);
    sol1 = new Solenoid(Constants.SOLENOID_PORT_1);
    sol2 = new Solenoid(Constants.SOLENOID_PORT_2);
}

private TopArm()
{
    solenoid = new DoubleSolenoid(RobotMap.TOP_ARM_SOLENOID_DEPLOY, RobotMap.TOP_ARM_SOLENOID_UNDEPLOY);
    try 
    {
        jag = new CANJaguar(RobotMap.TOP_ARM_JAG);
    } catch (CANTimeoutException ex) 
    {
        reportCANException(ex);
    }
}

public void initCAN()
{
    try
    {
        winch.configPotentiometerTurns(1);
        winch.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
        winch.setSafetyEnabled(false);
    }
    catch(CANTimeoutException ex)
    {
        reportCANException(ex);
    }
}

private ShooterRotator()
{
    pot = new ShooterPotentiometer(2);
    SmartDashboard.putNumber("Shooter Rotate Distance", 0);
    try 
    {
        rotationJag = new CANJaguar(RobotMap.SHOOTER_ROTATION_JAG);
        rotationJag.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    } 
    catch (CANTimeoutException ex) 
    {
        reportCANException(ex);
    }
}

public void initCAN()
{
    try 
    {
        rotationJag.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    } 
    catch (CANTimeoutException ex) 
    {
        reportCANException(ex);
    }
}

public final void canInit() throws CANTimeoutException {
    leftJag = new CANJaguar(4,CANJaguar.ControlMode.kPercentVbus);
    leftJag.setVoltageRampRate(524);
    leftJag.configNeutralMode(CANJaguar.NeutralMode.kCoast);
    rightJag = new CANJaguar(3, CANJaguar.ControlMode.kPercentVbus);
    rightJag.setVoltageRampRate(524);
    leftJag.configNeutralMode(CANJaguar.NeutralMode.kCoast);
}

public Shooter() throws CANTimeoutException{
    try{
       shooterJag = new CANJaguar(RobotMap.shooterJagID); 
       LiveWindow.addActuator("Shooter", "jag", shooterJag);
    } catch(CANTimeoutException e){
        throw e;
    }

}

public FrontLifter() throws CANTimeoutException {
    try {
        liftJag = new CANJaguar(RobotMap.frontLiftJagID);
        LiveWindow.addActuator("Front Lift", "liftJag", liftJag);
    } catch (CANTimeoutException ex) {
        throw ex;
    }

}

SteeringUnit(int steeringCanID, int leftCanID, int rightCanID)  throws CANTimeoutException {
    middle = new CANJaguar(steeringCanID, CANJaguar.ControlMode.kPosition);
    //middle = new CANJaguar(steeringCanID, CANJaguar.ControlMode.kPercentVbus);
    middle.configMaxOutputVoltage(ReboundRumble.MAX_MOTOR_VOLTAGE);
    middle.configPotentiometerTurns(1);
    middle.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
    middle.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    middle.setPID(kProportional, kIntegral, kDifferential);
    left = new Wheel(leftCanID);
    right = new Wheel(rightCanID);
    setpoint = 0.0;
}

/**
 * Puts the elevation Jaguar in position control mode using the angle sensor
 * to set the position.
 *
 * @throws CANTimeoutException
 */
protected void SetElevationPositionControl() throws CANTimeoutException
{
    elevation.changeControlMode(CANJaguar.ControlMode.kPosition);
    elevation.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
    elevation.setPID(ELEVATION_PID_PROPORTIONAL, ELEVATION_PID_INTEGRAL, ELEVATION_PID_DERIVATIVE);
    elevation.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    isElevationPIDControlled = true;
}

/**
 * Puts the elevation Jaguar in percent voltage mode so the joystick can be
 * used to set the elevation
 */
protected void SetElevationPercentVbusControl() throws CANTimeoutException
{
    elevation.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
    elevation.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    isElevationPIDControlled = false;
}

public void ClimberA(){
    try{
        leftGear = new CANJaguar(LEFT_GEAR_PORT);
        leftSpeed = new CANJaguar(LEFT_SPEED_PORT);
        rightGear = new CANJaguar(RIGHT_GEAR_PORT);
        rightSpeed = new CANJaguar(RIGHT_SPEED_PORT);
    }
    catch (CANTimeoutException e){}
}

public DriveTrain()
{
    try{
        left = new CANJaguar(LEFT_JAG_PORT);
        right = new CANJaguar(RIGHT_JAG_PORT);
    }
    catch (CANTimeoutException e)
    {

    }
}

private void setVoltageControlMode()
{
    try {
        CANMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
        CANMotor.enableControl();
    }
    catch(Exception e){}
}

public DriveTrain()
{
    try{
        left = new CANJaguar(LEFT_JAG_PORT);
        right = new CANJaguar(RIGHT_JAG_PORT);
    }
    catch (CANTimeoutException e)
    {

    }
}

/**  The control mode needs to be set in the constructor for the speed mode to work:
     *  http://www.chiefdelphi.com/forums/showthread.php?t=89721
     * 
     * Setting the "changeControlMode" after the constructor does not seem to work.
     * 
     */
    public Chassis() {
        try {
            System.out.println("Chassis Construtor started");
            rightFront = new CANJaguar(RobotMap.JAG_RIGHT_FRONT_MOTOR, CANJaguar.ControlMode.kSpeed);
            configSpeedControl(rightFront);
            System.out.println("JAG Right Front works, " + RobotMap.JAG_RIGHT_FRONT_MOTOR);
            rightRear = new CANJaguar(RobotMap.JAG_RIGHT_REAR_MOTOR, CANJaguar.ControlMode.kSpeed);
            configSpeedControl(rightRear);
            System.out.println("JAG Right Back works, " + RobotMap.JAG_RIGHT_REAR_MOTOR);
            leftFront = new CANJaguar(RobotMap.JAG_LEFT_FRONT_MOTOR, CANJaguar.ControlMode.kSpeed);
            configSpeedControl(leftFront);
            System.out.println("JAG Left Front works, " + RobotMap.JAG_LEFT_FRONT_MOTOR);
            leftRear = new CANJaguar(RobotMap.JAG_LEFT_REAR_MOTOR, CANJaguar.ControlMode.kSpeed);
            configSpeedControl(leftRear);
            System.out.println("JAG Left Back works, " + RobotMap.JAG_LEFT_REAR_MOTOR);

        } catch (CANTimeoutException ex) {
            System.out.println("Chassis constructor CANTimeoutException: ");
            ex.printStackTrace();
            //System.exit(-1);
        }

        drive = new RobotDrive(leftFront, leftRear, rightFront, rightRear);
        drive.setInvertedMotor(MotorType.kFrontLeft, true);//Left front motor normally opposite
        drive.setMaxOutput(2);//TODO: Fix the magic numbers
//  drive = new RobotDrive(leftRear, leftRear, leftRear, leftRear);
        drive.setSafetyEnabled(false);
    }

public Shooter() throws CANTimeoutException {
    motor = new CANJaguar(Parameters.WheelOneCANJaguarCANID, CANJaguar.ControlMode.kPercentVbus);
    motor.configMaxOutputVoltage(Parameters.MaxMotorOutputVoltage);
    motor.configNeutralMode(CANJaguar.NeutralMode.kBrake);
    discSensor = new DigitalInput(Parameters.DiscInShooterGPIOChannel);
    shooterCockedSensor = new DigitalInput(Parameters.ShooterIsCockedGPIOChannel);
    shooterRetractedSensor = new DigitalInput(Parameters.ShooterIsRetractedGPIOChannel);

}