Java 类edu.wpi.first.wpilibj.command.Scheduler 实例源码

@Override
public void robotPeriodic() {
  try {
    // measure total cycle time, time we take during robotPeriodic, and WPIlib overhead
    final long start = System.nanoTime();
    logger.trace("robotPeriodic()");
    Scheduler.getInstance().run();

    long currentNanos = System.nanoTime();

    if (currentNanos - nanosAtLastUpdate > RobotMap.SMARTDASHBOARD_UPDATE_RATE * 1000000000) {
      allSubsystems.forEach(this::tryToSendDataToSmartDashboard);
      nanosAtLastUpdate = currentNanos;
    }

    SmartDashboard.putNumber("cycleMillis", (currentNanos - prevNanos) / 1000000.0);
    SmartDashboard.putNumber("ourTime", (currentNanos - start) / 1000000.0);
    prevNanos = currentNanos;
  } catch (Throwable ex) {
    logger.error("robotPeriodic error", ex);
    ex.printStackTrace();
  }
}

/**
 * This function is called periodically during operator control
 */
// @Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();

    // DEBUG \\
    if (RobotConstants.isTestingEnvironment) readTestingEnvironment();
    updateSensorDisplay();

    System.out.println(Robot.drivetrain.getEncoderPosition());

    // drive control
    drive();
    // climb control
    climb();
}

public void disabledPeriodic() {
    Scheduler.getInstance().run();

    SmartDashboard.putString("Selected Auto", chooser.getSelected().getName());
    publishToSmartDashboard();
    //SmartDashboard.putString("Selected Autonomous", chooser.getSelected().getName());

}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();

    /* Create an instance of the DriveForwardCommand and
     * start it during autonomous mode. This will make the
     * robot drive forward during autonomous. */
    DriveForwardCommand d = new DriveForwardCommand();
    d.start();
}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    /*switch (autoSelected) {
    case customAuto:
        // Put custom auto code here
        break;
    case defaultAuto:
    default:
        // Put default auto code here
        break;
    }*/
    log();
    Scheduler.getInstance().run();
}

@Override
public void teleopPeriodic() {
    log();
    Scheduler.getInstance().run();

    /*
     * CustomDashboard.putNumber("DriveTrain Left Current 1", pdp.getCurrent(RobotMap.PDP.leftDriveMotor[0]));
     * CustomDashboard.putNumber("DriveTrain Left Current 2", pdp.getCurrent(RobotMap.PDP.leftDriveMotor[1]));
     * CustomDashboard.putNumber("DriveTrain Left Current 3", pdp.getCurrent(RobotMap.PDP.leftDriveMotor[2]));
        *       
     * CustomDashboard.putNumber("DriveTrain Right Voltage 1", pdp.getCurrent(RobotMap.PDP.rightDriveMotor[0]));
     * CustomDashboard.putNumber("DriveTrain Right Voltage 2", pdp.getCurrent(RobotMap.PDP.rightDriveMotor[1]));
     * CustomDashboard.putNumber("DriveTrain Right Voltage 3", pdp.getCurrent(RobotMap.PDP.rightDriveMotor[2]));
     */
}

/**
 * This function is called periodically during autonomous
 */
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
    if (! Robot.pixyCamera.read(0, 1, buffer))
        pixyValue =  buffer[0] & 0xFF;

    SmartDashboard.putNumber("Pixy X value", pixyValue  );

}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
    sendSensorData();
    Robot.leds.update();
    // Robot.vision.addCrosshairs();
}

@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
    sendSensorData();
    Robot.leds.update();
    // Robot.vision.addCrosshairs();
}

/**
 * This function is called periodically during operator control
 */
public void teleopPeriodic() {
    Scheduler.getInstance().run();
    SmartDashboard.putNumber("GyroAngle", RobotMap.gyro.getAngle());
    SmartDashboard.putNumber("JoystickX", Robot.oi.getLogitech().getX());
    SmartDashboard.putNumber("JoystickY", Robot.oi.getLogitech().getY());
    SmartDashboard.putNumber("JoystickTwist", Robot.oi.getLogitech().getTwist());
}

/**
 * Run every tick in teleop.
 */
@Override
public void teleopPeriodic() {
    //Refresh the current time.
    Clock.updateTime();
    //Read sensors
    this.robotMap.getUpdater().run();
    //Run all commands. This is a WPILib thing you don't really have to worry about.
    Scheduler.getInstance().run();
}

