/** Update steering calibration control */ private void updateCalibrateControl() { double stickAngle = driverstation.getStickAngle(driverstation.JoystickDriverX, driverstation.JoystickDriverY); // Branch into motor being calibrated if (driverstation.getStickDistance(driverstation.JoystickDriverX, driverstation.JoystickDriverY) > 0.5) { if (stickAngle < 0) { if (stickAngle < -0.5) { motorId = RobotMap.BACK_LEFT; } else { motorId = RobotMap.FRONT_LEFT; } } else { if (stickAngle > 0) { if (stickAngle > 0.5) { motorId = RobotMap.BACK_RIGHT; } else { motorId = RobotMap.FRONT_RIGHT; } } } } // Prints selected motor to the driverstation printSelectedMotor(); // Determine calibration mode if (driverstation.JoystickDriverButton3) { Calibration.stopWheelCalibrate(); steerMode = true; } if (driverstation.JoystickDriverButton4 && button4Debounce) { Calibration.toggleWheelCalibrate(); steerMode = false; button4Debounce = false; } if (!driverstation.JoystickDriverButton4) { button4Debounce = true; } // Branch into type of calibration if (steerMode) { driverstation.println("Steering Calibrate", 3); Calibration.updateSteeringCalibrate(motorId); System.out.println(drive.getSteeringAngle(motorId)); } else { driverstation.println("Wheel Calibrate", 3); Calibration.updateWheelCalibrate(motorId); } }
/** Prints the selected motor to the driverstation based on motor id */ private void printSelectedMotor() { switch (motorId) { case RobotMap.FRONT_LEFT: driverstation.println("Selected Motor: FL", 2); break; case RobotMap.FRONT_RIGHT: driverstation.println("Selected Motor: FR", 2); break; case RobotMap.BACK_LEFT: driverstation.println("Selected Motor: BL", 2); break; case RobotMap.BACK_RIGHT: driverstation.println("Selected Motor: BR", 2); break; } }
/** Update control of the shooter */ private void updateNavigatorControl() { if (driverstation.JoystickNavigatorCalibrate) { // lifterobject.moveArms(LifterObject.ARM_UP); } else { // lifterobject.moveArms(LifterObject.ARM_DOWN); } // Print current launch speed to driverstation driverstation.println("Launch Speed: " + (int) (launchSpeed * 100.0) + "%", 2); // Run launch motor on button 3 if (driverstation.JoystickNavigatorButton3) { System.out.println(launchSpeed); shooter.driveLaunchMotor(launchSpeed); } else { shooter.driveLaunchMotor(0.0); } // polls the trigger to see if fire, but only fires if wheel spin button held and at commanded // speed if (driverstation.JoystickNavigatorTrigger && driverstation.JoystickNavigatorButton3 && shooter.atCommandedSpeed()) { shooter.deployFrisbeePneu(Shooter.PNEU_DEPLOY_OUT); } else { shooter.deployFrisbeePneu(Shooter.PNEU_IDLE); } }
/** This function is called periodically during operator control */ public void teleopPeriodic() { // Read driverstation inputs driverstation.readInputs(); // Branch based on mode if (driverstation.JoystickDriverCalibrate) { driverstation.println("Mode: Calibrate", 1); updateCalibrateControl(); } else { driverstation.println("Mode: Drive", 1); updateDriveControl(); updateNavigatorControl(); // compressor.update(); } // Send printed data to driverstation driverstation.sendData(); }