import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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());
    }
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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();
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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());
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
/**
 * This function is called once each time the robot enters operator control.
 */
public void operatorControl() 
{
    while(isOperatorControl() && isEnabled())
    {
        //Tank stuff
        Drive.drive_tank(driver_left_joystick.getY(), driver_right_joystick.getY());
        System.out.println(Drive.get_front_left());
        try
        {
            front_left_jaguar.setX(Drive.get_front_left());
            back_left_jaguar.setX(Drive.get_back_left());
            back_right_jaguar.setX(Drive.get_back_right());
            front_right_jaguar.setX(Drive.get_front_right());
        }
        catch (CANTimeoutException ex)
        {
            ex.printStackTrace();
        }
        //todo: MECANUM
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void rotate(double distance, double i) {
    try {
        double zeroPosn = HardwareDefines.SHOOTER_POT_REV_LIM;
        double distToClick = distanceToPotClicks(distance);
        double currPosn = tiltJag.getPosition();
        double distErr = distToClick - currPosn;
        double p = (distErr / distToClick);
        
        if(distToClick > currPosn) {
            winchJag.configSoftPositionLimits(distToClick, zeroPosn);
        }
        else if(distToClick < currPosn) {
            winchJag.configSoftPositionLimits(zeroPosn, distToClick);
        }
        
        tiltJag.setPID(p, i, 0.0);
        tiltJag.enableControl();
        if(distToClick == currPosn) {
            rotated = true;
        }
    } catch(CANTimeoutException e) {
        System.out.println("Could not tilt the shooter!");
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void reload(double distance) {
    try {
        double zeroPosn = HardwareDefines.SHOOTER_LIN_ENC_REV_LIM;
        double distToClick = distanceToLinEncClicks(distance);
        double currPosn = winchJag.getPosition();
        double distErr = distToClick - currPosn;
        double p = (distErr / distToClick);
        winchJag.configSoftPositionLimits(distToClick, zeroPosn);
        winchJag.setPID(p, 0, 0);
        winchJag.enableControl();
        if(distToClick == currPosn) {
            loaded = true;
        }
    }
    catch(CANTimeoutException e) {
        System.out.println("Could not reload shooter!");
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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;
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void setSpeed(double left, double right){
    try{
        leftJag.setX(-left);
        rightJag.setX(right);
    }catch(CANTimeoutException e){
        try{
            canInit();
        }catch(CANTimeoutException f){
            System.out.println("canInit Failed");
        }
    }catch(NullPointerException g){
        try{
            canInit();
        }catch(CANTimeoutException h){
            System.out.println("canInit Failed");
        }
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
/***
  * RotateRight()
  * 
  * Turns the wheels to 90 degrees and then turn the wheels at what the speed is set at
  *
  * @param speed -1.0 ... 0.0 ... 1.0
  */
public void RotateRight(double speed)  throws CANTimeoutException
{
    if (!Steer(ROTATE_SETPOINT_RIGHT_90))
    {
        double error = middle.getPosition() 
                - ConvertJoystickToPosition(ROTATE_SETPOINT_RIGHT_90);
        //System.out.println(error);
        if (error < ReboundRumble.JOYSTICK_DEADBAND
                && error > (-1.0 * ReboundRumble.JOYSTICK_DEADBAND) )
        {
            left.set(speed);
            right.set(speed);
        }
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public ReboundRumble()
{
    super();
    if (DEBUG)
    {
        System.out.println("Entering Rebound Rumble constructor.");
    }
    try
    {
        drive = new CrabDrive();
    } catch (CANTimeoutException e)
    {
        System.out.println(e);
    }
    driverI = DriverStation.getInstance();
    game = new GameMech();
    if (DEBUG)
    {
        System.out.println("Exiting Rebound Rumble constructor.");
    }

}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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();
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
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;
    }
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
/**
 * SetElevation()
 *
 * This method will set the shooter to the angle specified by Aim()
 *
 * @param setPoint - angle reading in the range 0.1 ... 1.0
 *
 * @return boolean - true = the elevation is at the setpoint false = the
 * elevation is not yet at the setpoint
 */
public boolean SetElevation(double setPoint) throws CANTimeoutException
{
    elevationSetpoint = setPoint;
    if (!isElevationPIDControlled)
    {
        SetElevationPositionControl();
    }
    elevation.setX(setPoint);
    double angle = elevation.getPosition();
    if ((elevationSetpoint - angle) <= 0.05 && (elevationSetpoint - angle) >= -0.05)
    {
        return true;
    }
    return false;
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void disabled()
{
    
    try 
    {
        leftMotor.setX(0);
        rightMotor.setX(0);
        shooter.stop();   //  JAG CHANGE
        /*while(isDisabled())
        {
            System.out.println("Partial Sensor: " + climb.part.get() + " Top Sensor: " + climb.max.get());
           
        }*/
    }
    catch (CANTimeoutException ex) 
    {
        ex.printStackTrace();
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
/**
     *
     * SetLoadPosition()
     *
     * tells the elevation motor to move the shooter to the elevation setpoint.
     *
     * @return true if the loader is at the load elevation false if the loader
     * is not yet at the load elevation
     *
     * @throws CANTimeoutException
     */
    public boolean SetLoadPosition() throws CANTimeoutException
    {
//        if (!loadPosition.get())
//        {
//            return true;
//        }
        AdjustElevation(1.0);
////        double error = elevation.getPosition() - LOAD_POSITION;
////        if (error <= ELEVATION_ERROR && error>= -1.0 * ELEVATION_ERROR)
//        if (!loadPosition.get())
//        {
//            return true;
//        }
        if (!elevation.getForwardLimitOK())
            return true;
        return false;
    }
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void goForwardNormal (double inches)
{
    try {
        double ticks = 0;
        cfgNormalMode(leftMotor);
        cfgNormalMode(rightMotor);
        ticks = leftMotor.getPosition() + 2.0;
        System.out.println("Starting Position: " + leftMotor.getPosition());
        while(leftMotor.getPosition() < ticks && isEnabled())
        {
            drive.arcadeDrive(0.5, 0.0);
            System.out.println(leftMotor.getPosition());
        }
    } 
    catch (CANTimeoutException ex)
    {
        ex.printStackTrace();
    }
    drive.arcadeDrive(0.0,0.0);
    
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
/**
     * Aim()
     *
     * This method will aim the entire robot at the leftmost target in the
     * camera's field of view. It will set the SlingShot's elevation, the
     * SlingShot's force and rotate the robot to point at the target
     *
     * @return true - the robot is ready to shoot and score false - the robot is
     * not yet completely aimed at the target
     */
    public boolean Aim(CrabDrive drive, double forceAdjust) throws CANTimeoutException
    {

        if (camera != null && camera.r != null)
        {
            if (camera.FindTarget())
            {
                boolean isElevationSet;
                boolean isForceSet;
                boolean isAngleSet;
//                isForceSet = shoot.SetShooterForce(1.0, forceAdjust);
                isForceSet = shoot.SetShooterForce(camera.GetShooterForce(camera.GetDistance()), forceAdjust);
//                if (load.isLoaderIn())
//                {
                isElevationSet = shoot.SetShootPosition();
//                }
                isAngleSet = drive.FaceTarget(camera.cameraPan.getAngle() - 85);
                if (isElevationSet && isForceSet && isAngleSet)
                {
                    return true;
                }
            }
        }
        return false;
    }
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public void goForward(double inches)
{
    try {

        cfgPosMode(leftMotor);
        cfgPosMode(rightMotor);
        leftMotor.setX(-10);
        rightMotor.setX(10);
        while(isEnabled())
        {
            System.out.println(leftMotor.getPosition());

        }
        leftMotor.setX(0);
        rightMotor.setX(0);
        cfgNormalMode(leftMotor);
        cfgNormalMode(rightMotor);
        
    } 
    catch (CANTimeoutException ex) 
    {
        ex.printStackTrace();
    }
}
 

import edu.wpi.first.wpilibj.can.CANTimeoutException; //导入依赖的package包/类
public boolean shootWithJoy(Shooter shooter) throws CANTimeoutException{
    
    //Shooting wheel
    if(otherJoy.getRawButton(1)){
        shooter.shoot();
    }else{
        shooter.stop();
    }
    
    //Hopper
    if(otherJoy.getRawButton(2)){
        shooter.load();
    }else{
        shooter.reload();
    }
    
    //Return Success
    return true;
    
}