public void disabledPeriodic() {
    Scheduler.getInstance().run();
    // DEBUG CODE HERE \\
    updateSensorDisplay();

    // *************** \\
}

public void autonomousPeriodic() {
    Scheduler.getInstance().run();

    // DEBUG \\
    if (RobotConstants.isTestingEnvironment) readTestingEnvironment();
    updateSensorDisplay();
}

/**
 * This function is called periodically during autonomous
 */
// @Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();

    // DEBUG \\
    if (RobotConstants.isTestingEnvironment) updateTestingEnvironment();
}

/**
 * Run every tick in teleop.
 */
@Override
public void teleopPeriodic() {
    //Refresh the current time.
    Clock.updateTime();

    //Read sensors
    this.robotMap.getUpdater().run();

    //Run all commands. This is a WPILib thing you don't really have to worry about.
    Scheduler.getInstance().run();
}

/**
 * Runs every tick in autonomous.
 */
@Override
public void autonomousPeriodic() {
    //Update the current time
    Clock.updateTime();
    //Read sensors
    this.robotMap.getUpdater().run();
    //Run all commands. This is a WPILib thing you don't really have to worry about.
    Scheduler.getInstance().run();
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString code to get the auto name from the text box below the Gyro
 * <p>
 * You can add additional auto modes by adding additional commands to the
 * chooser code above (like the commented example) or additional comparisons
 * to the switch structure below with additional strings and commands.
 */
public void autonomousInit()
{
    DRIVE_TRAIN.setAutonSettings();
    ArrayList<Vector2f> waypoints = new ArrayList<>();

    waypoints.add(new Vector2f(0,0));
    waypoints.add(new Vector2f(5,5));
    waypoints.add(new Vector2f(6,21));
    Scheduler.getInstance().add(new PurePursuitCommand(waypoints, LOOKAHEAD_DISTANCE));
    // Scheduler.getInstance().add(AutoSwitcher.getAutoInstance());
    NAVX.reset();
}

@Override
public void autonomousInit() {
    Scheduler.getInstance().removeAll();
    new ShiftDown().start();
    // Attempt to prevent half the talons from cutting out
    new WaitCommand(0.1).start();
    new ConstantDrive(0, 0.1);

    autoSelector.getSelected().start();
}

@Override
public void autonomousPeriodic() {
    // RUN COMMANDS IF ANY
    Scheduler.getInstance().run();

    updateSmartDashboard();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
    publishToSmartDashboard();
}

/**
 * This function is called periodically during autonomous
 */
public void autonomousPeriodic() {
    publishToSmartDashboard();
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
public void teleopPeriodic() {
    Scheduler.getInstance().run();
    publishToSmartDashboard();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during autonomous
 */
@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
}

@Override
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
}

@Override
public void teleopPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during autonomous.
 */
public void autonomousPeriodic() {
    Scheduler.getInstance().run();
    isAuto = isAutonomous();
}

@Override
public void disabledPeriodic() {
    Scheduler.getInstance().run();
}

/**
 * This function is called periodically during operator control
 */
public void teleopPeriodic() {
    Scheduler.getInstance().run();
    if (! Robot.pixyCamera.read(0, 1, buffer))
        pixyValue =  buffer[0] & 0xFF;

    lTrigger = oi.getXBoxController().getRawAxis(2);
    rTrigger = oi.getXBoxController().getRawAxis(3);
    pov = oi.getXBoxController().getPOV(0);

    /* Climbing control - variable on Left Trigger */
    if(lTrigger > 0.1)
        new Climb().start();

    /* Shooting Balls - Right Trigger */
    if(rTrigger > 0.1) {
        if(shootMethod1) {
            Shooter.trigger();

        } else {
            //new Shoot();
        }
    } else {
        new ShooterSequenceOff().start();
    }

    if (isIngesting) {
        BallIntake.ballIntakeMotor.set(0.3);
    } else {
        BallIntake.ballIntakeMotor.set(0);
    }

    /* Allow adjusting Speed of Shooter Motor to test distance */
    //if(pov == -1) {
    //  povActivated = false;
    //} else {
        if (pov != -1)
            if(pov > 90 && pov < 270) {
                shootSpeeed--;
            } else {
                shootSpeeed++;
            }

    //}
    SmartDashboard.putNumber("Shooting speeed", shootSpeeed);

}