diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index 80d7042..8d10658 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -1,7 +1,7 @@ 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; @@ -11,10 +11,10 @@ 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.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -43,10 +43,16 @@ public class Auto12BallPedroPathing extends LinearOpMode { Turret turret; Spindexer spindexer; Servos servos; - Timer pathTimer, opModeTimer; + Timer opModeTimer, shootingTimer; + + // Wait Times + public static double shootTime = 2; + + // Extra Variables + public static double intakePower = 0.3; // Initialize path state machine - enum PathState { + private enum PathState { DRIVE_SHOOT0, WAIT_SHOOT0, DRIVE_PICKUP1, @@ -65,42 +71,240 @@ public class Auto12BallPedroPathing extends LinearOpMode { PathState pathState = PathState.DRIVE_SHOOT0; // Poses - public static double startPoseX = 110, startPoseY = 130, startPoseH = -90; - public static double shoot0X = 88, shoot0Y = 82, shoot0H = -45; + public static double startPoseX = 112, startPoseY = 132.5, startPoseH = -90; + public static double shoot0X = 106, shoot0Y = 106, shoot0H = -40; + public static double drivePickup1X = 102, drivePickup1Y = 82, drivePickup1H = 0; + public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; + public static double shoot1X = 86, shoot1Y = 82, shoot1H = -80; + public static double drivePickup2ControlX = 91.69828844730904, drivePickup2ControlY = 66.724457099909; + public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; + public static double pickup2X = 132, pickup2Y = 57, pickup2H = 0; + public static double shoot2ControlX = 86, shoot2ControlY = 57; + public static double shoot2X = 86, shoot2Y = 82, shoot2H = -90; + public static double drivePickup3ControlX = 97.97800291788306, drivePickup3ControlY = 50.10765863138859; + public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; + public static double pickup3X = 132, pickup3Y = 34.5, pickup3H = 0; + public static double shoot3ControlX = 86, shoot3ControlY = 34.5; + public static double shoot3X = 84, shoot3Y = 102, shoot3H = -90; Pose startPose; Pose shoot0; + Pose drivePickup1; + Pose pickup1; + Pose shoot1; + Pose drivePickup2Control; + Pose drivePickup2; + Pose pickup2; + Pose shoot2Control; + Pose shoot2; + Pose drivePickup3Control; + Pose drivePickup3; + Pose pickup3; + Pose shoot3Control; + Pose shoot3; + private void initializePoses(){ + startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); + shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); + drivePickup1 = new Pose(drivePickup1X, drivePickup1Y, Math.toRadians(drivePickup1H)); + pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); + shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); + drivePickup2Control = new Pose(drivePickup2ControlX, drivePickup2ControlY); + 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)); + drivePickup3Control = new Pose(drivePickup3ControlX, drivePickup3ControlY); + 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)); + } // add poses to void //Building Paths PathChain startPose_shoot0; - void buildPaths(){ + PathChain shoot0_drivePickup1; + PathChain drivePickup1_pickup1; + PathChain pickup1_shoot1; + PathChain shoot1_drivePickup2; + PathChain drivePickup2_pickup2; + PathChain pickup2_shoot2; + PathChain shoot2_drivePickup3; + PathChain drivePickup3_pickup3; + PathChain pickup3_shoot3; + private void buildPaths(){ startPose_shoot0 = follower.pathBuilder() .addPath(new BezierLine(startPose, shoot0)) .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) .build(); + + shoot0_drivePickup1 = follower.pathBuilder() + .addPath(new BezierLine(shoot0, drivePickup1)) + .setLinearHeadingInterpolation(shoot0.getHeading(), drivePickup1.getHeading()) + .build(); + + drivePickup1_pickup1 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup1, pickup1)) + .setTangentHeadingInterpolation() + .build(); + + pickup1_shoot1 = follower.pathBuilder() + .addPath(new BezierLine(pickup1, shoot1)) + .setLinearHeadingInterpolation(pickup1.getHeading(), shoot1.getHeading()) + .build(); + + shoot1_drivePickup2 = follower.pathBuilder() + .addPath(new BezierCurve(shoot1, drivePickup2Control, drivePickup2)) + .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading()) + .build(); + + drivePickup2_pickup2 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup2, pickup2)) + .setConstantHeadingInterpolation(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 BezierCurve(shoot2, drivePickup3Control, drivePickup3)) + .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading()) + .build(); + + drivePickup3_pickup3 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup3, pickup3)) + .setTangentHeadingInterpolation() + .build(); + + pickup3_shoot3 = follower.pathBuilder() + .addPath(new BezierCurve(pickup3, shoot3Control, shoot3)) + .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading()) + .build(); } //Path State Machine - void pathStateMachine(){ + private boolean startAuto = true; + private double timeStamp = 0; + private void pathStateMachine(){ + double currentTime = (double) System.currentTimeMillis() / 1000; switch(pathState){ case DRIVE_SHOOT0: - follower.followPath(startPose_shoot0); - pathState = PathState.WAIT_SHOOT0; + if (startAuto){ + follower.followPath(startPose_shoot0, true); + startAuto = false; + } + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT0; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } break; case WAIT_SHOOT0: - boolean shootStart = false; - if (!follower.isBusy() && !shootStart){ - spindexer.prepareShootAllContinous(); - shootStart = true; + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP1; + follower.followPath(shoot0_drivePickup1, true); } - if (shootStart && spindexer.shootAllComplete()){ - //pathState = PathState.DRIVE_PICKUP1; - TELE.addLine("End Auto"); + break; + case DRIVE_PICKUP1: + if (!follower.isBusy()){ + pathState = PathState.PICKUP1; + follower.followPath(drivePickup1_pickup1, intakePower, false); + } + break; + case PICKUP1: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT1; + follower.followPath(pickup1_shoot1, true); + } + break; + case DRIVE_SHOOT1: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT1; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT1: + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP2; + follower.followPath(shoot1_drivePickup2, true); + } + break; + case DRIVE_PICKUP2: + if (!follower.isBusy()){ + pathState = PathState.PICKUP2; + follower.followPath(drivePickup2_pickup2, intakePower, false); + } + break; + case PICKUP2: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT2; + follower.followPath(pickup2_shoot2, true); + } + break; + case DRIVE_SHOOT2: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT2; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT2: + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP3; + follower.followPath(shoot2_drivePickup3, true); + } + break; + case DRIVE_PICKUP3: + if (!follower.isBusy()){ + pathState = PathState.PICKUP3; + follower.followPath(drivePickup3_pickup3, intakePower, false); + } + break; + case PICKUP3: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT3; + follower.followPath(pickup3_shoot3, true); + } + break; + case DRIVE_SHOOT3: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT3; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT3: + if (spindexer.shootAllComplete()){ +// spindexer.resetSpindexer(); + TELE.addLine("Done Auto"); TELE.update(); } break; default: break; } + TELE.update(); + } + + private boolean driveToShoot(){ + return pathState == PathState.DRIVE_SHOOT0 || + pathState == PathState.DRIVE_SHOOT1 || + pathState == PathState.DRIVE_SHOOT2 || + pathState == PathState.DRIVE_SHOOT3; + } + + 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 @@ -115,52 +319,86 @@ public class Auto12BallPedroPathing extends LinearOpMode { 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); opModeTimer = new Timer(); - pathTimer = new Timer(); - hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); + shootingTimer = new Timer(); robot.light.setPosition(Color.LightRed); - int allianceTicks = 0; + boolean initializeRobot = false; while (opModeInInit()){ - boolean initalizeRobot = false; + follower.update(); - if (gamepad1.crossWasPressed() && !initalizeRobot){ - allianceTicks++; + if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; if (Color.redAlliance){ robot.light.setPosition(Color.LightRed); } else { robot.light.setPosition(Color.LightBlue); } - startPoseX = 144 - startPoseX; - startPoseH = 180 - startPoseH; - shoot0X = 144 - shoot0X; - shoot0H = 180 - shoot0H; - while (startPoseH > 180) {startPoseH-=360;} - while (startPoseH < -180) {startPoseH+=360;} + startPoseX = adjustXPoseBasedOnAlliance(startPoseX); + startPoseH = adjustHeadingBasedOnAlliance(startPoseH); - while (shoot0H > 180) {shoot0H-=360;} - while (shoot0H < -180) {shoot0H+=360;} + shoot0X = adjustXPoseBasedOnAlliance(shoot0X); + shoot0H = adjustHeadingBasedOnAlliance(shoot0H); + + drivePickup1X = adjustXPoseBasedOnAlliance(drivePickup1X); + drivePickup1H = adjustHeadingBasedOnAlliance(drivePickup1H); + + pickup1X = adjustXPoseBasedOnAlliance(pickup1X); + pickup1H = adjustHeadingBasedOnAlliance(pickup1H); + + shoot1X = adjustXPoseBasedOnAlliance(shoot1X); + shoot1H = adjustHeadingBasedOnAlliance(shoot1H); + + drivePickup2ControlX = adjustXPoseBasedOnAlliance(drivePickup2ControlX); + + drivePickup2X = adjustXPoseBasedOnAlliance(drivePickup2X); + drivePickup2H = adjustHeadingBasedOnAlliance(drivePickup2H); + + pickup2X = adjustXPoseBasedOnAlliance(pickup2X); + pickup2H = adjustHeadingBasedOnAlliance(pickup2H); + + shoot2ControlX = adjustXPoseBasedOnAlliance(shoot2ControlX); + + shoot2X = adjustXPoseBasedOnAlliance(shoot2X); + shoot2H = adjustHeadingBasedOnAlliance(shoot2H); + + drivePickup3ControlX = adjustXPoseBasedOnAlliance(drivePickup3ControlX); + + drivePickup3X = adjustXPoseBasedOnAlliance(drivePickup3X); + drivePickup3H = adjustHeadingBasedOnAlliance(drivePickup3H); + + pickup3X = adjustXPoseBasedOnAlliance(pickup3X); + pickup3H = adjustHeadingBasedOnAlliance(pickup3H); + + shoot3ControlX = adjustXPoseBasedOnAlliance(shoot3ControlX); + + shoot3X = adjustXPoseBasedOnAlliance(shoot3X); + shoot3H = adjustHeadingBasedOnAlliance(shoot3H); } if (gamepad1.triangleWasPressed()){ - initalizeRobot = true; + initializeRobot = true; } - if (initalizeRobot){ - follower.setStartingPose(startPose); + if (initializeRobot){ + 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)", initalizeRobot); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); TELE.update(); } @@ -168,20 +406,36 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (isStopRequested()) return; + robot.transfer.setPower(1); + limelightUsed = false; + opModeTimer.resetTimer(); + while (opModeIsActive()){ follower.update(); pathStateMachine(); Pose currentPose = follower.getPose(); - teleStartPoseX = currentPose.getX(); - teleStartPoseY = currentPose.getY(); - teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - turret.trackGoal(currentPose); - targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate); +// teleStartPoseX = currentPose.getX(); +// teleStartPoseY = currentPose.getY(); +// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - 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); +// turret.trackGoal(currentPose); +// targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 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(); +// } + +// TELE.addData("X:", currentPose.getX()); +// TELE.addData("Y:", currentPose.getY()); +// TELE.addData("H:", currentPose.getHeading()); +// TELE.update(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index dd67303..1c931de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -21,7 +21,7 @@ public class Constants { .forwardZeroPowerAcceleration(-29.512) .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) - .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02)) .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)) .centripetalScaling(0.0005); @@ -38,7 +38,7 @@ public class Constants { .xVelocity(64.675) .yVelocity(49.583); - public static double breakingStrength = 1.25; + public static double breakingStrength = 1; public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants()