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

protected void initialize() {
    setTimeout(5);
    Robot.intakeLauncher.aimMotor.disableControl();
    Robot.intakeLauncher.aimMotor.changeControlMode(CANTalon.TalonControlMode.Position);
    Robot.intakeLauncher.aimMotor.setPID(0.1, 0.01, 1);
 //   Robot.intakeLauncher.aimMotor.config
    System.out.println("Launcher go to " + myPosition + " command");
    switch (myPosition) {
        case ANGLE: 
            Robot.intakeLauncher.launcherJumpToAngle(myDegrees); //Known to be nonfunctional 
            break;
        case NEUTRAL:
            Robot.intakeLauncher.launcherSetNeutralPosition();
            break;
        case TRAVEL:
            Robot.intakeLauncher.launcherSetTravelPosition();
            break;
        case INTAKE:
            Robot.intakeLauncher.launcherSetIntakeHeightPosition();
            break;
        default:
            break;
    }
    Robot.intakeLauncher.aimMotor.enableControl();
    Robot.intakeLauncher.moveToSetPoint();
}

public Harvester() {        

    //TODO: sync left/right motors
    tiltRight = new CANTalon(RobotMap.HARVESTER_AIMING_RIGHT);
    tiltLeft = new CANTalon(RobotMap.HARVESTER_AIMING_LEFT);

    //tiltLeft.setVoltageRampRate(5); // this might be a good way to solve our ramp rate issue IE smooth out the jerkiness
    //tiltRight.setVoltageRampRate(5); // this might be a good way to solve our ramp rate issue IE smooth out the jerkiness

    limitLeft = new DigitalInput(RobotMap.HARVESTER_LEFT_BUMP_BOTTOM);
    limitRight = new DigitalInput(RobotMap.HARVESTER_RIGHT_BUMP_BOTTOM);
    limitLeftTop = new DigitalInput(RobotMap.HARVESTER_LEFT_BUMP_TOP);
    limitRightTop = new DigitalInput(RobotMap.HARVESTER_RIGHT_BUMP_TOP);

    leftPot = new ZeroablePotentiometer(RobotMap.LEFT_AIM_POT, 250);
    rightPot = new ZeroablePotentiometer(RobotMap.RIGHT_AIM_POT, 250);
    leftPot.setInverted(true);
}

public static void initialize()
{
    if (!initialized) {
        mFrontLeft = new CANTalon(LEFT_FRONT_TALON_ID);
        mBackLeft = new CANTalon(LEFT_REAR_TALON_ID);
        mFrontRight = new CANTalon(RIGHT_FRONT_TALON_ID);
        mBackRight = new CANTalon(RIGHT_REAR_TALON_ID);

        drive = new RobotDrive(mFrontLeft, mBackLeft, mFrontRight, mBackRight);

        drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);   

        leftStick = new Joystick(LEFT_JOYSTICK_ID);
        rightStick = new Joystick(RIGHT_JOYSTICK_ID);

        initialized = true;
    }
}

public DriveTrainSubsystem()
{
    lastLeft = 0.0D;
    lastRight = 0.0D;

    leftTalon0 = new CANTalon(RobotMap.Motor.LEFT_TALON_0);
    leftTalon1 = new CANTalon(RobotMap.Motor.LEFT_TALON_1);
    rightTalon0 = new CANTalon(RobotMap.Motor.RIGHT_TALON_0);
    rightTalon1 = new CANTalon(RobotMap.Motor.RIGHT_TALON_1);

    drive = new RobotDrive(leftTalon0, leftTalon1, rightTalon0, rightTalon1);
    drive.setExpiration(0.1D);

    setTalonSettings(leftTalon0);
    setTalonSettings(leftTalon1);
    setTalonSettings(rightTalon0);
    setTalonSettings(rightTalon1);
}

