Java 类edu.wpi.first.wpilibj.can.CANTimeoutException 实例源码

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());
    }
}

/**
 * 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
    }
}

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!");
    }
}

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!");
    }
}

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 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");
        }
    }
}

/***
  * 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);
        }
    }
}

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.");
    }

}

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 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;
    }

/**
 * 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;
}

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();
    }
}

/**
     *
     * 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;
    }

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);

}

/**
     * 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;
    }

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();
    }
}

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;

}

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 void calculate()
{
    if(!isEnabled){ return; }

    try
    {
        //If speed is < desired speed, then output = 1.0
        //If speed is anything else, output = 0.0 (STOP)
        if(motor.getSpeed() >= targetSpeed){
            motor.setX(0.0);
        } else {
            motor.setX(reversed ? -1.0 : 1.0);
        }
    }
    catch(CANTimeoutException ex)
    {
        ex.printStackTrace();
    }
}

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

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; }
    }
}

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

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

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

public boolean getForwardLimitOK() {
    if (!connected)
        return false;
    for (int i = 0; i < 3; i++) {
        try { return jaguar.getForwardLimitOK(); }
        catch (CANTimeoutException ex) { }
    }
    connected = false;
    return false;
}

public boolean getReverseLimitOK() {
    if (!connected)
        return false;
    for (int i = 0; i < 3; i++) {
        try { return jaguar.getReverseLimitOK(); }
        catch (CANTimeoutException ex) { }
    }
    connected = false;
    return false;
}

public double getOutputCurrent() {
    if (!connected)
        return 0;
    for (int i = 0; i < 3; i++) {
        try { return jaguar.getOutputCurrent(); }
        catch (CANTimeoutException ex) { }
    }
    connected = false;
    return 0;
}

public double getOutputVoltage() {
    if (!connected)
        return 0;
    for (int i = 0; i < 3; i++) {
        try { return jaguar.getOutputVoltage(); }
        catch (CANTimeoutException ex) { }
    }
    connected = false;
    return 0;
}

/**
 * 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);
    }
}

/**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        try {
//            rightFront.setX(joystick.getRawAxis(1)*100);
            if(joystick.getRawButton(1)){
                rightFront.setX(60);
            }else if(joystick.getRawButton(2)){
                rightFront.setX(40);
            }
            System.err.println("Encoder: " + rightFront.getSpeed());
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }

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

public void WindUp() {
        try {
            Arm2.setX(-1);
//            try {
//                Thread.sleep(2000);
//            } catch (InterruptedException ex) {
//                ex.printStackTrace();
//            }
            Arm2.setX(-.5);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }

public void UnWind() {
        try {
            Arm2.setX(1);
//            try {
//                Thread.sleep(100);
//            } catch (InterruptedException ex) {
//                ex.printStackTrace();
//            }            

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

public void setMotor(double purple) {
    try {
        Arm2.setX(purple);
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public void Stop() {
    try {
        Arm2.setX(0);
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public boolean get() {
    try {
        return Arm2.getReverseLimitOK();
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
    return false;
}