src/main/java/frc/robot/Robot.java
@Override
  public void teleopPeriodic() {
    // Puts the robot in arcade drive
    robotDrive.arcadeDrive(-joystick.getRawAxis(0), joystick.getRawAxis(1));

    // Joystick trigger activates motor
    if(joystick.getTrigger())
      intake.set(1);
    else if(joystick.getRawButton(2))
      intake.set(-1);
    else
      intake.motorOff();

    // Manually control the turret with bumpers
    if(xbox.getLeftBumper()) {
      turretPID.pause();
      turret.setPower(.3);
    }
    else if(xbox.getRightBumper()) {
      turretPID.pause();
      turret.setPower(-.3);
    }
    else if(turretPID.getP() == 0)
      turret.off();

    // Dpad controls
    switch(xbox.getPOV()){
      case 0: // UP
        rightclimberPID.activate(-238);
        leftclimberPID.activate(-238);
        break;
      case 180: // DOWN
        leftclimberPID.activate(-65);
        rightclimberPID.activate(-65);
        break;
      case 90: // RIGHT
        shooter.increasePowerBy(.004);
        break;
      case 270: // LEFT
        shooter.decreasePowerBy(.004);
        break;
      default: // NOT PRESSED
        // Right trigger pushes a ball into the shooter
        if(xbox.getRightTriggerAxis() > 0)
          elevator.shoot();
        else if(xbox.getAButton())
          elevator.lift();
        else
          elevator.auto();
    }
    
    if(xbox.getLeftTriggerAxis() > 0) {
      
      // Hood quartic regression; function of limelight distance that outputs position
      hoodPID.activate((.000002262119 * Math.pow(limelight.getDistance(), 4)) - (.000654706898 * Math.pow(limelight.getDistance(), 3)) + (.060942569498 * Math.pow(limelight.getDistance(), 2)) - (1.23311704654 * limelight.getDistance()) - .962075155165);
      
      // When limelight does not have a target, the turret will constinuously travel to left and right extrema
      // until limelight does have a target, in which the turret will focus on the target
      if (!(limelight.hasValidTarget) && !(TRL)) {
        searchPID.activate(70 * TURRET_RATIO);
        TRL = true;
      }
        
      else if (!(limelight.hasValidTarget) && turret.getPosition() > 28 && TRL) {
        searchPID.activate(-70 * TURRET_RATIO);
      }
  
      else if (!(limelight.hasValidTarget) && turret.getPosition() < -28) {
        TRL = false;
      }
    
      else if (limelight.hasValidTarget) {
        turretPID.activate(
          ((turret.getPosition() / TURRET_RATIO) - limelight.getRotationAngle()) * TURRET_RATIO );
        TRL = false;
      }

      // Flywheel quadratic regression; function of limelight distance that outputs velocity
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
    }
      
    // Left stick Y-axis controls left climber arm
    if(Math.abs(xbox.getLeftY()) > CLIMBER_DEADZONE ) {
      leftclimberPID.pause();
      climber.setLeft(xbox.getLeftY());
    }
    else if(leftclimberPID.getP() == 0)
      climber.leftOff();
      

    // Right stick Y-axis controls left climber arm
    if(Math.abs(xbox.getRightY()) > CLIMBER_DEADZONE ) {
      rightclimberPID.pause();
      climber.setRight(xbox.getRightY());
    }
    else if (leftclimberPID.getP() == 0)
      climber.rightOff();

    // X button lowers intake
    if(xbox.getXButton())
      intake.down();
    // Y button raises intake
    else if(xbox.getYButton())
      intake.up();

    // joystick controls intake state
    if(joystick.getRawButton(3))
      intake.down();
    else if (joystick.getRawButton(4))
      intake.up();

    // Get ready to climb!
    if(xbox.getStartButton()) {
      hoodPID.activate(0);
      turretPID.activate(0);
      shooterPID.activate(0);
      intake.up();
      leftclimberPID.activate(0);
      rightclimberPID.activate(0);
    }
    
    // preset motor value to shoot ball
    if(xbox.getLeftStickButton()) {
      shooterPID.activate(3100);
    }

    // preset motor value to shoot ball at low speed (reject ball)
    if(xbox.getRightStickButton()) {
      shooterPID.activate(1700);
    }


    // B is essentially an e-stop
    if(xbox.getBButton()){
      shooter.off();
      elevator.off();
      climber.off();
      turret.off();
      hood.off();
      turretPID.pause();
    }
  }