public DrivetrainPID() {
    super(RobotMap.DRIVE_TURN_P, RobotMap.DRIVE_TURN_I, RobotMap.DRIVE_TURN_D);
    frontLeft = new CANTalon(RobotMap.FRONT_LEFT_MOTOR);
frontRight = new CANTalon(RobotMap.FRONT_RIGHT_MOTOR);
rearLeft = new CANTalon(RobotMap.REAR_LEFT_MOTOR);
rearRight = new CANTalon(RobotMap.REAR_RIGHT_MOTOR);

frontLeft.enableControl();
frontRight.enableControl();
rearLeft.enableControl();
rearRight.enableControl();

oneDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
SmartDashboard.putNumber("Front right", 0);
SmartDashboard.putNumber("Front left", 0);
  }

/**
 * This initializes the variables for the distance calculator.
 */
protected void initialize() {
    desiredTicksValue = new ArrayList<Double>();

    double ticksToMove = inputDistance * 12 * 1000 / (6 * Math.PI);

    System.out.println("Input distance: " + inputDistance + " ft, ticks to move: " + ticksToMove);
    System.out.println("*************READY TO DRIVE**************");
    for (int i = 0; i < motors.size(); i++) {
        CANTalon motor = motors.get(i);

        double startingTickValue = motor.getPosition();
        double endValue = startingTickValue + ticksToMove;
        if (i == 1 || i == 3) {
            // right motors are inverted and we are reversing the back motors
            endValue = startingTickValue - ticksToMove;
        }

        System.out.println("Motor " + i + ": starting position " + startingTickValue + ", desired position " + endValue);
        desiredTicksValue.add(endValue);
    }
}

/**
 * This initializes the variables for the distance calculator.
 */
protected void initialize() {
    desiredTicksValue = new ArrayList<Double>();

    double ticksToMove = inputDistance * 12 * 1000 / (6 * Math.PI);

    Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Input distance: " + inputDistance + " ft, ticks to move: " + ticksToMove);

    for (int i = 0; i < motors.size(); i++) {
        CANTalon motor = motors.get(i);

        double startingTickValue = motor.getPosition();
        double endValue = startingTickValue + ticksToMove;
        if (i >= 2) {
            // right motors are inverted
            endValue = startingTickValue - ticksToMove;
        }
        System.out.println("!!!!!!!!!" + inputSpeed + "!!!!!!!!!!!");
        Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Motor " + i + ": starting position " + startingTickValue + ", desired position " + endValue);

        desiredTicksValue.add(endValue);
    }
}

/**
 * @param newLevel The level amt to move up/down
 * @return The amount of meters high that level is.
 */
public static double moveToLevel(int newLevel)
{
    if (RobotMap.forkliftMotor.getControlMode() != CANTalon.ControlMode.Position)
        return (RobotMap.forkliftMotor.getPosition());

    if(newLevel >= metersToLevel.length)
        newLevel = metersToLevel.length - 1;
    else if(newLevel < 0)
        newLevel = 0;

    double target = metersToLevel[newLevel] * pulsePerMeter;

    moveToPosition(target);

    return (target);
}

/**
 * Moves the lift up or down.
 * @param pos The position to move to, in encoder ticks. Bottom is 0, top is (TODO: Find Top).
 */
public static void moveToPosition(double pos) {

    if (RobotMap.forkliftMotor.getControlMode() != CANTalon.ControlMode.Position)
        return;

    // Make sure not above the top stop
    if(pos > maxValue) 
        pos = maxValue;

    // Make sure not below the bottom stop
    if(pos < 0) 
        pos = 0;

    RobotMap.forkliftMotor.enableControl();
    RobotMap.forkliftMotor.set(pos);
}

/**
 * Creates a {@link CANTalonEncoderWheelController} that uses the specified
 * PID constants, max speed and distance per encoder pulse. It also takes a
 * list of motors that form this wheel controller. The first Talon SRX
 * serves as the master and it does the PID calculation, while the rest
 * follow its output. This means that the encoder should be attached to the
 * first Talon in the list.
 * 
 * @param speedPIDConstants the constants for the speed PID controller
 * @param maxSpeed the maximum speed the wheel can turn
 * @param encoderDistancePerPulse the distance the wheel moves per encoder
 *            pulse
 * @param pdpPorts the PDP ports the motors are connected to
 * @param motors the list of motors that make up this wheel controller
 */
