auto coded
This commit is contained in:
@@ -1,341 +1,366 @@
|
||||
//package org.firstinspires.ftc.teamcode.autonomous;
|
||||
//
|
||||
//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
|
||||
//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
|
||||
//import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
//import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
|
||||
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
|
||||
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
|
||||
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
|
||||
//
|
||||
//import com.acmerobotics.dashboard.FtcDashboard;
|
||||
//import com.acmerobotics.dashboard.config.Config;
|
||||
//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
//import com.pedropathing.follower.Follower;
|
||||
//import com.pedropathing.geometry.BezierCurve;
|
||||
//import com.pedropathing.geometry.BezierLine;
|
||||
//import com.pedropathing.geometry.Pose;
|
||||
//import com.pedropathing.paths.PathChain;
|
||||
//import com.qualcomm.hardware.lynx.LynxModule;
|
||||
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
//
|
||||
//import org.firstinspires.ftc.teamcode.constants.Color;
|
||||
//import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
|
||||
//import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
|
||||
//import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
|
||||
//import org.firstinspires.ftc.teamcode.utilsv2.Robot;
|
||||
//import org.firstinspires.ftc.teamcode.utils.Servos;
|
||||
//import org.firstinspires.ftc.teamcode.utils.Spindexer;
|
||||
//import org.firstinspires.ftc.teamcode.utils.Targeting;
|
||||
//import org.firstinspires.ftc.teamcode.utilsv2.Turret;
|
||||
//
|
||||
//import java.util.List;
|
||||
//
|
||||
//@Config
|
||||
//@Autonomous (preselectTeleOp = "TeleopV4")
|
||||
//public class Auto12Ball_Back_Sorted extends LinearOpMode {
|
||||
// Robot robot;
|
||||
// MultipleTelemetry TELE;
|
||||
// // Flywheel flywheel;
|
||||
//// Targeting targeting;
|
||||
//// Targeting.Settings targetingSettings;
|
||||
// Follower follower;
|
||||
// // Turret turret;
|
||||
//// Spindexer spindexer;
|
||||
//// Servos servos;
|
||||
// MeasuringLoopTimes loopTimes;
|
||||
//
|
||||
// // Wait Times
|
||||
// public static double shootTime = 2;
|
||||
//
|
||||
// // Extra Variables
|
||||
// public static double intakePower = 0.3;
|
||||
// double shootX, shootY, shootH;
|
||||
//
|
||||
// // Initialize path state machine
|
||||
// private enum PathState {
|
||||
// PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
|
||||
// PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
|
||||
// DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
|
||||
// DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
|
||||
// }
|
||||
// PathState pathState = PathState.DRIVE_SHOOT0;
|
||||
//
|
||||
// // Poses
|
||||
// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
|
||||
// public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
|
||||
// public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
|
||||
// public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
|
||||
// public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
|
||||
// public static double openGateX = 129, openGateY = 74, openGateH = 0;
|
||||
// public static double shoot1ControlX = 112, shoot1ControlY = 75;
|
||||
// public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
|
||||
// public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
|
||||
// public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
|
||||
// public static double shoot2ControlX = 102, shoot2ControlY = 63;
|
||||
// public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
|
||||
// public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
|
||||
// public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
|
||||
// public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
|
||||
// public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
|
||||
// Pose startPose, pushBot, shoot0,
|
||||
// pickup1, openGateControl, openGate, shoot1Control, shoot1,
|
||||
// drivePickup2, pickup2, shoot2Control, shoot2,
|
||||
// drivePickup3, pickup3, shoot3Control, shoot3;
|
||||
// private void initializePoses(){
|
||||
// startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
|
||||
// pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
|
||||
// shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
|
||||
// pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
|
||||
// openGateControl = new Pose(openGateControlX, openGateControlY);
|
||||
// openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
|
||||
// shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
|
||||
// shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
|
||||
// drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
|
||||
// pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
|
||||
// shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
|
||||
// shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
|
||||
// drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
|
||||
// pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
|
||||
// shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
|
||||
// shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
|
||||
// }
|
||||
//
|
||||
// //Building Paths
|
||||
// PathChain startPose_pushBot, pushBot_shoot0,
|
||||
// shoot0_pickup1, pickup1_openGate, openGate_shoot1,
|
||||
// shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
|
||||
// shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
|
||||
// private void buildPaths(){
|
||||
// startPose_pushBot = follower.pathBuilder()
|
||||
// .addPath(new BezierLine(startPose, pushBot))
|
||||
// .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
|
||||
// .build();
|
||||
// }
|
||||
//
|
||||
// //Path State Machine
|
||||
// private boolean startAuto = true;
|
||||
// private double timeStamp = 0;
|
||||
// private void pathStateMachine(){
|
||||
// double currentTime = (double) System.currentTimeMillis() / 1000;
|
||||
// switch(pathState){
|
||||
// case DRIVE_SHOOT0:
|
||||
// if (startAuto){
|
||||
// follower.followPath(startPose_shoot0, true);
|
||||
// startAuto = false;
|
||||
// shootX = shoot0X;
|
||||
// shootY = shoot0Y;
|
||||
// shootH = shoot0H;
|
||||
// }
|
||||
// driveShoot(PathState.WAIT_SHOOT0, currentTime);
|
||||
// break;
|
||||
// case WAIT_SHOOT0:
|
||||
// waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
|
||||
// break;
|
||||
// case DRIVE_PICKUP1:
|
||||
// drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
|
||||
// break;
|
||||
// case PICKUP1:
|
||||
// pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
|
||||
// shootX = shoot1X;
|
||||
// shootY = shoot1Y;
|
||||
// shootH = shoot1H;
|
||||
// break;
|
||||
// case DRIVE_SHOOT1:
|
||||
// intakePowerDown(timeStamp, currentTime);
|
||||
// driveShoot(PathState.WAIT_SHOOT1, currentTime);
|
||||
// break;
|
||||
// case WAIT_SHOOT1:
|
||||
// waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
|
||||
// break;
|
||||
// case DRIVE_PICKUP2:
|
||||
// drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
|
||||
// break;
|
||||
// case PICKUP2:
|
||||
// pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
|
||||
// shootX = shoot2X;
|
||||
// shootY = shoot2Y;
|
||||
// shootH = shoot2H;
|
||||
// break;
|
||||
// case DRIVE_SHOOT2:
|
||||
// intakePowerDown(timeStamp, currentTime);
|
||||
// driveShoot(PathState.WAIT_SHOOT2, currentTime);
|
||||
// break;
|
||||
// case WAIT_SHOOT2:
|
||||
// waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
|
||||
// break;
|
||||
// case DRIVE_PICKUP3:
|
||||
// drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
|
||||
// break;
|
||||
// case PICKUP3:
|
||||
// pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
|
||||
// shootX = shoot3X;
|
||||
// shootY = shoot3Y;
|
||||
// shootH = shoot3H;
|
||||
// break;
|
||||
// case DRIVE_SHOOT3:
|
||||
// intakePowerDown(timeStamp, currentTime);
|
||||
// driveShoot(PathState.WAIT_SHOOT3, currentTime);
|
||||
// break;
|
||||
// case WAIT_SHOOT3:
|
||||
//// if (spindexer.shootAllComplete()){
|
||||
//// spindexer.resetSpindexer();
|
||||
//// TELE.addLine("Done Auto");
|
||||
//// }
|
||||
// break;
|
||||
// default:
|
||||
// break;
|
||||
// }
|
||||
// TELE.update(); // use for debugging
|
||||
// }
|
||||
//
|
||||
// // Voids for State Machine
|
||||
// private void intakePowerDown(double stamp, double currentTime) {
|
||||
// double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
|
||||
// if (pow < -1) {pow = 0;}
|
||||
//// spindexer.setIntakePower(pow);
|
||||
// }
|
||||
// private void driveShoot(PathState nextState, double currentTime){
|
||||
// if (!follower.isBusy()){
|
||||
// pathState = nextState;
|
||||
// timeStamp = currentTime;
|
||||
//// spindexer.prepareShootAllContinous();
|
||||
// }
|
||||
// }
|
||||
// private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
|
||||
// if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
|
||||
//// spindexer.resetSpindexer();
|
||||
// pathState = nextState;
|
||||
// follower.followPath(nextPath, true);
|
||||
//// spindexer.setIntakePower(1);
|
||||
// }
|
||||
// }
|
||||
// private void drivePickup(PathState nextState, PathChain nextPath) {
|
||||
// if (!follower.isBusy()) {
|
||||
// pathState = nextState;
|
||||
// follower.followPath(nextPath, intakePower, false);
|
||||
// }
|
||||
// }
|
||||
// private void pickup(PathState nextState, PathChain nextPath) {
|
||||
// if (!follower.isBusy()) {
|
||||
// pathState = nextState;
|
||||
// follower.followPath(nextPath, true);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // Used for changing alliance
|
||||
// private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
|
||||
// private double adjustHeadingBasedOnAlliance(double heading){
|
||||
// heading = 180 - heading;
|
||||
// while (heading > 180) {heading-=360;}
|
||||
// while (heading <= -180) {heading+=360;}
|
||||
// return heading;
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void runOpMode() throws InterruptedException {
|
||||
// Robot.resetInstance();
|
||||
// robot = Robot.getInstance(hardwareMap);
|
||||
// List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
|
||||
// for (LynxModule hub : allHubs) {
|
||||
// hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
|
||||
// }
|
||||
// TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
//// flywheel = new Flywheel(hardwareMap);
|
||||
//// targeting = new Targeting();
|
||||
//// targetingSettings = new Targeting.Settings(0,0);
|
||||
// follower = Constants.createFollower(hardwareMap);
|
||||
// follower.setStartingPose(new Pose(72,72,0));
|
||||
//// turret = new Turret(robot, TELE, robot.limelight);
|
||||
//// spindexer = new Spindexer(hardwareMap);
|
||||
//// servos = new Servos(hardwareMap);
|
||||
// loopTimes = new MeasuringLoopTimes();
|
||||
// loopTimes.init();
|
||||
//
|
||||
//// robot.light.setPosition(Color.LightRed);
|
||||
//
|
||||
// boolean initializeRobot = false;
|
||||
// while (opModeInInit()){
|
||||
// follower.update();
|
||||
//
|
||||
// if (gamepad1.crossWasPressed() && !initializeRobot){
|
||||
// Color.redAlliance = !Color.redAlliance;
|
||||
//// if (Color.redAlliance){
|
||||
//// robot.light.setPosition(Color.LightRed);
|
||||
//// } else {
|
||||
//// robot.light.setPosition(Color.LightBlue);
|
||||
//// }
|
||||
//
|
||||
//// double[] xPoses = {startPoseX, shoot0X,
|
||||
//// drivePickup1X, pickup1X, shoot1X,
|
||||
//// drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
|
||||
//// drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
|
||||
////
|
||||
//// double[] headings = {startPoseH, shoot0H,
|
||||
//// drivePickup1H, pickup1H, shoot1H,
|
||||
//// drivePickup2H, pickup2H, shoot2H,
|
||||
//// drivePickup3H, pickup3H, shoot3H};
|
||||
//
|
||||
//// for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
|
||||
//// for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
|
||||
// }
|
||||
//
|
||||
// if (gamepad1.triangleWasPressed()){
|
||||
// initializeRobot = true;
|
||||
// initializePoses();
|
||||
// follower.setPose(startPose);
|
||||
// buildPaths();
|
||||
// sleep(2000);
|
||||
//
|
||||
//// turret.setTurret(turrDefault);
|
||||
//// servos.setSpinPos(spinStartPos);
|
||||
// }
|
||||
//
|
||||
// TELE.addData("Red Alliance?", Color.redAlliance);
|
||||
// TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
|
||||
// TELE.addData("Start Pose", follower.getPose());
|
||||
// TELE.update();
|
||||
// }
|
||||
//
|
||||
// waitForStart();
|
||||
//
|
||||
// if (isStopRequested()) return;
|
||||
//
|
||||
//// robot.transfer.setPower(1);
|
||||
// limelightUsed = false;
|
||||
//
|
||||
// while (opModeIsActive()){
|
||||
// follower.update();
|
||||
// pathStateMachine();
|
||||
// Pose currentPose = follower.getPose();
|
||||
//// teleStartPoseX = currentPose.getX();
|
||||
//// teleStartPoseY = currentPose.getY();
|
||||
//// teleStartPoseH = Math.toDegrees(currentPose.getHeading());
|
||||
////
|
||||
//// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
|
||||
//// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
|
||||
////
|
||||
//// double voltage = robot.voltage.getVoltage();
|
||||
//// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
|
||||
//// flywheel.manageFlywheel(targetingSettings.flywheelRPM);
|
||||
//// servos.setHoodPos(targetingSettings.hoodAngle);
|
||||
////
|
||||
//// if (driveToShoot()){servos.setSpinPos(spinStartPos);}
|
||||
//// else {spindexer.processIntake();}
|
||||
//
|
||||
// for (LynxModule hub : allHubs) {
|
||||
// hub.clearBulkCache();
|
||||
// }
|
||||
// loopTimes.loop();
|
||||
//
|
||||
// TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
|
||||
// TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
|
||||
// TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
|
||||
// TELE.addData("X:", currentPose.getX());
|
||||
// TELE.addData("Y:", currentPose.getY());
|
||||
// TELE.addData("H:", currentPose.getHeading());
|
||||
// TELE.update();
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.pedropathing.follower.Follower;
|
||||
import com.pedropathing.geometry.BezierCurve;
|
||||
import com.pedropathing.geometry.BezierLine;
|
||||
import com.pedropathing.geometry.Pose;
|
||||
import com.pedropathing.paths.PathChain;
|
||||
import com.qualcomm.hardware.lynx.LynxModule;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.constants.Color;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
|
||||
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
|
||||
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
|
||||
import org.firstinspires.ftc.teamcode.utilsv2.Turret;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
@Config
|
||||
@Autonomous (preselectTeleOp = "TeleopV4")
|
||||
public class Auto12Ball_Back_Sorted extends LinearOpMode {
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
Follower follower;
|
||||
MeasuringLoopTimes loopTimes;
|
||||
|
||||
// Wait Times
|
||||
public static double shootTime = 2;
|
||||
public static double openGateTime = 1.5;
|
||||
|
||||
// Extra Variables
|
||||
public static double intakePower = 0.3;
|
||||
double shootX, shootY, shootH;
|
||||
|
||||
// Initialize path state machine
|
||||
private enum PathState {
|
||||
PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
|
||||
PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
|
||||
DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
|
||||
DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
|
||||
}
|
||||
PathState pathState = PathState.PUSHBOT;
|
||||
|
||||
// Poses
|
||||
public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
|
||||
public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
|
||||
public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454;
|
||||
public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
|
||||
public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031;
|
||||
public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
|
||||
public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
|
||||
public static double openGateX = 129, openGateY = 74, openGateH = 0;
|
||||
public static double shoot1ControlX = 112, shoot1ControlY = 75;
|
||||
public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
|
||||
public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
|
||||
public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
|
||||
public static double shoot2ControlX = 102, shoot2ControlY = 63;
|
||||
public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
|
||||
public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
|
||||
public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
|
||||
public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
|
||||
public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
|
||||
Pose startPose, pushBot, shoot0Control, shoot0,
|
||||
pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
|
||||
drivePickup2, pickup2, shoot2Control, shoot2,
|
||||
drivePickup3, pickup3, shoot3Control, shoot3;
|
||||
private void initializePoses(){
|
||||
startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
|
||||
pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
|
||||
shoot0Control = new Pose(shoot0ControlX, shoot0ControlY);
|
||||
shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
|
||||
pickup1Control = new Pose(pickup1ControlX, pickup1ControlY);
|
||||
pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
|
||||
openGateControl = new Pose(openGateControlX, openGateControlY);
|
||||
openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
|
||||
shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
|
||||
shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
|
||||
drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
|
||||
pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
|
||||
shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
|
||||
shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
|
||||
drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
|
||||
pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
|
||||
shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
|
||||
shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
|
||||
}
|
||||
|
||||
//Building Paths
|
||||
PathChain startPose_pushBot, pushBot_shoot0,
|
||||
shoot0_pickup1, pickup1_openGate, openGate_shoot1,
|
||||
shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
|
||||
shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
|
||||
private void buildPaths(){
|
||||
startPose_pushBot = follower.pathBuilder()
|
||||
.addPath(new BezierLine(startPose, pushBot))
|
||||
.setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
|
||||
.build();
|
||||
|
||||
pushBot_shoot0 = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(pushBot, shoot0Control, shoot0))
|
||||
.setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading())
|
||||
.build();
|
||||
|
||||
shoot0_pickup1 = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
|
||||
.setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
|
||||
.build();
|
||||
|
||||
pickup1_openGate = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(pickup1, openGateControl, openGate))
|
||||
.setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading())
|
||||
.build();
|
||||
|
||||
openGate_shoot1 = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(openGate, shoot1Control, shoot1))
|
||||
.setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading())
|
||||
.build();
|
||||
|
||||
shoot1_drivePickup2 = follower.pathBuilder()
|
||||
.addPath(new BezierLine(shoot1, drivePickup2))
|
||||
.setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading())
|
||||
.build();
|
||||
|
||||
drivePickup2_pickup2 = follower.pathBuilder()
|
||||
.addPath(new BezierLine(drivePickup2, pickup2))
|
||||
.setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading())
|
||||
.build();
|
||||
|
||||
pickup2_shoot2 = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(pickup2, shoot2Control, shoot2))
|
||||
.setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading())
|
||||
.build();
|
||||
|
||||
shoot2_drivePickup3 = follower.pathBuilder()
|
||||
.addPath(new BezierLine(shoot2, drivePickup3))
|
||||
.setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading())
|
||||
.build();
|
||||
|
||||
drivePickup3_pickup3 = follower.pathBuilder()
|
||||
.addPath(new BezierLine(drivePickup3, pickup3))
|
||||
.setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading())
|
||||
.build();
|
||||
|
||||
pickup3_shoot3 = follower.pathBuilder()
|
||||
.addPath(new BezierCurve(pickup3, shoot3Control, shoot3))
|
||||
.setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
|
||||
.build();
|
||||
}
|
||||
|
||||
//Path State Machine
|
||||
private boolean startAuto = true;
|
||||
private double timeStamp = 0;
|
||||
private void pathStateMachine(){
|
||||
double currentTime = (double) System.currentTimeMillis() / 1000;
|
||||
switch(pathState){
|
||||
case PUSHBOT:
|
||||
if (startAuto){
|
||||
follower.followPath(startPose_pushBot, true);
|
||||
startAuto = false;
|
||||
shootX = shoot0X;
|
||||
shootY = shoot0Y;
|
||||
shootH = shoot0H;
|
||||
}
|
||||
if (!follower.isBusy()){
|
||||
follower.followPath(pushBot_shoot0, true);
|
||||
pathState = PathState.DRIVE_SHOOT0;
|
||||
}
|
||||
break;
|
||||
case DRIVE_SHOOT0:
|
||||
if (!follower.isBusy()){
|
||||
timeStamp = currentTime;
|
||||
pathState = PathState.WAIT_SHOOT0;
|
||||
}
|
||||
break;
|
||||
case WAIT_SHOOT0:
|
||||
if (currentTime - timeStamp > shootTime){
|
||||
follower.followPath(shoot0_pickup1, intakePower, false);
|
||||
pathState = PathState.PICKUP1;
|
||||
}
|
||||
break;
|
||||
case PICKUP1:
|
||||
if (!follower.isBusy()){
|
||||
follower.followPath(pickup1_openGate, true);
|
||||
pathState = PathState.OPENGATE;
|
||||
shootX = shoot1X;
|
||||
shootY = shoot1Y;
|
||||
shootH = shoot1H;
|
||||
}
|
||||
break;
|
||||
case DRIVE_OPENGATE:
|
||||
if (!follower.isBusy()){
|
||||
pathState = PathState.OPENGATE;
|
||||
timeStamp = currentTime;
|
||||
}
|
||||
break;
|
||||
case OPENGATE:
|
||||
if (currentTime - timeStamp > openGateTime){
|
||||
follower.followPath(openGate_shoot1, true);
|
||||
pathState = PathState.DRIVE_SHOOT1;
|
||||
}
|
||||
break;
|
||||
case DRIVE_SHOOT1:
|
||||
if (!follower.isBusy()){
|
||||
pathState = PathState.WAIT_SHOOT1;
|
||||
timeStamp = currentTime;
|
||||
}
|
||||
break;
|
||||
case WAIT_SHOOT1:
|
||||
if (currentTime - timeStamp > shootTime){
|
||||
follower.followPath(shoot1_drivePickup2, true);
|
||||
pathState = PathState.DRIVE_PICKUP2;
|
||||
}
|
||||
break;
|
||||
case DRIVE_PICKUP2:
|
||||
if (!follower.isBusy()) {
|
||||
follower.followPath(drivePickup2_pickup2, intakePower, false);
|
||||
pathState = PathState.PICKUP2;
|
||||
}
|
||||
break;
|
||||
case PICKUP2:
|
||||
if (!follower.isBusy()){
|
||||
follower.followPath(pickup2_shoot2, true);
|
||||
pathState = PathState.DRIVE_SHOOT2;
|
||||
}
|
||||
shootX = shoot2X;
|
||||
shootY = shoot2Y;
|
||||
shootH = shoot2H;
|
||||
break;
|
||||
case DRIVE_SHOOT2:
|
||||
if (!follower.isBusy()){
|
||||
pathState = PathState.WAIT_SHOOT2;
|
||||
timeStamp = currentTime;
|
||||
}
|
||||
break;
|
||||
case WAIT_SHOOT2:
|
||||
if (currentTime - timeStamp > shootTime){
|
||||
follower.followPath(shoot2_drivePickup3, true);
|
||||
pathState = PathState.DRIVE_PICKUP3;
|
||||
}
|
||||
break;
|
||||
case DRIVE_PICKUP3:
|
||||
if (!follower.isBusy()){
|
||||
follower.followPath(drivePickup3_pickup3, intakePower, false);
|
||||
pathState = PathState.PICKUP3;
|
||||
}
|
||||
break;
|
||||
case PICKUP3:
|
||||
if (!follower.isBusy()){
|
||||
follower.followPath(pickup3_shoot3, true);
|
||||
pathState = PathState.DRIVE_SHOOT3;
|
||||
}
|
||||
shootX = shoot3X;
|
||||
shootY = shoot3Y;
|
||||
shootH = shoot3H;
|
||||
break;
|
||||
case DRIVE_SHOOT3:
|
||||
if (!follower.isBusy()){
|
||||
pathState = PathState.WAIT_SHOOT3;
|
||||
}
|
||||
break;
|
||||
case WAIT_SHOOT3:
|
||||
// add line here to say "done auto'
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
TELE.update(); // use for debugging
|
||||
}
|
||||
|
||||
// Used for changing alliance
|
||||
private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
|
||||
private double adjustHeadingBasedOnAlliance(double heading){
|
||||
heading = 180 - heading;
|
||||
while (heading > 180) {heading-=360;}
|
||||
while (heading <= -180) {heading+=360;}
|
||||
return heading;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Robot.resetInstance();
|
||||
robot = Robot.getInstance(hardwareMap);
|
||||
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
|
||||
for (LynxModule hub : allHubs) {
|
||||
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
|
||||
}
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
follower = Constants.createFollower(hardwareMap);
|
||||
follower.setStartingPose(new Pose(72,72,0));
|
||||
loopTimes = new MeasuringLoopTimes();
|
||||
loopTimes.init();
|
||||
|
||||
boolean initializeRobot = false;
|
||||
while (opModeInInit()){
|
||||
follower.update();
|
||||
|
||||
if (gamepad1.crossWasPressed() && !initializeRobot){
|
||||
Color.redAlliance = !Color.redAlliance;
|
||||
|
||||
double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
|
||||
pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
|
||||
drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
|
||||
drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
|
||||
|
||||
double[] headings = {startPoseH, pushBotH, shoot0H,
|
||||
pickup1H, openGateH, shoot1H,
|
||||
drivePickup2H, pickup2H, shoot2H,
|
||||
drivePickup3H, pickup3H, shoot3H};
|
||||
|
||||
for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
|
||||
for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
|
||||
}
|
||||
|
||||
if (gamepad1.triangleWasPressed()){
|
||||
initializeRobot = true;
|
||||
initializePoses();
|
||||
follower.setPose(startPose);
|
||||
buildPaths();
|
||||
sleep(2000);
|
||||
}
|
||||
|
||||
TELE.addData("Red Alliance?", Color.redAlliance);
|
||||
TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
|
||||
TELE.addData("Start Pose", follower.getPose());
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
limelightUsed = false;
|
||||
|
||||
while (opModeIsActive()){
|
||||
follower.update();
|
||||
pathStateMachine();
|
||||
Pose currentPose = follower.getPose();
|
||||
teleStartPoseX = currentPose.getX();
|
||||
teleStartPoseY = currentPose.getY();
|
||||
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
|
||||
|
||||
for (LynxModule hub : allHubs) {
|
||||
hub.clearBulkCache();
|
||||
}
|
||||
loopTimes.loop();
|
||||
|
||||
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
|
||||
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
|
||||
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
|
||||
TELE.addData("X:", currentPose.getX());
|
||||
TELE.addData("Y:", currentPose.getY());
|
||||
TELE.addData("H:", currentPose.getHeading());
|
||||
TELE.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user