// This function is called every 20ms during teleop
This function is called every 20ms during teleop
This function is called every 20ms during teleop
@Override
  public void teleopPeriodic() {
    robotDrive.arcadeDrive(-joystick.getRawAxis(0), joystick.getRawAxis(1));
    if(joystick.getTrigger())
      intake.set(1);
    else if(joystick.getRawButton(2))
      intake.set(-1);
    else
      intake.motorOff();
    if(xbox.getLeftBumper()) {
      turretPID.pause();
      turret.setPower(.3);
    }
    else if(xbox.getRightBumper()) {
      turretPID.pause();
      turret.setPower(-.3);
    }
    else if(turretPID.getP() == 0)
      turret.off();
    switch(xbox.getPOV()){
      case 0: 
        rightclimberPID.activate(-238);
        leftclimberPID.activate(-238);
        break;
      case 180: 
        leftclimberPID.activate(-65);
        rightclimberPID.activate(-65);
        break;
      case 90: 
        shooter.increasePowerBy(.004);
        break;
      case 270: 
        shooter.decreasePowerBy(.004);
        break;
      default: 
        if(xbox.getRightTriggerAxis() > 0)
          elevator.shoot();
        else if(xbox.getAButton())
          elevator.lift();
        else
          elevator.auto();
    }
    if(xbox.getLeftTriggerAxis() > 0) {
      hoodPID.activate((.000002262119 * Math.pow(limelight.getDistance(), 4)) - (.000654706898 * Math.pow(limelight.getDistance(), 3)) + (.060942569498 * Math.pow(limelight.getDistance(), 2)) - (1.23311704654 * limelight.getDistance()) - .962075155165);
      if (!(limelight.hasValidTarget) && !(TRL)) {
        searchPID.activate(70 * TURRET_RATIO);
        TRL = true;
      }
      else if (!(limelight.hasValidTarget) && turret.getPosition() > 28 && TRL) {
        searchPID.activate(-70 * TURRET_RATIO);
      }
      else if (!(limelight.hasValidTarget) && turret.getPosition() < -28) {
        TRL = false;
      }
      else if (limelight.hasValidTarget) {
        turretPID.activate(
          ((turret.getPosition() / TURRET_RATIO) - limelight.getRotationAngle()) * TURRET_RATIO );
        TRL = false;
      }
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
    }
    if(Math.abs(xbox.getLeftY()) > CLIMBER_DEADZONE ) {
      leftclimberPID.pause();
      climber.setLeft(xbox.getLeftY());
    }
    else if(leftclimberPID.getP() == 0)
      climber.leftOff();
    if(Math.abs(xbox.getRightY()) > CLIMBER_DEADZONE ) {
      rightclimberPID.pause();
      climber.setRight(xbox.getRightY());
    }
    else if (leftclimberPID.getP() == 0)
      climber.rightOff();
    if(xbox.getXButton())
      intake.down();
    else if(xbox.getYButton())
      intake.up();
    if(joystick.getRawButton(3))
      intake.down();
    else if (joystick.getRawButton(4))
      intake.up();
    if(xbox.getStartButton()) {
      hoodPID.activate(0);
      turretPID.activate(0);
      shooterPID.activate(0);
      intake.up();
      leftclimberPID.activate(0);
      rightclimberPID.activate(0);
    }
    if(xbox.getLeftStickButton()) {
      shooterPID.activate(3100);
    }
     at low speed (reject ball)
    if(xbox.getRightStickButton()) {
      shooterPID.activate(1700);
    }
    if(xbox.getBButton()){
      shooter.off();
      elevator.off();
      climber.off();
      turret.off();
      hood.off();
      turretPID.pause();
    }
  }