public CANTalonEncoderWheelController(PIDConstants speedPIDConstants,
        double maxSpeed, double encoderDistancePerPulse, int[] pdpPorts, CANTalon... motors) {
    super(pdpPorts, motors);

    this.maxSpeed = maxSpeed;
    this.encoderDistancePerPulse = encoderDistancePerPulse;

    // Master controller does all the controlling while the others are
    // slaves.
    masterMotor = motors[0];
    // Set all other motors as slaves
    for (int i = 1; i < motors.length; i++) {
        motors[i].changeControlMode(CANTalon.ControlMode.Follower);
        // Setup the motors to follow the master
        motors[i].set(motors[0].getDeviceID());
    }

    masterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);

    // Speed profile (using 0 ramp value to disable ramping)
    masterMotor.setPID(speedPIDConstants.p, speedPIDConstants.i, speedPIDConstants.d,
            speedPIDConstants.f, 0, 0, 0);
}

public static void initialize(){
    climbingWinch = new CANTalon(8); //this value is guessed as of 3/21
    release = new Servo(2); //this value is guessed as of 3/21
    motorLatch = new Servo(3); //this value is guessed as of 3/21
    climbingWinch.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
    climbingWinch.enableBrakeMode(true);
}

public static void upSpeedMode()
{
    RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
    loadPreferences();
    RobotMap.winchMotor.setPID(UP_PID_P, UP_PID_I, UP_PID_D);
    RobotMap.winchMotor.setAllowableClosedLoopErr(0);
}

public static void downSpeedMode()
{
    RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
    loadPreferences();
    RobotMap.winchMotor.setPID(DOWN_PID_P, DOWN_PID_I, DOWN_PID_D);
    RobotMap.winchMotor.setAllowableClosedLoopErr(0);
}

public void reset() {
    for (int i = 0; i < motors.length; i++) {
        if (motors[i] == null) {
            continue;
        }
        if (motors[i] instanceof CANTalon) {
            ((CANTalon) motors[i]).delete();
        } else if (motors[i] instanceof Victor) {
            ((Victor) motors[i]).free();
        }
    }
    motors = new SpeedController[64];

    for (int i = 0; i < solenoids.length; i++) {
        if (solenoids[i] == null) {
            continue;
        }
        solenoids[i].free();
    }
    solenoids = new Solenoid[64];

    for (int i = 0; i < relays.length; i++) {
        if (relays[i] == null) {
            continue;
        }
        relays[i].free();
    }
    relays = new Relay[64];

    for (int i = 0; i < analogInputs.length; i++) {
        if (analogInputs[i] == null) {
            continue;
        }
        analogInputs[i].free();
    }
    analogInputs = new AnalogInput[64];

    if (compressor != null)
    compressor.free();
}

protected void initialize() {
    if (!m_isRunning){
        m_initialHeading = Robot.driveTrain.getCurrentHeading(); 
        m_isRunning = true;
    }
    RobotMap.leftMasterMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
    RobotMap.rightMasterMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
    System.out.println("DriveStraightCommand:init");
    Robot.driveTrain.init();
}

public HarvesterRollers() {
        shooterWheelA = new CANTalon(RobotMap.HARVESTER_SHOOTER_WHEEL_A);//Construct shooter A as CANTalon, this should have encoder into it
        shooterWheelB = new CANTalon(RobotMap.HARVESTER_SHOOTER_WHEEL_B);//Construct shooter B as CANTalon
//      shooterWheelB.changeControlMode(CANTalon.TalonControlMode.Follower);//turn shooter motor B to a slave
//      shooterWheelB.set(shooterWheelA.get());//slave shooter motor B to shooter motor A

        harvesterBallControl = new CANTalon(RobotMap.HARVESTER_BALL_CONTROL);

        ballSensorLeft = new DigitalInput(RobotMap.BALL_SENSOR_LEFT);
        ballSensorRight = new DigitalInput(RobotMap.BALL_SENSOR_RIGHT);
        pdp = new PowerDistributionPanel();
    }

static public void brakeAndVoltage(CANTalon t) {
    t.setPosition(0);
    t.enableBrakeMode(Constants.brakeMode);
    if (Constants.useVoltageRamp) {
        t.setVoltageRampRate(Constants.voltageRampRate);
    } else {
        t.setVoltageRampRate(0);
    }
}

