src/main/java/frc/robot/Robot.java
@Override
  public void autonomousPeriodic() { 
    double currentTime = Timer.getFPGATimestamp() - autonStartTime;

    // drops intake
    if (currentTime > 0.01 && currentTime < 1.5) {
      drivetrain.getLeftMotorGroup().set(0);
      drivetrain.getRightMotorGroup().set(0);
    }

    // drives backwards
    if (currentTime > 1.5 && currentTime < 2.6) {
      drivetrain.getLeftMotorGroup().set(0.4);
      drivetrain.getRightMotorGroup().set(-0.4);
    }

    // stops drive
    if (currentTime > 2.6 && currentTime < 4.9) {
      drivetrain.getLeftMotorGroup().set(0);
      drivetrain.getRightMotorGroup().set(0);
    }

    // aims hood and starts flywheel and aims turret
    if (currentTime > 5 && currentTime < 7.5) {
      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);
      turretPID.activate(
        ((turret.getPosition() / TURRET_RATIO) - limelight.getRotationAngle()) * TURRET_RATIO );
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
    }

    // shoots balls
    if (currentTime > 7.5 && currentTime < 12) {
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
      elevator.shoot();
    }

    // stops elevator
    if (currentTime == 12) {
      elevator.off(); 
    }
  }
// This function is called every 20ms during auton
This function is called every 20ms during auton
This function is called every 20ms during auton
@Override
  public void autonomousPeriodic() { 
    double currentTime = Timer.getFPGATimestamp() - autonStartTime;
    if (currentTime > 0.01 && currentTime < 1.5) {
      drivetrain.getLeftMotorGroup().set(0);
      drivetrain.getRightMotorGroup().set(0);
    }
    if (currentTime > 1.5 && currentTime < 2.6) {
      drivetrain.getLeftMotorGroup().set(0.4);
      drivetrain.getRightMotorGroup().set(-0.4);
    }
    if (currentTime > 2.6 && currentTime < 4.9) {
      drivetrain.getLeftMotorGroup().set(0);
      drivetrain.getRightMotorGroup().set(0);
    }
    if (currentTime > 5 && currentTime < 7.5) {
      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);
      turretPID.activate(
        ((turret.getPosition() / TURRET_RATIO) - limelight.getRotationAngle()) * TURRET_RATIO );
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
    }
    if (currentTime > 7.5 && currentTime < 12) {
      shooterPID.activate(.056650444657 * Math.pow(limelight.getDistance(), 2) + 8.50119265165 * limelight.getDistance() + 2383.56516106);
      elevator.shoot();
    }
    if (currentTime == 12) {
      elevator.off(); 
    }
  }
@ Override public void autonomousPeriodic ( ) { double currentTime = Timer . getFPGATimestamp ( ) - autonStartTime ; if ( currentTime > 0.01 && currentTime < 1.5 ) { drivetrain . getLeftMotorGroup ( ) . set ( 0 ) ; drivetrain . getRightMotorGroup ( ) . set ( 0 ) ; } if ( currentTime > 1.5 && currentTime < 2.6 ) { drivetrain . getLeftMotorGroup ( ) . set ( 0.4 ) ; drivetrain . getRightMotorGroup ( ) . set ( - 0.4 ) ; } if ( currentTime > 2.6 && currentTime < 4.9 ) { drivetrain . getLeftMotorGroup ( ) . set ( 0 ) ; drivetrain . getRightMotorGroup ( ) . set ( 0 ) ; } if ( currentTime > 5 && currentTime < 7.5 ) { 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 ) ; turretPID . activate ( ( ( turret . getPosition ( ) / TURRET_RATIO ) - limelight . getRotationAngle ( ) ) * TURRET_RATIO ) ; shooterPID . activate ( .056650444657 * Math . pow ( limelight . getDistance ( ) , 2 ) + 8.50119265165 * limelight . getDistance ( ) + 2383.56516106 ) ; } if ( currentTime > 7.5 && currentTime < 12 ) { shooterPID . activate ( .056650444657 * Math . pow ( limelight . getDistance ( ) , 2 ) + 8.50119265165 * limelight . getDistance ( ) + 2383.56516106 ) ; elevator . shoot ( ) ; } if ( currentTime == 12 ) { elevator . off ( ) ; } }
This function is called every 20ms during auton
This function is called every 20ms during auton
// drops intake // drives backwards // stops drive // aims hood and starts flywheel and aims turret // shoots balls // stops elevator