public void drive() {
    // Driving controls
    /* flips which direction on the joysticks is 'forward'
     * the variable 'controlFlipClean' needs some explanation:
     * it forces the driver to release the button before it recognizes
     * another click. */
    if (xboxDriver.getBtnL3()) { // left stick click
      if (controlFlipClean) {
        controlFlipClean = false;
        controlFlip = !controlFlip;
      }
    } else {
      controlFlipClean = true;
    }

    // controls right side
    double yRight = -xboxDriver.getRightY();
    // controls left side
    double yLeft = xboxDriver.getLeftY();

    // trigger values will change the maximum speed of the robot
    double throttle = xboxDriver.getTriggers();

    // half speed
    if (throttle > 0.5) {
      yRight = yRight * 0.5;
      yLeft = yLeft * 0.5;
    }
    // quarter speed
    if (throttle < -0.5) {
      yRight = yRight * 0.25;
      yLeft = yLeft * 0.25;
    }

    // flip controls if we need to. Make them go 'backwards'.
    // We need to swap and yRight and yLeft values.
    if (controlFlip) {
      double tempRight = yRight;
      yRight = yLeft;
      yLeft = tempRight;
    }

    /* DEADZONE **
     * The springs in the joysticks wear out so they don't always come
     * back to the centre. To fix this we're going to ignore a small region
     * around the centre.
     */
    if (yRight > -DEADZONE && yRight < DEADZONE) {
      yRight = 0;
    }
    if (yLeft > -DEADZONE && yLeft < DEADZONE) {
      yLeft = 0;
    }

    vicDriveRight.set(yRight);
    vicDriveLeft.set(yLeft);
  } // end of drive
Пример #2
0
  public void run() {
    double power;
    if (shooting == false && retracting == false) {
      power = 0.0;
    } else if (shooting == false && retracting == true) {
      power = m_retract();
    } else if (shooting == true && retracting == false) {
      power = m_shoot();
    } else { // shooting == true && retracting == true
      power = 0.0;
    }

    motor1.set(power);
    motor2.set(power);
    motor3.set(power);
  }
  public void autonomousInit() {
    lightsOutput1.set(true);
    lightsOutput2.set(true);
    lightsOutput3.set(false);

    canShoot = false;

    vicDriveRight.set(0);
    vicDriveLeft.set(0);
    armVictor.set(0);
    winchRelay.set(Relay.Value.kOff);
    shooterWheel.set(0);
    shooterLoft.set(0);
    bettyRelay.set(Relay.Value.kOff);
    starterDelay = 0;
  }
  /** Called every time tele-op mode starts */
  public void teleopInit() {
    vicDriveRight.set(0);
    vicDriveLeft.set(0);
    armVictor.set(0);
    winchRelay.set(Relay.Value.kOff);
    shooterWheel.set(0);
    shooterLoft.set(0);
    bettyRelay.set(Relay.Value.kOff);
    lightsOutput1.set(true);
    lightsOutput2.set(true);
    lightsOutput3.set(true);
    lightsTimer.reset();
    lightsTimer.start();

    teleopActive = false;
    controlFlip = true;
    controlFlipClean = true;
    shootingSpeed = 0.0;
    starterDelay = 0;
  }
  public void shooterUse() {
    // Betty operation
    if (xboxOperator.getBtnLB()) { // hold right bumper to fire
      starterDelay++;
      if (xboxOperator.getTriggers() < -0.5) { // feeder shot
        if (shootingSpeed > feederShotSpeed) { // accelerating shooter speed until top speed
          shootingSpeed = shootingSpeed + flyWheelAcceleration;
        } else {
          shootingSpeed = feederShotSpeed; // ensures desired value
        }
      } else { // normal shot
        if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed
          shootingSpeed = shootingSpeed + flyWheelAcceleration;
        } else {
          shootingSpeed = normalShotSpeed; // ensures desired value
        }
      }
      shooterWheel.set(shootingSpeed);
      if (xboxOperator.getBtnRB() && starterDelay > 60) { // shoot normal shot
        bettyRelay.setDirection(Relay.Direction.kForward);
        bettyRelay.set(Relay.Value.kOn);
      } else if (xboxOperator.getBtnB() && starterDelay > 60) { // shoot normal shot
        bettyRelay.setDirection(Relay.Direction.kReverse);
        bettyRelay.set(Relay.Value.kOn);
      } else {
        bettyRelay.set(Relay.Value.kOff);
      }

    } else if (bettyOpen.get()) { // close betty if not shooting
      shootingSpeed = 0.0;
      bettyRelay.setDirection(Relay.Direction.kForward);
      bettyRelay.set(Relay.Value.kOn);
      shooterWheel.set(0);
      starterDelay = 0;
    } else {
      shootingSpeed = 0.0;
      bettyRelay.set(Relay.Value.kOff);
      shooterWheel.set(0);
      starterDelay = 0;
    }
  } // end of shooter use
  /** This function is called periodically during autonomous */
  public void autonomousPeriodic() {
    //        int loftPotValue = loftPot.getValue();
    //        // ajust for far shot
    //        if (!canShoot) {
    //            if (loftPotValue < farShotValue - 8) {
    //                starterDelay = 0;
    //                shooterLoft.set(-0.5);
    //
    //            }
    //            else if (loftPotValue > farShotValue + 8) {
    //                starterDelay = 0;
    //                shooterLoft.set(0.5);
    //
    //            }
    //            else { // if ajusted, shoot
    //                canShoot = true;
    //                shooterLoft.set(0);
    //            }
    //        }
    //        if(canShoot) {
    if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed(1.0)
      shootingSpeed = shootingSpeed + flyWheelAcceleration;
    } else {
      shootingSpeed = normalShotSpeed; // ensures desired value and no higher
    }
    shooterWheel.set(shootingSpeed);
    if (shootingSpeed == normalShotSpeed) {
      starterDelay++;
      if (starterDelay > 60) {
        bettyRelay.setDirection(Relay.Direction.kForward);
        bettyRelay.set(Relay.Value.kOn);
      } else if (starterDelay > 310) { // kill flywheel and betty
        bettyRelay.set(Relay.Value.kOff);
        shooterWheel.set(0);
      } else {
        bettyRelay.set(Relay.Value.kOff);
      }
    }
    //        }

  }