@Override
public void stop() {

    RobotMap.arm1.disableControl();
    RobotMap.brakeAndVoltage(RobotMap.arm1);
    RobotMap.arm1.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Absolute);
    RobotMap.arm1.reverseSensor(true);
    RobotMap.arm1.setF(Constants.armF);
    RobotMap.arm1.setPID(Constants.armP, Constants.armI, Constants.armD);
    RobotMap.arm1.changeControlMode(CANTalon.TalonControlMode.Position);
    RobotMap.arm1.set(RobotMap.arm1.getPosition());
    RobotMap.arm1.enableControl();
}

@Override
public void init() {
    RobotMap.armLock = this;
    double range = VisionHandler.getInstance().getRange();
    if (RobotMap.arm1.getControlMode() == CANTalon.TalonControlMode.Position) {
        RobotMap.arm1.set(RobotMap.arm1.getSetpoint() + angle + (Constants.rangeAdjustment * range));
    }

}

@Override
public void init() {
    if (RobotMap.arm1.getControlMode() == CANTalon.TalonControlMode.Position) {
        RobotMap.arm1.setSetpoint(RobotMap.arm1.getSetpoint() + amount);
    }
    timer.reset();
    timer.start();
}

@Override
public void update() {
    if (RobotMap.arm1.getControlMode() == CANTalon.TalonControlMode.Position
            && timer.get() > 0.1) {
        RobotMap.arm1.setSetpoint(RobotMap.arm1.getSetpoint() + amount);
        timer.reset();
        timer.start();
    }
}

public Robot() {
    //make objects for drive train, joystick, and gyro
    myRobot = new RobotDrive(new CANTalon(leftMotorChannel), new CANTalon(leftRearMotorChannel),
      new CANTalon(rightMotorChannel), new CANTalon(rightRearMotorChannel));
    myRobot.setInvertedMotor(MotorType.kFrontLeft, true); // invert the left side motors
    myRobot.setInvertedMotor(MotorType.kRearLeft, true); // you may need to change or remove this to match your robot

    joystick = new Joystick(0);
    gyro = new AnalogGyro(gyroChannel);
}

public Robot()
{
    //make objects for the drive train, gyro, and joystick
    myRobot = new RobotDrive(new CANTalon(leftMotorChannel), new CANTalon(
      leftRearMotorChannel), new CANTalon(rightMotorChannel),
      new CANTalon(rightRearMotorChannel));
    gyro = new AnalogGyro(gyroChannel);
    joystick = new Joystick(joystickChannel);
}

/**
 * Constructor
 */
private Turret() {
    winch = new CANTalon(TURRET);
    winch.changeControlMode(PERCENT_VBUS_MODE);
    winch.enableBrakeMode(BRAKE_MODE);
    lower = new DigitalInput(0);
    upper = new DigitalInput(1);
    // winch.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
}

/**
 * Constructor
 */
private Drivetrain() {
    left = new CANTalon(LEFT);
    leftSlave = new CANTalon(LEFT_SLAVE);
    right = new CANTalon(RIGHT);
    rightSlave = new CANTalon(RIGHT_SLAVE);
    left.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    right.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    setTalonDefaults();
    //shifter.set(DoubleSolenoid.Value.kForward);
}

public FlywheelEncoderSubsystem()
{
    flywheelTalon = new CANTalon(RobotMap.Motor.FLYWHEEL_TALON_0);
    feederTalon1 = new CANTalon(RobotMap.Motor.FEEDER_TALON_0);
    feederTalon2 = new CANTalon(RobotMap.Motor.FEEDER_TALON_1);
    flywheelTalon.set(1);
    feederTalon1.set(1);
    feederTalon2.set(-1);
}

public ShooterSubsystem()
{
     flywheelTalon = new CANTalon(RobotMap.Motor.FLYWHEEL_TALON_0);
     feederTalon0 = new CANTalon(RobotMap.Motor.FEEDER_TALON_0);
     feederTalon1 = new CANTalon(RobotMap.Motor.FEEDER_TALON_1);
     feederTalon2 = new CANTalon(RobotMap.Motor.FEEDER_TALON_2);
}