@ Override public void teleopPeriodic ( ) { robotDrive . arcadeDrive ( - joystick . getRawAxis ( 0 ) , joystick . getRawAxis ( 1 ) ) ; if ( joystick . getTrigger ( ) ) intake . set ( 1 ) ; else if ( joystick . getRawButton ( 2 ) ) intake . set ( - 1 ) ; else intake . motorOff ( ) ; if ( xbox . getLeftBumper ( ) ) { turretPID . pause ( ) ; turret . setPower ( .3 ) ; } else if ( xbox . getRightBumper ( ) ) { turretPID . pause ( ) ; turret . setPower ( - .3 ) ; } else if ( turretPID . getP ( ) == 0 ) turret . off ( ) ; switch ( xbox . getPOV ( ) ) { case 0 : rightclimberPID . activate ( - 238 ) ; leftclimberPID . activate ( - 238 ) ; break ; case 180 : leftclimberPID . activate ( - 65 ) ; rightclimberPID . activate ( - 65 ) ; break ; case 90 : shooter . increasePowerBy ( .004 ) ; break ; case 270 : shooter . decreasePowerBy ( .004 ) ; break ; default : if ( xbox . getRightTriggerAxis ( ) > 0 ) elevator . shoot ( ) ; else if ( xbox . getAButton ( ) ) elevator . lift ( ) ; else elevator . auto ( ) ; } if ( xbox . getLeftTriggerAxis ( ) > 0 ) { hoodPID . activate ( ( .000002262119 * Math . pow ( limelight . getDistance ( ) , 4 ) ) - ( .000654706898 * Math . pow ( limelight . getDistance ( ) , 3 ) ) + ( .060942569498 * Math . pow ( limelight . getDistance ( ) , 2 ) ) - ( 1.23311704654 * limelight . getDistance ( ) ) - .962075155165 ) ; if ( ! ( limelight . hasValidTarget ) && ! ( TRL ) ) { searchPID . activate ( 70 * TURRET_RATIO ) ; TRL = true ; } else if ( ! ( limelight . hasValidTarget ) && turret . getPosition ( ) > 28 && TRL ) { searchPID . activate ( - 70 * TURRET_RATIO ) ; } else if ( ! ( limelight . hasValidTarget ) && turret . getPosition ( ) < - 28 ) { TRL = false ; } else if ( limelight . hasValidTarget ) { turretPID . activate ( ( ( turret . getPosition ( ) / TURRET_RATIO ) - limelight . getRotationAngle ( ) ) * TURRET_RATIO ) ; TRL = false ; } shooterPID . activate ( .056650444657 * Math . pow ( limelight . getDistance ( ) , 2 ) + 8.50119265165 * limelight . getDistance ( ) + 2383.56516106 ) ; } if ( Math . abs ( xbox . getLeftY ( ) ) > CLIMBER_DEADZONE ) { leftclimberPID . pause ( ) ; climber . setLeft ( xbox . getLeftY ( ) ) ; } else if ( leftclimberPID . getP ( ) == 0 ) climber . leftOff ( ) ; if ( Math . abs ( xbox . getRightY ( ) ) > CLIMBER_DEADZONE ) { rightclimberPID . pause ( ) ; climber . setRight ( xbox . getRightY ( ) ) ; } else if ( leftclimberPID . getP ( ) == 0 ) climber . rightOff ( ) ; if ( xbox . getXButton ( ) ) intake . down ( ) ; else if ( xbox . getYButton ( ) ) intake . up ( ) ; if ( joystick . getRawButton ( 3 ) ) intake . down ( ) ; else if ( joystick . getRawButton ( 4 ) ) intake . up ( ) ; if ( xbox . getStartButton ( ) ) { hoodPID . activate ( 0 ) ; turretPID . activate ( 0 ) ; shooterPID . activate ( 0 ) ; intake . up ( ) ; leftclimberPID . activate ( 0 ) ; rightclimberPID . activate ( 0 ) ; } if ( xbox . getLeftStickButton ( ) ) { shooterPID . activate ( 3100 ) ; } if ( xbox . getRightStickButton ( ) ) { shooterPID . activate ( 1700 ) ; } if ( xbox . getBButton ( ) ) { shooter . off ( ) ; elevator . off ( ) ; climber . off ( ) ; turret . off ( ) ; hood . off ( ) ; turretPID . pause ( ) ; } }
This function is called every 20ms during teleop
This function is called every 20ms during teleop
// Puts the robot in arcade drive // Joystick trigger activates motor // Manually control the turret with bumpers // Dpad controls // UP // DOWN // RIGHT // LEFT // NOT PRESSED // Right trigger pushes a ball into the shooter // Hood quartic regression; function of limelight distance that outputs position // When limelight does not have a target, the turret will constinuously travel to left and right extrema // until limelight does have a target, in which the turret will focus on the target // Flywheel quadratic regression; function of limelight distance that outputs velocity // Left stick Y-axis controls left climber arm // Right stick Y-axis controls left climber arm // X button lowers intake // Y button raises intake // joystick controls intake state // Get ready to climb! // preset motor value to shoot ball // preset motor value to shoot ball at low speed (reject ball) // B is essentially an e-stop