Пример #7
0
 public void teleopPeriodic() {
   armpid.setPID(
       SmartDashboard.getNumber("kP"),
       SmartDashboard.getNumber("kI"),
       SmartDashboard.getNumber("kD"));
   if (joy1.getRawButton(1)) {
     armpid.enable();
   } else {
     armpid.disable();
     arm.set(0);
   }
   SmartDashboard.putBoolean("Checkbox 2", limit.get());
   System.out.println("limit " + limit.get());
 }
Пример #8
0
 public void manualControl(double power) {
   motor1.set(power);
 }
Пример #9
0
 public void setRollers(double i) {
   rollers.set(i);
 }
Пример #10
0
  /** This function is called periodically during operator control */
  public void teleopPeriodic() {
    lights.set(Relay.Value.kForward);
    // tracking.run();
    /*try {
        readString = serial.readString();
        readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
        xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
        xTurnVal = Integer.parseInt(xTurn);
    } catch (VisaException ex) {
        System.out.println("Error Reading string - " + ex);
    }*/
    y = drive_control.getRawAxis(1);
    x = drive_control.getRawAxis(2);
    t = drive_control.getRawAxis(3);

    if (y > 1) y = 1;
    else if (y < -1) y = -1;

    if (x > 1) x = 1;
    else if (x < -1) x = -1;

    drive.setDriveJ(x, y, t);

    /*if(drive_control.getRawButton(12))
        //wheelsOn = true;
    else if(drive_control.getRawButton(11));
        wheelsOn = false;*/

    /*try{
        launcher.setX((drive_control.getRawAxis(4)-1)/2);
    }catch(Exception e){}*/

    // System.out.println((drive_control.getRawAxis(4)-1)*-1350);
    if (!autoWheels) {
      csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
      try {
        // launcher.setX((drive_control.getRawAxis(4)+1)*2000);
        System.out.println(
            -launcher.getSpeed()
                + " -- "
                + csc.tmpResult
                + " -- "
                + (drive_control.getRawAxis(4) - 1) * -1500);
      } catch (Exception e) {
        System.out.println(e);
      }
      if (drive_control.getRawButton(11)) autoWheels = true;
    } else {
      csc.setSetpoint(2300);
      if (drive_control.getRawButton(11)) autoWheels = false;
    }

    /*if(wheelsOn)
        shoot(1);
    else
        shoot(0);*/

    if (drive_control.getRawButton(5)) {
      wedge.moveDown();
      // servo1.set(1);
      // servo2.set(0);
      // wedge.set(Relay.Value.kForward);
    } else if (drive_control.getRawButton(6)) {
      wedge.moveUp();
      // servo1.set(0);
      // servo2.set(1);
      // wedge.set(Relay.Value.kReverse);
    } else {
      // servo1.set(.5);
      // servo2.set(.5);
      // wedge.set(Relay.Value.kOff);
    }

    if (drive_control.getRawButton(3)) turret.set(.5);
    else if (drive_control.getRawButton(4)) turret.set(-.5);
    else turret.set(0);

    if (drive_control.getRawButton(2)) driveMainBelt(true, true);
    else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
    else driveMainBelt(false, false);

    if (drive_control.getTrigger()) {
      try {
        belt2.setX(-1);
      } catch (Exception e) {
      }
    } else if (drive_control.getRawButton(8)) {
      try {
        belt2.setX(1);
      } catch (Exception e) {
      }
    } else {
      try {
        belt2.setX(0);
      } catch (Exception e) {
      }
    }

    if (control.getRawButton(3)) {
      turret.set(.25);
    } else if (control.getRawButton(4)) {
      turret.set(-.25);
    } else {
      turret.set(0);
    }

    try {
      station.println(
          DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
    } catch (CANTimeoutException ex) {
    }
    /*try{
        if(tracking.foundTarget()){
            station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
            station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
            if(x>0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
            else if(x<0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
            else
                station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
        }
        else{
            station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
            station.println(DriverStationLCD.Line.kUser3, 1, "");
            station.println(DriverStationLCD.Line.kUser4, 1, "");
        }
    }catch(Exception e){}*/
    station.updateLCD();
  }
 public void armUse() {
   // Arm contols
   int armPotValue = armPot.getValue();
   if (xboxDriver.getBtnB()) { // arm for slot feeding
     if (armPotValue < armSlotFeedingValue - angleTolerance) {
       armVictor.set(-.2);
     } else if (armPotValue < armSlotFeedingValue - focusTolerance) {
       armVictor.set(-.02);
     } else if (armPotValue > armSlotFeedingValue + angleTolerance) {
       armVictor.set(.5);
     } else if (armPotValue > armSlotFeedingValue + focusTolerance) {
       armVictor.set(.05);
     } else {
       armVictor.set(0);
     }
   } else if (xboxDriver.getBtnY()) { // arm for shovel feeding
     if (armPotValue < armShovelFeedingValue - angleTolerance) {
       armVictor.set(-.2);
     } else if (armPotValue < armShovelFeedingValue - focusTolerance) {
       armVictor.set(-.02);
     } else if (armPotValue > armShovelFeedingValue + angleTolerance) {
       armVictor.set(.5);
     } else if (armPotValue > armShovelFeedingValue + focusTolerance) {
       armVictor.set(.05);
     } else {
       armVictor.set(0);
     }
   } else {
     if (xboxDriver.getBtnLB() && armPotValue > minArmValue) {
       armVictor.set(-.2); // driver left bumper = down, right bumper = up;
     } else if (xboxDriver.getBtnRB() && armPotValue < maxArmValue) {
       armVictor.set(.5);
     } else {
       armVictor.set(0);
     }
     if (xboxDriver.getBtnA()
         && armPotValue
             < climbStopValue) { // when driver controls, hold button A to use winch, release to
       // stop
       winchRelay.set(Relay.Value.kOn);
       winchRelay.setDirection(Relay.Direction.kReverse);
     } else {
       winchRelay.set(Relay.Value.kOff);
     }
   }
 } // end of arm use
Пример #12
0
 public void moveRollerReverse() {
   // Move the roller to suck in the ball
   roller.set(-1);
 }
Пример #13
0
 public void turnRollerOff() {
   roller.set(0);
 }
Пример #14
0
 private void setPower(double power) {
   motor1.set(-power);
 }
 public void shooterLoft() {
   // Ajust loft
   double operatorLeftY = xboxOperator.getLeftY(); // operator has loft control
   int loftPotValue = loftPot.getValue();
   if (xboxOperator.getBtnX()) { // hold to ajust for close shot
     if (loftPotValue < closeShotValue - focusTolerance) {
       if (loftPotValue < closeShotValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > closeShotValue + focusTolerance) {
       if (loftPotValue > closeShotValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (xboxOperator.getBtnY()) { // hold to ajust for far shot
     if (loftPotValue < farShotValue - focusTolerance) {
       if (loftPotValue < farShotValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > farShotValue + focusTolerance) {
       if (loftPotValue > farShotValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (xboxOperator.getBtnB()) { // hold to ajust for feeding
     if (loftPotValue < shooterFeedingValue - focusTolerance) {
       if (loftPotValue < shooterFeedingValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > shooterFeedingValue + focusTolerance) {
       if (loftPotValue > shooterFeedingValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (loftPotValue > maxHeightValue
       && loftPotValue < minHeightValue
       && ((operatorLeftY > .3) || operatorLeftY < -.3)) { // move loft with left operator stick
     shooterLoft.set(operatorLeftY);
   } else {
     shooterLoft.set(0); // don't move loft
   }
 } // end of shooter loft
Пример #16
0
 public void stop() {
   motor1.set(0);
 }
Пример #17
0
 public void moveRollerForward() {
   // Move the roller to eject the ball
   roller.set(1);
 }