private void setTalonSettings(CANTalon talon)
{
    talon.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    talon.configEncoderCodesPerRev(256);
    talon.reverseSensor(false);
    talon.configNominalOutputVoltage(0.0D, -0.0D);
    talon.configPeakOutputVoltage(12.0D, -12.0D);
}

public TankDrive() {
    super();
    leftMotor1 = new CANTalon(RobotMap.DRIVE_LEFT_MOTOR1);
    leftMotor2 = new CANTalon(RobotMap.DRIVE_LEFT_MOTOR2);
    rightMotor1 = new CANTalon(RobotMap.DRIVE_RIGHT_MOTOR1);
    rightMotor2 = new CANTalon(RobotMap.DRIVE_RIGHT_MOTOR2);
    leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODERA, RobotMap.DRIVE_LEFT_ENCODERB, false);// would spin clockwise or +; T=-, f=-?
    rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODERA, RobotMap.DRIVE_RIGHT_ENCODERB,
            /*RobotMap.ROBOT_TYPE == RobotMap.COMP_BOT_NUM*/ false);// would spin counter-clockwise or -; boolean reverses direction
    leftEncoder.setDistancePerPulse(RobotMap.DRIVE_RATIO);
    rightEncoder.setDistancePerPulse(RobotMap.DRIVE_RATIO);
}

public FlyWheels() {
    left = new CANTalon(RobotMap.LEFT_SHOOTER_MOTOR);
    right = new CANTalon(RobotMap.RIGHT_SHOOTER_MOTOR);
    left.enableBrakeMode(true);
    right.enableBrakeMode(true);

    right.changeControlMode(CANTalon.TalonControlMode.Follower);
    right.reverseOutput(true);

    pusher = new DoubleSolenoid(RobotMap.PUSHER_OUT_PORT, RobotMap.PUSHER_IN_PORT);

    wheelSwitch = new DigitalInput(RobotMap.FLYWHEEL_ENCODER_SWITCH);
    counter = new Counter(wheelSwitch);
}

public Pitch(){
    pitch = new CANTalon(RobotMap.TARGETING_PITCH_MOTOR);
    pitch.reset();
    pitch.enableControl();
    encoderMin = pitch.getEncPosition();
    encoderMax = encoderMin + RobotMap.ENCODER_RANGE;
    SmartDashboard.putNumber("Encoder minimum", encoderMin);
    SmartDashboard.putNumber("Encoder maximum", encoderMax);
}

public Yaw(){
    yaw = new CANTalon(RobotMap.TARGETING_YAW_MOTOR);
    yaw.enableControl();
    yawZero = yaw.getEncPosition();

    yaw.changeControlMode(TalonControlMode.Position);
    yaw.setPID(RobotMap.YAW_P, RobotMap.YAW_I, RobotMap.YAW_D);
}

public Drivetrain() {
    frontLeft = new CANTalon(RobotMap.FRONT_LEFT_MOTOR);
    frontRight = new CANTalon(RobotMap.FRONT_RIGHT_MOTOR);
    rearLeft = new CANTalon(RobotMap.REAR_LEFT_MOTOR);
    rearRight = new CANTalon(RobotMap.REAR_RIGHT_MOTOR);

    frontLeft.enableControl();
    frontRight.enableControl();
    rearLeft.enableControl();
    rearRight.enableControl();

    frontDrive = new RobotDrive(rearLeft, rearRight);
    rearDrive = new RobotDrive(rearLeft, rearRight);
}

public Robot() {
    motor = new CANTalon(1); // Initialize the CanTalonSRX on device 1.

    // This sets the mode of the m_motor. The options are:
    // PercentVbus: basic throttle; no closed-loop.
    // Current: Runs the motor with the specified current if possible.
    // Speed: Runs a PID control loop to keep the motor going at a constant
    //   speed using the specified sensor.
    // Position: Runs a PID control loop to move the motor to a specified move
    //   the motor to a specified sensor position.
    // Voltage: Runs the m_motor at a constant voltage, if possible.
    // Follower: The m_motor will run at the same throttle as the specified
    //   other talon.
    motor.changeControlMode(CANTalon.ControlMode.Position);
    // This command allows you to specify which feedback device to use when doing
    // closed-loop control. The options are:
    // AnalogPot: Basic analog potentiometer
    // QuadEncoder: Quadrature Encoder
    // AnalogEncoder: Analog Encoder
    // EncRising: Counts the rising edges of the QuadA pin (allows use of a
    //   non-quadrature encoder)
    // EncFalling: Same as EncRising, but counts on falling edges.
    motor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot);
    // This sets the basic P, I , and D values (F, Izone, and rampRate can also
    //   be set, but are ignored here).
    // These must all be positive floating point numbers (reverseSensor will
    //   multiply the sensor values by negative one in case your sensor is flipped
    //   relative to your motor).
    // These values are in units of throttle / sensor_units where throttle ranges
    //   from -1023 to +1023 and sensor units are from 0 - 1023 for analog
    //   potentiometers, encoder ticks for encoders, and position / 10ms for
    //   speeds.
    motor.setPID(1.0, 0.0, 0.0);
}

/**
 * This uses the wheel circumference and the number of rotations to compute
 * the distance traveled.
 */

// Called repeatedly when this Command is scheduled to run
protected void execute() {
    // This loop allows for a negative input that will have the robot run
    // backwards.
    if (inputDistance < 0) {
        Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Driving backwards...");
        driveTrain.driveStraight(-movementSpeed);
    } else {
        Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Driving forwards...");
        driveTrain.driveStraight(movementSpeed);
    }
    // creates a loop to track the distance traveled
    // uses the time to determine the distance traveled since the last time
    // the robot was sampled.
    endTime = System.currentTimeMillis();
    elapsed = endTime - startTime;
    distanceSinceElapsed = 0;
    Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Recorded end time: " + endTime + " (difference of " + elapsed + " seconds)s");
    // maybe a method???
    for (CANTalon motor : motors) {
        double distanceForMotor = driveTrain.getDistanceForMotor(motor, elapsed);
        Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Distance for motor " + motor + ": " + distanceForMotor);
        distanceSinceElapsed = Math.max(distanceForMotor, distanceSinceElapsed);
        Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Distance since elapsed: " + distanceSinceElapsed);
    }

    distance += distanceSinceElapsed;
    startTime = endTime;
    Robot.debugger.logError(CustomDebugger.LoggerNames.DRIVETRAIN, "Completed movement of " + distance + " ft.");
}

/**
 * Sets the speed of this wheel controller. This is maintained using a PID
 * controller inside the Talon and an encoder.
 * 
 * @param speed the speed of the wheel (-1.0 to 1.0)
 */
@Override
public void set(double speed) {
    if (isEncoderEnabled()) {
        masterMotor.changeControlMode(CANTalon.ControlMode.Speed);
        masterMotor.set(convertThrottleToCountPer10ms(speed));
    } else {
        masterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
        masterMotor.set(speed);
    }
}

public static void toPositionMode()
{
    RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Position);
    RobotMap.winchMotor.setPID(3, 0.01, 0);
    RobotMap.winchMotor.setAllowableClosedLoopErr(0);
}

public static void toProfiledVelocityMode()
{
    RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
    RobotMap.winchMotor.setPID(1, 0.01, 0);
    RobotMap.winchMotor.setAllowableClosedLoopErr(0);
}

public Drive(int frontLeftChannel, int rearLeftChannel, int frontRightChannel, int rearRightChannel,
        int joystickPort) {

    frontLeft = new CANTalon(frontLeftChannel);
    rearLeft = new CANTalon(rearLeftChannel);
    frontRight = new CANTalon(frontRightChannel);
    rearRight = new CANTalon(rearRightChannel);

    frontLeft.setInverted(true);
    rearLeft.setInverted(true);
    frontRight.setInverted(true);
    rearRight.setInverted(true);


    drive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);

    joystick = new Joystick(joystickPort);


    //encoder = new WheelRotation (6, 360);

}

protected void initialize() {
    Robot.intakeLauncher.aimMotor.disableControl();
    Robot.intakeLauncher.aimMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
    System.out.println("Back up Joystick Command");
    Robot.intakeLauncher.aimMotor.enableControl();
}