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 new file mode 100644 index 0000000..8d10658 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -0,0 +1,441 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +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.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.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.utils.Turret; +import com.pedropathing.util.Timer; + +import java.util.List; + +@Config +@Autonomous (preselectTeleOp = "TeleopV3") +public class Auto12BallPedroPathing extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Flywheel flywheel; + Targeting targeting; + Targeting.Settings targetingSettings; + Follower follower; + Turret turret; + Spindexer spindexer; + Servos servos; + Timer opModeTimer, shootingTimer; + + // Wait Times + public static double shootTime = 2; + + // Extra Variables + public static double intakePower = 0.3; + + // Initialize path state machine + private enum PathState { + DRIVE_SHOOT0, + WAIT_SHOOT0, + DRIVE_PICKUP1, + PICKUP1, + 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 = 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; + 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 + 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; + } + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT0; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT0: + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP1; + follower.followPath(shoot0_drivePickup1, true); + } + 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 + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + List 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); + opModeTimer = new Timer(); + shootingTimer = new Timer(); + + 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); + } + + startPoseX = adjustXPoseBasedOnAlliance(startPoseX); + startPoseH = adjustHeadingBasedOnAlliance(startPoseH); + + 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()){ + initializeRobot = true; + } + + 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)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); + TELE.update(); + } + + waitForStart(); + + 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); +// +// 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/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 34d7bef..8b8b5a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos0; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; @@ -89,10 +90,6 @@ public class Auto_LT_Close extends LinearOpMode { public static double intake1GateTime = 3.3; public static double lastShootTime = 27; - public static double openGateX = 26; - public static double openGateY = 48; - public static double openGateH = Math.toRadians(155); - Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -124,6 +121,8 @@ public class Auto_LT_Close extends LinearOpMode { double xShootGate, yShootGate, hShootGate; double xLeave, yLeave, hLeave; double xLeaveGate, yLeaveGate, hLeaveGate; + double openGateCloseX = 0, openGateCloseY = 0, openGateCloseH = 0; + double openGateMiddleX = 0, openGateMiddleY = 0, openGateMiddleH = 0; int ballCycles = 3; int prevMotif = 0; @@ -134,6 +133,7 @@ public class Auto_LT_Close extends LinearOpMode { double obeliskTurrPos3 = 0.0; double waitToPickupGate = 0; double obeliskTurrPosAutoStart = 0; + boolean limelightStart = false; // initialize path variables here TrajectoryActionBuilder shoot0 = null; @@ -179,13 +179,14 @@ public class Auto_LT_Close extends LinearOpMode { servos.setTransferPos(transferServo_out); limelightUsed = false; +// Spindexer.teleop = false; robot.light.setPosition(1); hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); while (opModeInInit()) { - if (limelightUsed && !gateCycle){ + if (limelightUsed && !gateCycle && limelightStart){ Actions.runBlocking( autoActions.detectObelisk( 0.1, @@ -207,14 +208,6 @@ public class Auto_LT_Close extends LinearOpMode { } } - if (!gateCycle) { - turret.pipelineSwitch(1); - } else if (redAlliance) { - turret.pipelineSwitch(4); - } else { - turret.pipelineSwitch(2); - } - if (gateCycle) { servos.setHoodPos(hoodGate0Start); } else { @@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode { if (gamepad2.squareWasPressed()) { drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + sleep(100); robot.limelight.start(); limelightUsed = true; + limelightStart = true; gamepad2.rumble(500); } @@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode { if (redAlliance) { robot.light.setPosition(0.28); + if (gateCycle){ + turret.pipelineSwitch(1); + } else { + turret.pipelineSwitch(4); + } + // ---- FIRST SHOT ---- x1 = rx1; y1 = ry1; @@ -307,6 +308,14 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = rPickupGateBY; pickupGateBH = rPickupGateBH; + openGateCloseX = rOpenGateCloseX; + openGateCloseY = rOpenGateCloseY; + openGateCloseH = rOpenGateCloseH; + + openGateMiddleX = rOpenGateMiddleX; + openGateMiddleY = rOpenGateMiddleY; + openGateMiddleH = rOpenGateMiddleH; + obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -314,6 +323,12 @@ public class Auto_LT_Close extends LinearOpMode { } else { robot.light.setPosition(0.6); + if (gateCycle){ + turret.pipelineSwitch(5); + } else { + turret.pipelineSwitch(2); + } + // ---- FIRST SHOT ---- x1 = bx1; y1 = by1; @@ -363,25 +378,34 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; - obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; + openGateCloseX = bOpenGateCloseX; + openGateCloseY = bOpenGateCloseY; + openGateCloseH = bOpenGateCloseH; + + openGateMiddleX = bOpenGateMiddleX; + openGateMiddleY = bOpenGateMiddleY; + openGateMiddleH = bOpenGateMiddleH; + + obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; } - if (gateCycle) { - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); - } else { +// if (gateCycle) { +// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) +// .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); +// } else { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - } +// } if (gateCycle) { pickup2 = shoot0.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateMiddleX, openGateMiddleY), Math.toRadians(openGateMiddleH)); } else { pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) @@ -389,13 +413,14 @@ public class Auto_LT_Close extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); } - if (gateCycle&& withPartner) { +// if (gateCycle && withPartner) { +// shoot2 = pickup2.endTrajectory().fresh() +// .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) +// .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); +// } else + if (gateCycle) { shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); - } else if (gateCycle) { - shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); } else if (ballCycles < 3) { shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -417,10 +442,11 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - pickup1 = gateCycleShoot.endTrajectory().fresh() + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateCloseX, openGateCloseY), Math.toRadians(openGateCloseH)); } else { pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) @@ -431,7 +457,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { shoot1 = pickup1.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } else if (ballCycles < 2) { shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -477,30 +503,13 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - startAutoGate(); + startAuto(); shoot(0.501, 0.501, 0.501); - cycleStackMiddleGate(); + cycleStackClose(); shoot(0.501,0.501, 0.501); - - while (getRuntime() - stamp < endGateTime) { - cycleGateIntake(); - if (getRuntime() - stamp < lastShootTime) { - cycleGateShoot(); - shoot(0.501, 0.501, 0.501); - } - } - cycleStackCloseIntakeGate(); - - if (getRuntime() - stamp < lastShootTime) { - cycleStackCloseShootGate(); - } - + cycleStackMiddle(); shoot(0.501, 0.501, 0.501); - } else { - - - startAuto(); shoot(0.501, 0.501,0.501); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index fbb7d67..763d938 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -1,6 +1,11 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBY; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; @@ -63,8 +68,8 @@ public class Auto_LT_Far extends LinearOpMode { boolean stack3 = true; double xStackPickupA, yStackPickupA, hStackPickupA; double xStackPickupB, yStackPickupB, hStackPickupB; - public static int pickupStackSpeed = 17; - public static int pickupGateSpeed = 25; + public static int pickupStackSpeed = 12; + public static int pickupGateSpeed = 30; int prevMotif = 0; public static double spindexerSpeedIncrease = 0.014; public static double shootAllTime = 2; @@ -74,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 4.5; - public static double intakeGateTime = 8; + public static double intakeStackTime = 5; + public static double intakeGateTime = 3.75; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -117,12 +122,10 @@ public class Auto_LT_Far extends LinearOpMode { robot.limelight.pipelineSwitch(1); turret = new Turret(robot, TELE, robot.limelight); + limelightUsed = false; - turret.setTurret(turrDefault); - servos.setSpinPos(spinStartPos); - - servos.setTransferPos(transferServo_out); +// Spindexer.teleop = false; while (opModeInInit()) { @@ -164,17 +167,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - xStackPickupA = rStackPickupAX; - yStackPickupA = rStackPickupAY; - hStackPickupA = rStackPickupAH; + xStackPickupA = rStackPickupFarAX; + yStackPickupA = rStackPickupFarAY; + hStackPickupA = rStackPickupFarAH; - xStackPickupB = rStackPickupBX; - yStackPickupB = rStackPickupBY; - hStackPickupB = rStackPickupBH; + xStackPickupB = rStackPickupFarBX; + yStackPickupB = rStackPickupFarBY; + hStackPickupB = rStackPickupFarBH; - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; + pickupGateX = rPickupGateXA; + pickupGateY = rPickupGateYA; + pickupGateH = rPickupGateHA; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -184,9 +187,14 @@ public class Auto_LT_Far extends LinearOpMode { if (gamepad2.squareWasPressed()){ turret.pipelineSwitch(4); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + + servos.setSpinPos(spinStartPos); + limelightUsed = true; + + servos.setTransferPos(transferServo_out); } } else { robot.light.setPosition(0.6); @@ -203,17 +211,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - xStackPickupA = bStackPickupAX; - yStackPickupA = bStackPickupAY; - hStackPickupA = bStackPickupAH; + xStackPickupA = bStackPickupFarAX; + yStackPickupA = bStackPickupFarAY; + hStackPickupA = bStackPickupFarAH; - xStackPickupB = bStackPickupBX; - yStackPickupB = bStackPickupBY; - hStackPickupB = bStackPickupBH; + xStackPickupB = bStackPickupFarBX; + yStackPickupB = bStackPickupFarBY; + hStackPickupB = bStackPickupFarBH; - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; + pickupGateX = bPickupGateXA; + pickupGateY = bPickupGateYA; + pickupGateH = bPickupGateHA; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -223,9 +231,14 @@ public class Auto_LT_Far extends LinearOpMode { if (gamepad2.squareWasPressed()){ turret.pipelineSwitch(2); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + limelightUsed = true; + + servos.setSpinPos(spinStartPos); + + servos.setTransferPos(transferServo_out); } } @@ -244,19 +257,16 @@ public class Auto_LT_Far extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH)) - .waitSeconds(0.2) - .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) - .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), + .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH), new TranslationalVelConstraint(pickupGateSpeed)); shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - limelightUsed = true; TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Limelight On?",limelightUsed); TELE.addData("Gate Cycle?", gatePickup); TELE.addData("Pickup Stack?", stack3); TELE.addData("Start Position", autoStart); @@ -275,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode { robot.transfer.setPower(1); startAuto(); - shoot(); + if (redAlliance){ + shoot(autoStartRX, autoStartRY, autoStartRH); + + } else { + shoot(autoStartBX, autoStartBY, autoStartBH); + + } if (stack3){ cycleStackFar(); - shoot(); + shoot(xShoot, yShoot, hShoot); } while (gatePickup && getRuntime() - stamp < endAutoTime){ @@ -288,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode { break; } cycleGatePrepareShoot(); - if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){ - break; - } - shoot(); + shoot(xShoot, yShoot, hShoot); } if (gatePickup || stack3){ @@ -318,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode { } - void shoot(){ + void shoot(double x, double y, double z){ Actions.runBlocking( new ParallelAction( - autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501) + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z) ) ); } void startAuto(){ - Actions.runBlocking( - new ParallelAction( - autoActions.manageShooterAuto( - flywheel0Time, - 0.501, - 0.501, - 0.501, - true - ) - ) - ); + if (redAlliance){ + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartRX, + autoStartRY, + autoStartRH, + true + ) + + ) + ); + + } else { + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartBX, + autoStartBY, + autoStartBH, + true + ) + + ) + ); + } + } void leave3Ball(){ @@ -361,34 +392,17 @@ public class Auto_LT_Far extends LinearOpMode { xStackPickupB, yStackPickupB, hStackPickupB - ), - autoActions.detectObelisk( - intakeStackTime, - xStackPickupB, - yStackPickupB, - posXTolerance, - posYTolerance, - obeliskTurrPos3 ) - ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = 22; - prevMotif = motif; - + servos.setSpinPos(spinStartPos); + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( shoot3.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootStackTime, - motif, - xShoot, - yShoot, - hShoot) + autoActions.manageShooterAuto( + shootStackTime,xShoot, yShoot, hShoot, false + ) ) ); } @@ -398,39 +412,26 @@ public class Auto_LT_Far extends LinearOpMode { new ParallelAction( pickupGate.build(), autoActions.intake( - intakeStackTime, - pickupGateX, - pickupGateY, - pickupGateH - ), - autoActions.detectObelisk( intakeGateTime, pickupGateX, pickupGateY, - posXTolerance, - posYTolerance, - obeliskTurrPos3 + pickupGateH ) ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; } void cycleGatePrepareShoot(){ + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( shootGate.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootGateTime, - motif, + autoActions.manageShooterAuto( + shootGateTime, xShoot, yShoot, - hShoot + hShoot, + false ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 3f07acd..d696af1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -18,6 +18,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -54,8 +56,8 @@ public class AutoActions { public static double firstSpindexShootPos = spinStartPos; private boolean shootForward = true; public int motif = 0; - double spinEndPos = ServoPositions.spinEndPos; - + double spinEndPos = 0.95; + private boolean intaking = false; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { this.robot = rob; this.drive = dri; @@ -427,8 +429,10 @@ public class AutoActions { if ((System.currentTimeMillis() - stamp) > (time * 1000)) { servos.setSpinPos(spindexer_intakePos1); + intaking = false; return false; } else { + intaking = true; return true; } } @@ -523,7 +527,7 @@ public class AutoActions { drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); + Pose2d currentPose = null; //drive.localizer.getPose(); if (ticker == 0) { stamp = System.currentTimeMillis(); @@ -540,10 +544,10 @@ public class AutoActions { ticker++; - double robotX = currentPose.position.x; - double robotY = currentPose.position.y; + double robotX = 0.0;//currentPose.position.x; + double robotY = 0.0;//currentPose.position.y; - double robotHeading = currentPose.heading.toDouble(); + double robotHeading = 0.0;//currentPose.heading.toDouble(); double goalX = -15; double goalY = 0; @@ -552,11 +556,11 @@ public class AutoActions { double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (posX != 0.501) { - deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); + deltaPose = new Pose(posX, posY, Math.toRadians(posH)); } else { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); } Turret.limelightUsed = true; @@ -644,9 +648,9 @@ public class AutoActions { double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (turr == 0.501) { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); if (!detectingObelisk) { turret.trackGoal(deltaPose); } @@ -672,6 +676,44 @@ public class AutoActions { } }; } + + public Action ShakeDrivetrain( + double time + ){ + return new Action() { + int ticker = 0; + double stamp = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0){ + stamp = System.currentTimeMillis(); + } + ticker++; + + double currentStamp = System.currentTimeMillis(); + if (currentStamp - stamp < time*1000 && (intaking || ticker < 50)) { + if (ticker % 10000 < 5000) { + robot.frontLeft.setPower(0.5); + robot.backLeft.setPower(0.5); + robot.frontRight.setPower(0.5); + robot.backRight.setPower(0.5); + } else { + robot.frontLeft.setPower(-0.5); + robot.backLeft.setPower(-0.5); + robot.frontRight.setPower(-0.5); + robot.backRight.setPower(-0.5); + } + return true; + } else { + robot.frontLeft.setPower(0); + robot.backLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backRight.setPower(0); + return false; + } + } + }; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index b4ca67a..9d0f8c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java @@ -72,6 +72,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -489,7 +490,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index c32dc38..1ff9d28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -77,6 +77,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -628,7 +629,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index c5fcfd3..472fd95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -7,23 +7,29 @@ public class Back_Poses { public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; - public static double rShootX = 100, rShootY = 55, rShootH = 90; - public static double bShootX = 100, bShootY = -55, bShootH = -90; + public static double rShootX = 100, rShootY = 60, rShootH = 125.2; + public static double bShootX = 100, bShootY = -60, bShootH = -125.2; - public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140; - public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150; + public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150; - public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; - public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; + public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1; + public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1; - public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140; - public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140; - public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140; - public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190; + public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145; + public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145; + public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1; + public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; + public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140; + public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140; + public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; + public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; + public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; + public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; - public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; - public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; + public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54; + public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -54; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 9a8274f..f75a111 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -7,14 +7,14 @@ import com.acmerobotics.roadrunner.Pose2d; public class Front_Poses { - public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; - public static double bx1 = 20, by1 = -0.5, bh1 = -0.1; + public static double rx1 = 30, ry1 = 5, rh1 = 0.1; + public static double bx1 = 30, by1 = -5, bh1 = -0.1; public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; public static double rx2b = 21, ry2b = 34, rh2b = 140.1; - public static double bx2b = 23, by2b = -36, bh2b = -140.1; + public static double bx2b = 23, by2b = -34, bh2b = -140.1; public static double rx3a = 55, ry3a = 39, rh3a = 140; public static double bx3a = 55, by3a = -39, bh3a = -140; @@ -33,8 +33,8 @@ public class Front_Poses { public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; - public static double rShootX = 40, rShootY = 10, rShootH = 50; - public static double bShootX = 40, bShootY = -10, bShootH = -50; + public static double rShootX = 60, rShootY = 10, rShootH = 50; + public static double bShootX = 60, bShootY = -10, bShootH = -50; public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; @@ -58,5 +58,14 @@ public class Front_Poses { public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180; public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220; + public static double rOpenGateCloseX = 20, rOpenGateCloseY = 35, rOpenGateCloseH = 230; + public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230; + + public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50; + public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50; + public static Pose2d teleStart = new Pose2d(0, 0, 0); + + //For PedroPathing TODO: figure out how to change start poses in auto + public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 775b583..24ad1be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -9,17 +9,17 @@ public class ServoPositions { public static double spindexer_intakePos2 = 0.37; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.56; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.55; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.84; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24; public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4; - public static double spinStartPos = 0.10; - public static double spinEndPos = 0.95; + public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4; + public static double spinStartPos = 0; + public static double spinEndPos = 0.6; - public static double shootAllSpindexerSpeedIncrease = 0.014; + public static double shootAllSpindexerSpeedIncrease = 0.01; public static double transferServo_out = 0.15; @@ -27,7 +27,7 @@ public class ServoPositions { public static double hoodAuto = 0.27; - public static double hoodOffset = -0.04; // offset from 0.93 + public static double hoodOffset = -0.05; // offset from 0.93 (or position at 0,0 in targeting class) public static double turret_redClose = 0; public static double turret_blueClose = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java deleted file mode 100644 index d596ce1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode.libs.pedroPathing; - -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.paths.PathConstraints; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); - - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); - - public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) - .pathConstraints(pathConstraints) - .build(); - } -} \ No newline at end of file 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 new file mode 100644 index 0000000..1c931de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.acmerobotics.dashboard.config.Config; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +@Config +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(15.5) + .forwardZeroPowerAcceleration(-29.512) + .lateralZeroPowerAcceleration(-72.872) + .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)) + .centripetalScaling(0.0005); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .rightFrontMotorName("fr") + .rightRearMotorName("br") + .leftRearMotorName("bl") + .leftFrontMotorName("fl") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(64.675) + .yVelocity(49.583); + + public static double breakingStrength = 1; + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-7.5) + .strafePodX(-3.75) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("pinpoint") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .pinpointLocalizer(localizerConstants) + .build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java similarity index 52% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index da2b0d6..55cc2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -1,29 +1,42 @@ -package org.firstinspires.ftc.teamcode.libs.pedroPathing; +package org.firstinspires.ftc.teamcode.pedroPathing; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.changes; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrent; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrentAndHistory; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.follower; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.stopRobot; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.telemetryM; +import static com.pedropathing.math.MathFunctions.quadraticFit; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrentAndHistory; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; + +import android.annotation.SuppressLint; import com.bylazar.configurables.PanelsConfigurables; -import com.bylazar.configurables.annotations.Configurable; -import com.bylazar.configurables.annotations.IgnoreConfigurable; import com.bylazar.field.FieldManager; import com.bylazar.field.PanelsField; import com.bylazar.field.Style; import com.bylazar.telemetry.PanelsTelemetry; import com.bylazar.telemetry.TelemetryManager; import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.*; -import com.pedropathing.math.*; -import com.pedropathing.paths.*; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; import com.pedropathing.telemetry.SelectableOpMode; -import com.pedropathing.util.*; +import com.pedropathing.util.PoseHistory; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.util.ElapsedTime; import java.util.ArrayList; import java.util.List; @@ -34,24 +47,22 @@ import java.util.List; * @author Baron Henderson - 20077 The Indubitables * @version 1.0, 6/26/2025 */ -@Configurable +@Config @TeleOp(name = "Tuning", group = "Pedro Pathing") public class Tuning extends SelectableOpMode { public static Follower follower; - @IgnoreConfigurable static PoseHistory poseHistory; - @IgnoreConfigurable static TelemetryManager telemetryM; - @IgnoreConfigurable static ArrayList changes = new ArrayList<>(); public Tuning() { super("Select a Tuning OpMode", s -> { s.folder("Localization", l -> { l.add("Localization Test", LocalizationTest::new); + l.add("Offsets Tuner", OffsetsTuner::new); l.add("Forward Tuner", ForwardTuner::new); l.add("Lateral Tuner", LateralTuner::new); l.add("Turn Tuner", TurnTuner::new); @@ -61,6 +72,7 @@ public class Tuning extends SelectableOpMode { a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + a.add("Predictive Braking Tuner", PredictiveBrakingTuner::new); }); s.folder("Manual", p -> { p.add("Translational Tuner", TranslationalTuner::new); @@ -73,6 +85,11 @@ public class Tuning extends SelectableOpMode { p.add("Triangle", Triangle::new); p.add("Circle", Circle::new); }); + s.folder("Swerve", p-> { + p.add("Analog Min / Max Tuner", AnalogMinMaxTuner::new); + p.add("Swerve Offsets Test", SwerveOffsetsTest::new); + p.add("Swerve Turn Test", SwerveTurnTest::new); + }); }); } @@ -97,7 +114,7 @@ public class Tuning extends SelectableOpMode { public static void drawCurrent() { try { - Drawing.drawRobot(follower.getPose()); + Drawing.drawRobot(follower.getPose(), "green"); Drawing.sendPacket(); } catch (Exception e) { throw new RuntimeException("Drawing failed " + e); @@ -105,7 +122,7 @@ public class Tuning extends SelectableOpMode { } public static void drawCurrentAndHistory() { - Drawing.drawPoseHistory(poseHistory); + Drawing.drawPoseHistory(poseHistory, "blue"); drawCurrent(); } @@ -117,24 +134,36 @@ public class Tuning extends SelectableOpMode { } /** - * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * This is the LocalizationTest OpMode. This is basically just a simple drive attached to a * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. * You should use this to check the robot's localization. * * @author Anyi Lin - 10158 Scott's Bots * @author Baron Henderson - 20077 The Indubitables + * @author Kabir Goyal * @version 1.0, 5/6/2024 */ class LocalizationTest extends OpMode { - @Override - public void init() {} + boolean debugStringEnabled = false; - /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init() { + follower.setStartingPose(new Pose(72,72, 0)); + } + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("This will print your robot's position to telemetry while " - + "allowing robot control through a basic mecanum drive on gamepad 1."); - telemetryM.update(telemetry); + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + + telemetry.addLine("This will print your robot's position to telemetry while " + + "allowing robot control through a basic drive on gamepad 1."); + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); + telemetry.update(); follower.update(); drawCurrent(); } @@ -146,19 +175,27 @@ class LocalizationTest extends OpMode { } /** - * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * This updates the robot's pose estimate, the simple drive, and updates the * Panels telemetry with the robot's position as well as draws the robot's position. */ @Override public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); follower.update(); - telemetryM.debug("x:" + follower.getPose().getX()); - telemetryM.debug("y:" + follower.getPose().getY()); - telemetryM.debug("heading:" + follower.getPose().getHeading()); - telemetryM.debug("total heading:" + follower.getTotalHeading()); - telemetryM.update(telemetry); + telemetry.addLine("x:" + follower.getPose().getX()); + telemetry.addLine("y:" + follower.getPose().getY()); + telemetry.addLine("heading:" + follower.getPose().getHeading()); + telemetry.addLine("total heading:" + follower.getTotalHeading()); + if (debugStringEnabled) { + telemetry.addLine("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } + telemetry.update(); drawCurrentAndHistory(); } @@ -182,6 +219,7 @@ class ForwardTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -189,8 +227,8 @@ class ForwardTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -202,10 +240,10 @@ class ForwardTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Distance Moved: " + follower.getPose().getX()); - telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getX()); + telemetry.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -229,6 +267,7 @@ class LateralTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -236,8 +275,8 @@ class LateralTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -249,10 +288,10 @@ class LateralTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Distance Moved: " + follower.getPose().getY()); - telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getY()); + telemetry.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -276,6 +315,7 @@ class TurnTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -283,8 +323,8 @@ class TurnTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -297,10 +337,10 @@ class TurnTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); - telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); - telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Total Angle: " + follower.getTotalHeading()); + telemetry.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetry.addLine("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -312,7 +352,7 @@ class TurnTuner extends OpMode { * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * empirically represents the direction your wheels actually prefer to go in, allowing for * more accurate following. * * @author Anyi Lin - 10158 Scott's Bots @@ -323,23 +363,25 @@ class TurnTuner extends OpMode { */ class ForwardVelocityTuner extends OpMode { private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; + public static double DISTANCE = 120; public static double RECORD_NUMBER = 10; private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.debug("pose", follower.getPose()); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.addData("pose", follower.getPose()); + telemetry.update(); follower.update(); drawCurrent(); @@ -374,7 +416,7 @@ class ForwardVelocityTuner extends OpMode { if (!end) { - if (Math.abs(follower.getPose().getX()) > DISTANCE) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { end = true; stopRobot(); } else { @@ -391,15 +433,15 @@ class ForwardVelocityTuner extends OpMode { average += velocity; } average /= velocities.size(); - telemetryM.debug("Forward Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + telemetry.addLine("Forward Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Velocity temporarily (while robot remains on)."); for (int i = 0; i < velocities.size(); i++) { telemetry.addData(String.valueOf(i), velocities.get(i)); } - telemetryM.update(telemetry); + telemetry.update(); telemetry.update(); if (gamepad1.aWasPressed()) { @@ -417,7 +459,7 @@ class ForwardVelocityTuner extends OpMode { * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * empirically represents the direction your wheels actually prefer to go in, allowing for * more accurate following. * * @author Anyi Lin - 10158 Scott's Bots @@ -429,13 +471,15 @@ class ForwardVelocityTuner extends OpMode { class LateralVelocityTuner extends OpMode { private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; + public static double DISTANCE = 120; public static double RECORD_NUMBER = 10; private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** * This initializes the drive motors as well as the cache of velocities and the Panels @@ -443,11 +487,11 @@ class LateralVelocityTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); @@ -480,7 +524,7 @@ class LateralVelocityTuner extends OpMode { drawCurrentAndHistory(); if (!end) { - if (Math.abs(follower.getPose().getY()) > DISTANCE) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { end = true; stopRobot(); } else { @@ -497,10 +541,10 @@ class LateralVelocityTuner extends OpMode { } average /= velocities.size(); - telemetryM.debug("Strafe Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Strafe Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.setYVelocity(average); @@ -537,17 +581,19 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** This initializes the drive motors as well as the Panels telemetryM. */ @Override public void init_loop() { - telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -601,10 +647,10 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Forward Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setForwardZeroPowerAcceleration(average); @@ -639,17 +685,19 @@ class LateralZeroPowerAccelerationTuner extends OpMode { private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** This initializes the drive motors as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -703,10 +751,10 @@ class LateralZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setLateralZeroPowerAcceleration(average); @@ -717,6 +765,183 @@ class LateralZeroPowerAccelerationTuner extends OpMode { } } +/** + * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power + * levels, recording the robot’s velocity and position immediately before braking. The motors are + * then set to a reverse power, which represents the fastest theoretical braking the robot + * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance. + * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a + * quadratic curve to model the braking behavior. + * + * @author Ashay Sarda - 19745 Turtle Walkers + * @author Jacob Ophoven - 18535 Frozen Code + * @version 1.0, 12/26/2025 + */ +class PredictiveBrakingTuner extends OpMode { + private static final double[] TEST_POWERS = + {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; + private static final double BRAKING_POWER = -0.2; + + private static final int DRIVE_TIME_MS = 1000; + + private enum State { + START_MOVE, + WAIT_DRIVE_TIME, + APPLY_BRAKE, + WAIT_BRAKE_TIME, + RECORD, + DONE + } + + private static class BrakeRecord { + double timeMs; + Pose pose; + double velocity; + + BrakeRecord(double timeMs, Pose pose, double velocity) { + this.timeMs = timeMs; + this.pose = pose; + this.velocity = velocity; + } + } + + private State state = State.START_MOVE; + + private final ElapsedTime timer = new ElapsedTime(); + + private int iteration = 0; + + private Vector startPosition; + private double measuredVelocity; + + private final List velocityToBrakingDistance = new ArrayList<>(); + private final List brakeData = new ArrayList<>(); + + @Override + public void init() {} + + @Override + public void init_loop() { + telemetry.addLine("The robot will move forwards and backwards starting at max speed and slowing down."); + telemetry.addLine("Make sure you have enough room. Leave at least 4-5 feet."); + telemetry.addLine("After stopping, kFriction and kBraking will be displayed."); + telemetry.addLine("Make sure to turn the timer off."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + timer.reset(); + follower.update(); + follower.startTeleOpDrive(true); + } + + @SuppressLint("DefaultLocale") + @Override + public void loop() { + follower.update(); + + if (gamepad1.b) { + stopRobot(); + requestOpModeStop(); + return; + } + + double direction = (iteration % 2 == 0) ? 1 : -1; + + switch (state) { + case START_MOVE: { + if (iteration >= TEST_POWERS.length) { + state = State.DONE; + break; + } + + double currentPower = TEST_POWERS[iteration]; + follower.setMaxPower(currentPower); + follower.setTeleOpDrive(direction, 0, 0, true); + + timer.reset(); + state = State.WAIT_DRIVE_TIME; + break; + } + + case WAIT_DRIVE_TIME: { + if (timer.milliseconds() >= DRIVE_TIME_MS) { + measuredVelocity = follower.getVelocity().getMagnitude(); + startPosition = follower.getPose().getAsVector(); + state = State.APPLY_BRAKE; + } + break; + } + + case APPLY_BRAKE: { + follower.setTeleOpDrive(BRAKING_POWER * direction, 0, 0, true); + + timer.reset(); + state = State.WAIT_BRAKE_TIME; + break; + } + + case WAIT_BRAKE_TIME: { + double t = timer.milliseconds(); + Pose currentPose = follower.getPose(); + double currentVelocity = follower.getVelocity().getMagnitude(); + + brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); + + if (follower.getVelocity().dot(new Vector(direction, + follower.getHeading())) <= 0) { + state = State.RECORD; + } + break; + } + + case RECORD: { + Vector endPosition = follower.getPose().getAsVector(); + double brakingDistance = endPosition.minus(startPosition).getMagnitude(); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + telemetry.addData("Test " + iteration, String.format("v=%.3f d=%.3f", measuredVelocity, brakingDistance)); + telemetry.update(); + + iteration++; + state = State.START_MOVE; + + break; + } + + case DONE: { + stopRobot(); + + double[] coefficients = quadraticFit(velocityToBrakingDistance); + + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadratic", coefficients[1]); + telemetry.addData("kLinear", coefficients[0]); + telemetry.update(); + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadraticFriction", coefficients[1]); + telemetry.addData("kLinearBraking", coefficients[0]); + for (BrakeRecord record : brakeData) { + Pose p = record.pose; + telemetry.addLine(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", + record.timeMs, p.getX(), p.getY(), + p.getHeading(), + record.velocity)); + } + telemetry.update(); + break; + } + } + } +} + /** * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. @@ -735,15 +960,17 @@ class TranslationalTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** This initializes the Follower and creates the forward and backward Paths. */ @Override public void init_loop() { - telemetryM.debug("This will activate the translational PIDF(s)"); - telemetryM.debug("The robot will try to stay in place while you push it laterally."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the translational PIDF(s)"); + telemetry.addLine("The robot will try to stay in place while you push it laterally."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -775,8 +1002,11 @@ class TranslationalTuner extends OpMode { } } - telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); + telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); + telemetry.update(); } } @@ -799,7 +1029,9 @@ class HeadingTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** * This initializes the Follower and creates the forward and backward Paths. Additionally, this @@ -807,10 +1039,10 @@ class HeadingTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will activate the heading PIDF(s)."); - telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the heading PIDF(s)."); + telemetry.addLine("The robot will try to stay at a constant heading while you try to turn it."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -819,9 +1051,9 @@ class HeadingTuner extends OpMode { public void start() { follower.deactivateAllPIDFs(); follower.activateHeading(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + backwards = new Path(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -845,8 +1077,10 @@ class HeadingTuner extends OpMode { } } - telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); + telemetry.update(); } } @@ -867,7 +1101,9 @@ class DriveTuner extends OpMode { private PathChain backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** * This initializes the Follower and creates the forward and backward Paths. Additionally, this @@ -875,10 +1111,10 @@ class DriveTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); - telemetryM.debug("The robot will go forward and backward continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); + telemetry.addLine("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetry.addLine("The robot will go forward and backward continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -890,13 +1126,13 @@ class DriveTuner extends OpMode { forwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))) + .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))) .setConstantHeadingInterpolation(0) .build(); backwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))) + .addPath(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))) .setConstantHeadingInterpolation(0) .build(); @@ -922,8 +1158,10 @@ class DriveTuner extends OpMode { } } - telemetryM.debug("Driving forward?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving forward?: " + forward); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); + telemetry.update(); } } @@ -945,15 +1183,17 @@ class Line extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** This initializes the Follower and creates the forward and backward Paths. */ @Override public void init_loop() { - telemetryM.debug("This will activate all the PIDF(s)"); - telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate all the PIDF(s)"); + telemetry.addLine("The robot will go forward and backward continuously along the path while correcting."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -961,9 +1201,9 @@ class Line extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + backwards = new Path(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -984,8 +1224,8 @@ class Line extends OpMode { } } - telemetryM.debug("Driving Forward?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving Forward?: " + forward); + telemetry.update(); } } @@ -1010,7 +1250,9 @@ class CentripetalTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } /** * This initializes the Follower and creates the forward and backward Paths. @@ -1018,10 +1260,10 @@ class CentripetalTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); - telemetryM.debug("The robot will go continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); + telemetry.addLine("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetry.addLine("The robot will go continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1029,8 +1271,8 @@ class CentripetalTuner extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0))); + forwards = new Path(new BezierCurve(new Pose(0,0), new Pose(Math.abs(DISTANCE) ,0), new Pose(Math.abs(DISTANCE) ,DISTANCE ))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0))); backwards.setTangentHeadingInterpolation(); backwards.reverseHeadingInterpolation(); @@ -1056,8 +1298,8 @@ class CentripetalTuner extends OpMode { } } - telemetryM.debug("Driving away from the origin along the curve?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving away from the origin along the curve?: " + forward); + telemetry.update(); } } @@ -1071,7 +1313,7 @@ class CentripetalTuner extends OpMode { */ class Triangle extends OpMode { - private final Pose startPose = new Pose(0, 0, Math.toRadians(0)); + private final Pose startPose = new Pose(0,0, Math.toRadians(0)); private final Pose interPose = new Pose(24, -24, Math.toRadians(90)); private final Pose endPose = new Pose(24, 24, Math.toRadians(45)); @@ -1092,13 +1334,15 @@ class Triangle extends OpMode { } @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(0,0)); + } @Override public void init_loop() { - telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); - telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetry.addLine("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1138,30 +1382,32 @@ class Circle extends OpMode { public void start() { circle = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) .build(); follower.followPath(circle); } @Override public void init_loop() { - telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); - telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); - telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetry.addLine("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetry.addLine("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetry.update(); follower.update(); drawCurrent(); } @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This runs the OpMode, updating the Follower as well as printing out the debug statements to @@ -1179,147 +1425,350 @@ class Circle extends OpMode { } /** - * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. - * - * @author Lazar - 19234 - * @version 1.1, 5/19/2025 + * Tuning OpMode to get the min and max encoder values for swerve pods + * @author Kabir Goyal */ -class Drawing { - public static final double ROBOT_RADIUS = 9; // woah - private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); +class AnalogMinMaxTuner extends OpMode { + //populate the below with your names for the servos and encoders + public String[] encoderNames = {"leftFrontEncoder", "rightFrontEncoder", "leftBackEncoder", "rightBackEncoder"}; + public AnalogInput[] encoders = new AnalogInput[encoderNames.length]; + public double[] minVoltages = new double[encoderNames.length]; + public double[] maxVoltages = new double[encoderNames.length]; - private static final Style robotLook = new Style( - "", "#3F51B5", 0.0 - ); - private static final Style historyLook = new Style( - "", "#4CAF50", 0.0 - ); + public List lynxModules; //js to improve loop times a bit yk - /** - * This prepares Panels Field for using Pedro Offsets - */ - public static void init() { - panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + public void start() { + } + + @Override + public void init_loop() { + telemetry.addLine("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry."); + telemetry.update(); + } + + @Override + public void init() { + lynxModules = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : lynxModules) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + for (int i = 0; i < encoders.length; i++) { + encoders[i] = hardwareMap.get(AnalogInput.class, encoderNames[i]); + minVoltages[i] = 5; //bigger value than should ever be read + } } + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + for (LynxModule hub : lynxModules) { + hub.clearBulkCache(); + } + + telemetry.addLine("Spin each pod slowly for 4 to 5 full rotations.\n" + + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n"); + + for (int i = 0; i < encoders.length; i++) { + double currentVoltage = encoders[i].getVoltage(); + minVoltages[i] = Math.min(minVoltages[i], currentVoltage); + maxVoltages[i] = Math.max(maxVoltages[i], currentVoltage); + telemetryM.addData(encoderNames[i] + "min value:", minVoltages[i]); + telemetryM.addData(encoderNames[i] + "max value:", maxVoltages[i]); + telemetryM.addLine(""); + } + + telemetry.update(); + } +} + +/** + * This is the SwerveOffsetsTest + * You should use this to check how good your swerve angle offsets are and if your motor directions are correct + * @author Kabir Goyal + * + */ +class SwerveOffsetsTest extends OpMode { + boolean debugStringEnabled = false; + + @Override + public void init() {} + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + + telemetry.addLine("This OpMode will run all four swerve pods in the direction they think is forward" + + "\nensure your bot is not on the ground while running"); + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); + telemetry.update(); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + follower.setTeleOpDrive(0.25, 0, 0, true); + follower.update(); + + if (debugStringEnabled) { + telemetry.addLine("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } + telemetry.update(); + + drawCurrentAndHistory(); + } +} + +/** + * This is the SwerveTurnTest + * You should use this to check your encoder directions and x/y pod offsets + * @author Kabir Goyal + * + */ +class SwerveTurnTest extends OpMode { + boolean debugStringEnabled = false; + + @Override + public void init() {} + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + + telemetry.addLine("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " + + "\nrun this once off the ground to check servo directions and motor directions before testing on the ground"); + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); + telemetry.update(); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + follower.setTeleOpDrive(0, 0, 0.25, true); + follower.update(); + + if (debugStringEnabled) { + telemetry.addLine("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } + telemetry.update(); + + drawCurrentAndHistory(); + } +} + +/** + * This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees, + * and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets + * are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants. + * + * @author Havish Sripada - 12808 RevAmped Robotics + * @author Baron Henderson + */ +class OffsetsTuner extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(0,0)); + follower.update(); + drawCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetry.addLine("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); + telemetry.addLine("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); + telemetry.update(); + + drawCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated offsets and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetry.addLine("Total Angle: " + follower.getTotalHeading()); + + telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer."); + telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); + telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetry.update(); + + drawCurrentAndHistory(); + } +} + + +/** + * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 2.0, 11/03/2025 + */ +class Drawing { + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; + /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes - * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * a Follower as an input, so an instance of the DashboardDrawingHandler class is not needed. * - * @param follower Pedro Follower instance. + * @param follower */ public static void drawDebug(Follower follower) { if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), robotLook); + drawPath(follower.getCurrentPath(), "#3F51B5"); Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); - drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); } - drawPoseHistory(follower.getPoseHistory(), historyLook); - drawRobot(follower.getPose(), historyLook); - + drawPoseHistory(follower.getPoseHistory(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); sendPacket(); } /** - * This draws a robot at a specified Pose with a specified - * look. The heading is represented as a line. + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. * - * @param pose the Pose to draw the robot at - * @param style the parameters used to draw the robot with + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with */ - public static void drawRobot(Pose pose, Style style) { - if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + public static void drawRobot(Pose pose, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); + } + + /** + * This adds instructions to the current packet to draw a Path with a specified color. If no + * packet exists, then a new one is created. + * + * @param path the Path to draw + * @param color the color to draw the Path with + */ + public static void drawPath(Path path, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + Drawing.drawPath(packet.fieldOverlay(), path.getPanelsDrawingPoints()); + } + + /** + * This adds instructions to the current packet to draw all the Paths in a PathChain with a + * specified color. If no packet exists, then a new one is created. + * + * @param pathChain the PathChain to draw + * @param color the color to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, String color) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), color); + } + } + + /** + * This adds instructions to the current packet to draw the pose history of the robot. If no + * packet exists, then a new one is created. + * + * @param poseTracker the DashboardPoseTracker to get the pose history from + * @param color the color to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); + } + + /** + * This tries to send the current packet to FTC Dashboard. + * + * @return returns if the operation was successful. + */ + public static boolean sendPacket() { + if (packet != null) { + FtcDashboard.getInstance().sendTelemetryPacket(packet); + packet = null; + return true; + } + return false; + } + + /** + * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the + * actual robot, since the Pose contains the direction the robot is facing as well as its position. + * + * @param c the Canvas on the Dashboard on which this will draw at + * @param t the Pose to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Pose t) { + if (t == null || Double.isNaN(t.getX()) || Double.isNaN(t.getY()) || Double.isNaN(t.getHeading())) { return; } - panelsField.setStyle(style); - panelsField.moveCursor(pose.getX(), pose.getY()); - panelsField.circle(ROBOT_RADIUS); - - Vector v = pose.getHeadingAsUnitVector(); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingAsUnitVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; - double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); - - panelsField.setStyle(style); - panelsField.moveCursor(x1, y1); - panelsField.line(x2, y2); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, x2, y2); } /** - * This draws a robot at a specified Pose. The heading is represented as a line. + * This draws a Path on the Dashboard from a specified Array of Points. * - * @param pose the Pose to draw the robot at + * @param c the Canvas on the Dashboard on which this will draw + * @param points the Points to draw */ - public static void drawRobot(Pose pose) { - drawRobot(pose, robotLook); - } - - /** - * This draws a Path with a specified look. - * - * @param path the Path to draw - * @param style the parameters used to draw the Path with - */ - public static void drawPath(Path path, Style style) { - double[][] points = path.getPanelsDrawingPoints(); - - for (int i = 0; i < points[0].length; i++) { - for (int j = 0; j < points.length; j++) { - if (Double.isNaN(points[j][i])) { - points[j][i] = 0; - } - } - } - - panelsField.setStyle(style); - panelsField.moveCursor(points[0][0], points[0][1]); - panelsField.line(points[1][0], points[1][1]); - } - - /** - * This draws all the Paths in a PathChain with a - * specified look. - * - * @param pathChain the PathChain to draw - * @param style the parameters used to draw the PathChain with - */ - public static void drawPath(PathChain pathChain, Style style) { - for (int i = 0; i < pathChain.size(); i++) { - drawPath(pathChain.getPath(i), style); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - * @param style the parameters used to draw the pose history with - */ - public static void drawPoseHistory(PoseHistory poseTracker, Style style) { - panelsField.setStyle(style); - - int size = poseTracker.getXPositionsArray().length; - for (int i = 0; i < size - 1; i++) { - - panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); - panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - */ - public static void drawPoseHistory(PoseHistory poseTracker) { - drawPoseHistory(poseTracker, historyLook); - } - - /** - * This tries to send the current packet to FTControl Panels. - */ - public static void sendPacket() { - panelsField.update(); + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 8320805..c7b175e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -2,6 +2,10 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -11,13 +15,17 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDFController; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Light; @@ -33,13 +41,15 @@ import java.util.List; @Config @TeleOp public class TeleopV3 extends LinearOpMode { + private double metersToInches = 39.3700787402; public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; - + private double predictedResetX, predictedResetY, predictedResetH; + public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0; + public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180; public static double spinPow = 0.09; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; - public static double spinSpeedIncrease = 0.03; - public static int resetSpinTicks = 4; + public static int resetSpinTicks = 0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; public double vel = 3000; @@ -53,12 +63,13 @@ public class TeleopV3 extends LinearOpMode { Light light; Servos servo; Flywheel flywheel; - MecanumDrive drive; +// MecanumDrive drive; Spindexer spindexer; Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; MeasuringLoopTimes loopTimes; + Follower follower; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -76,12 +87,11 @@ public class TeleopV3 extends LinearOpMode { int intakeTicker = 0; private boolean shootAll = false; - boolean relocalize = false; + public static boolean relocalize = false; @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - robot.light.setPosition(0); List allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { @@ -91,12 +101,15 @@ public class TeleopV3 extends LinearOpMode { TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); flywheel = new Flywheel(hardwareMap); - drive = new MecanumDrive(hardwareMap, teleStart); +// drive = new MecanumDrive(hardwareMap, teleStart); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); spindexer = new Spindexer(hardwareMap); targeting = new Targeting(); targetingSettings = new Targeting.Settings(0.0, 0.0); - drivetrain = new Drivetrain(robot, drive); + drivetrain = new Drivetrain(robot, follower); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); @@ -112,23 +125,36 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.MANUAL); limelightUsed = true; + Spindexer.teleop = true; while (opModeInInit()) { + //ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS + if (gamepad1.crossWasPressed()){ + redAlliance = !redAlliance; + } + robot.limelight.start(); if (redAlliance) { turret.pipelineSwitch(4); light.setManualLightColor(Color.LightRed); + predictedResetX = redPredictedResetX; + predictedResetY = redPredictedResetY; + predictedResetH = Math.toRadians(redPredictedResetH); } else { turret.pipelineSwitch(2); light.setManualLightColor(Color.LightBlue); - + predictedResetX = bluePredictedResetX; + predictedResetY = bluePredictedResetY; + predictedResetH = Math.toRadians(bluePredictedResetH); } - robot.light.setPosition(1); + limelightUsed = true; + + TELE.addData("Red Alliance?", redAlliance); + TELE.update(); light.update(); } - limelightUsed = true; waitForStart(); if (isStopRequested()) return; @@ -139,10 +165,71 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); + follower.update(); + Pose currentPose = follower.getPose(); + + if (enableSpindexerManager) { + //if (!shootAll) { + spindexer.processIntake(); + //} + + // RIGHT_BUMPER + if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { + spindexer.setIntakePower(1); + } else if (gamepad1.cross) { + spindexer.setIntakePower(-1); + } else { + spindexer.setIntakePower(0); + } + + // LEFT_BUMPER + if (!shootAll && gamepad1.leftBumperWasReleased()) { + shootStamp = getRuntime(); + shootAll = true; + + shooterTicker = 0; + + } + intakeTicker++; + if (shootAll) { + intakeTicker = 0; + intake = false; + reject = false; + + if (shooterTicker == 0) { + spindexer.prepareShootAllContinous(); + //TELE.addLine("preparing to shoot"); +// } else if (shooterTicker == 2) { +// //servo.setTransferPos(transferServo_in); +// spindexer.shootAll(); +// TELE.addLine("starting to shoot"); + } else if (spindexer.shootAllComplete()) { + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + //spindexer.processIntake(); + //TELE.addLine("stop shooting"); + } + shooterTicker++; + //spindexer.processIntake(); + } + + if (gamepad1.left_stick_button) { + servo.setTransferPos(transferServo_out); + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + } + } //DRIVETRAIN: - drivetrain.drive(-gamepad1.right_stick_y, gamepad1.right_stick_x, gamepad1.left_stick_x, gamepad1.left_trigger); + drivetrain.drive( + -gamepad1.right_stick_y, + gamepad1.right_stick_x, + gamepad1.left_stick_x, + gamepad1.left_trigger + ); if (gamepad1.right_bumper) { @@ -151,46 +238,52 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.BALL_COUNT); - } else if (gamepad2.triangle) { - light.setState(StateEnums.LightState.BALL_COLOR); + //} else if (gamepad2.triangle){ + //light.setState(StateEnums.LightState.BALL_COLOR); + //} } else { - light.setState(StateEnums.LightState.GOAL_LOCK); + light.setState(StateEnums.LightState.BALL_COUNT); } //TURRET TRACKING - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robH = drive.localizer.getPose().heading.toDouble(); + double robX = currentPose.getX(); + double robY = currentPose.getY(); + double robH = currentPose.getHeading(); double robotX = robX + xOffset; double robotY = robY + yOffset; double robotHeading = robH + hOffset; - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); +// double goalX = -15; +// double goalY = 0; +// +// double dx = robotX - goalX; // delta x from robot to goal +// double dy = robotY - goalY; // delta y from robot to goal +// Pose deltaPose = new Pose(dx, dy, robotHeading); + Pose deltaPose = new Pose(robotX, robotY, robotHeading); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - targetingSettings = targeting.calculateSettings(robotX, robotY, robotHeading, 0.0, turretInterpolate); + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, turretInterpolate); //RELOCALIZATION - if (gamepad2.squareWasPressed()) { + if (gamepad2.triangleWasPressed()){ relocalize = !relocalize; gamepad2.rumble(500); } - if (relocalize) { + if (relocalize){ turret.relocalize(); - xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; - yOffset = (turret.getLimelightX() * 39.3701) - robY; - hOffset = (Math.toRadians(turret.getLimelightH())) - robH; + xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX; + yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY; + hOffset = (Math.toRadians(turret.getLimelightH() + 90)); + while (hOffset > 180) {hOffset-=360;} + while (hOffset < -180) {hOffset+=360;} + hOffset = hOffset - robH; } else { turret.trackGoal(deltaPose); } @@ -225,7 +318,6 @@ public class TeleopV3 extends LinearOpMode { //HOOD: - if (targetingHood) { servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset); } else { @@ -258,66 +350,19 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad2.crossWasPressed()) { - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); +// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH)); + gamepad2.rumble(200); + sleep(500); } - - if (enableSpindexerManager) { - //if (!shootAll) { - spindexer.processIntake(); - //} - - // RIGHT_BUMPER - if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { - spindexer.setIntakePower(1); - } else if (gamepad1.cross) { - spindexer.setIntakePower(-1); - } else { - spindexer.setIntakePower(0); - } - - // LEFT_BUMPER - if (!shootAll && gamepad1.leftBumperWasReleased()) { - shootStamp = getRuntime(); - shootAll = true; - - shooterTicker = 0; - - } - intakeTicker++; - if (shootAll) { - intakeTicker = 0; - intake = false; - reject = false; - - if (shooterTicker == 0) { - spindexer.prepareShootAllContinous(); - //TELE.addLine("preparing to shoot"); -// else if (shooterTicker == 2) { -// //servo.setTransferPos(transferServo_in); -// spindexer.shootAll(); -// TELE.addLine("starting to shoot"); - } else if (spindexer.shootAllComplete()) { - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - //spindexer.processIntake(); - //TELE.addLine("stop shooting"); - } - shooterTicker++; - //spindexer.processIntake(); - } - - if (gamepad1.left_stick_button) { - servo.setTransferPos(transferServo_out); - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - } + if (gamepad2.squareWasPressed()){ + shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease-0.01; + } else if (gamepad2.circleWasPressed()){ + shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01; } //EXTRA STUFFINESS: - drive.updatePoseEstimate(); for (LynxModule hub : allHubs) { hub.clearBulkCache(); @@ -363,9 +408,9 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); - TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); - TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); +// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); +// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); +// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 12a3c8b..9c4f5bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -8,13 +8,13 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -107,7 +107,7 @@ public class ShooterTest extends LinearOpMode { double dx = robX - goalX; // delta x from robot to goal double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java index 0e30dbb..de6189d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java @@ -65,46 +65,51 @@ public class SortingTest extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - while (opModeIsActive()){ - spindexer.setIntakePower(1); - robot.transfer.setPower(1); - - if (gamepad1.crossWasPressed()){ - motif = 21; - } else if (gamepad1.squareWasPressed()){ - motif = 22; - } else if (gamepad1.triangleWasPressed()){ - motif = 23; - } - flywheel.manageFlywheel(2500); - - if (gamepad1.leftBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.prepareShootAll( - 3, - 5, - motif, - 0.501, - 0.501, - 0.501 - ) - ); - } else if (gamepad1.rightBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.shootAllAuto( - 3.5, - 0.014, - 0.501, - 0.501, - 0.501 - ) - ); - intaking = true; - } else if (intaking){ - spindexer.processIntake(); - } + if (opModeIsActive()){ + Actions.runBlocking( + autoActions.ShakeDrivetrain( + 100 + ) + ); +// spindexer.setIntakePower(1); +// robot.transfer.setPower(1); +// +// if (gamepad1.crossWasPressed()){ +// motif = 21; +// } else if (gamepad1.squareWasPressed()){ +// motif = 22; +// } else if (gamepad1.triangleWasPressed()){ +// motif = 23; +// } +// flywheel.manageFlywheel(2500); +// +// if (gamepad1.leftBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.prepareShootAll( +// 3, +// 5, +// motif, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// } else if (gamepad1.rightBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.shootAllAuto( +// 3.5, +// 0.014, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// intaking = true; +// } else if (intaking){ +// spindexer.processIntake(); +// } } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 34c34e3..0b3fcaf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -4,11 +4,14 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Turret; @@ -25,26 +28,31 @@ public class TurretTest extends LinearOpMode { ); Turret turret = new Turret(robot, TELE, robot.limelight); + + Follower follower; + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(72, 72, 0); + follower.setStartingPose(start); + follower.update(); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); + Turret.limelightUsed = false; while(opModeIsActive()){ + follower.update(); + turret.trackGoal(follower.getPose()); - drive.updatePoseEstimate(); - turret.trackGoal(drive.localizer.getPose()); +// TELE.addData("tpos", turret.getTurrPos()); +// TELE.addData("Limelight tx", turret.getBearing()); +// TELE.addData("Limelight ty", turret.getTy()); +// TELE.addData("Limelight X", turret.getLimelightX()); +// TELE.addData("Limelight Y", turret.getLimelightY()); - TELE.addData("tpos", turret.getTurrPos()); - TELE.addData("Limelight tx", turret.getBearing()); - TELE.addData("Limelight ty", turret.getTy()); - TELE.addData("Limelight X", turret.getLimelightX()); - TELE.addData("Limelight Y", turret.getLimelightY()); +// if(zeroTurr){ +// turret.zeroTurretEncoder(); +// } - if(zeroTurr){ - turret.zeroTurretEncoder(); - } - - TELE.update(); +// TELE.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index accdc8b..cb5fc64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -1,54 +1,76 @@ package org.firstinspires.ftc.teamcode.utils; -import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; public class Drivetrain { - Robot robot; + Robot robot; boolean autoDrive = false; - Pose2d brakePos = new Pose2d(0, 0, 0); + Pose brakePos = new Pose(0, 0, 0); - MecanumDrive drive; +// MecanumDrive drive; + Follower follower; private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); - public Drivetrain (Robot rob, MecanumDrive mecanumDrive){ + public Drivetrain (Robot rob, Follower follower){ this.robot = rob; - this.drive = mecanumDrive; + this.follower = follower; } + private double prevY = 0; + private double prevX = 0; + private double prevRX = 0; + private double prevBrake = 0; public void drive(double y, double x, double rx, double brake){ + int countConstant = 0; + boolean brakeChange = false; + if (Math.abs(prevY - y) > 0.05){ + prevY = y; + countConstant++; + } + if (Math.abs(prevX - x) > 0.05){ + prevX = x; + countConstant++; + } + if (Math.abs(prevRX - rx) > 0.05){ + prevRX = rx; + countConstant++; + } + if (Math.abs(prevBrake - brake) > 0.05){ + prevBrake = brake; + brakeChange = true; + } - if (!autoDrive) { + if (!autoDrive && countConstant > 0) { x = x* 1.1; // Counteract imperfect strafing - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; + double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1); + double frontLeftPower = (prevY + prevX + prevRX) / denominator; + double backLeftPower = (prevY - prevX + prevRX) / denominator; + double frontRightPower = (prevY - prevX - prevRX) / denominator; + double backRightPower = (prevY + prevX - prevRX) / denominator; robot.frontLeft.setPower(frontLeftPower); robot.backLeft.setPower(backLeftPower); robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); } + Pose currentPos = follower.getPose(); + brakePos = currentPos; if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -56,23 +78,17 @@ public class Drivetrain { robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.updatePoseEstimate(); - - brakePos = drive.localizer.getPose(); autoDrive = true; } else if (brake > 0.4) { - drive.updatePoseEstimate(); - Pose2d currentPos = drive.localizer.getPose(); + PathChain traj2 = follower.pathBuilder() + .addPath(new BezierLine(currentPos, brakePos)) + .setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading()) + .build(); - TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) - .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); - - if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { - Actions.runBlocking( - traj2.build() - ); + if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) { + follower.followPath(traj2); } } else { autoDrive = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java index 6a5f09d..6fdd2cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java @@ -64,17 +64,17 @@ public final class Light { break; case BALL_COLOR: - - if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { + double currentTime = System.currentTimeMillis(); + if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getRearCenterLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getDriverLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { lightColor = spindexer.getPassengerLight(); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 8f6d885..232ed35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -79,6 +79,6 @@ public class Servos { } public boolean spinEqual(double pos) { - return Math.abs(pos - this.getSpinPos()) < 0.03; + return Math.abs(pos - this.getSpinPos()) < 0.05; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 43c8a7d..2228bd5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -48,7 +48,7 @@ public class Spindexer { public double distanceFrontDriver = 0.0; public double distanceFrontPassenger = 0.0; - public double spindexerWiggle = 0.01; + public double spindexerWiggle = 0.03; public double spindexerOuttakeWiggle = 0.01; private double prevPos = 0.0; @@ -173,10 +173,25 @@ public class Spindexer { // Detects if a ball is found and what color. // Returns true is there was a new ball found in Position 1 // FIXIT: Reduce number of times that we read the color sensors for loop times. + public static boolean teleop = false; public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { boolean newPos1Detection = false; int spindexerBallPos = 0; + double rearDistance; + double frontDriverDistance; + double frontPassengerDistance; + if (teleop){ + rearDistance = 48; + frontDriverDistance = 50; + frontPassengerDistance = 29; + detectFrontColor = false; + detectRearColor = false; + } else { + rearDistance = 48; + frontDriverDistance = 56; + frontPassengerDistance = 29; + } // Read Distances double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); @@ -187,12 +202,12 @@ public class Spindexer { distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 48) { + if (distanceRearCenter < rearDistance) { // Mark Ball Found newPos1Detection = true; - if (detectRearColor) { + if (detectRearColor && !teleop) { // Detect which color NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); @@ -209,10 +224,10 @@ public class Spindexer { // Position 2 // Find which ball position this is in the spindexer spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; - if (distanceFrontDriver < 50) { + if (distanceFrontDriver < frontDriverDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - if (detectFrontColor) { + if (detectFrontColor && !teleop) { NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); @@ -235,11 +250,11 @@ public class Spindexer { // Position 3 spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; - if (distanceFrontPassenger < 29) { + if (distanceFrontPassenger < frontPassengerDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - if (detectFrontColor) { + if (detectFrontColor && !teleop) { NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); @@ -433,13 +448,13 @@ public class Spindexer { case MOVING: // Stopping when we get to the new position if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - if (intakeTicker > 1){ + //if (intakeTicker > 1){ currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); intakeTicker = 0; - } else { - intakeTicker++; - } + //} else { + // intakeTicker++; + //} //detectBalls(false, false); } else { // Keep moving the spindexer @@ -535,13 +550,11 @@ public class Spindexer { break; case SHOOT_PREP_CONTINOUS: - if (shootTicks > waitFirstBallTicks){ - currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; - shootTicks++; - } else if (servos.spinEqual(spinStartPos)){ - shootTicks++; + if (servos.spinEqual(spinStartPos)){ servos.setTransferPos(transferServo_in); + currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { + servos.setTransferPos(transferServo_out); servos.setSpinPos(spinStartPos); } break; @@ -557,7 +570,7 @@ public class Spindexer { shootTicks = 0; currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; + double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } @@ -671,10 +684,8 @@ public class Spindexer { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; } private double prevPow = 0.501; - private boolean firstIntakePow = true; public void setIntakePower(double pow){ - if (firstIntakePow || prevPow != pow){ - firstIntakePow = false; + if (prevPow != 0.501 && prevPow != pow){ robot.intake.setPower(pow); } prevPow = pow; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index b738fac..95ce968 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -1,9 +1,12 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import java.lang.Math; +import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; public class Targeting { @@ -71,7 +74,7 @@ public class Targeting { public final int TILE_UPPER_QUARTILE = 18; public final int TILE_LOWER_QUARTILE = 6; public double robotInchesX, robotInchesY = 0.0; - public int robotGridX, robotGridY = 0; + public int robotGridX = 0, robotGridY = 0; MultipleTelemetry TELE; double cancelOffsetX = 0.0; // was -40.0 double cancelOffsetY = 0.0; // was 7.0 @@ -82,40 +85,64 @@ public class Targeting { public Targeting() { } - double cos54 = Math.cos(Math.toRadians(-54)); - double sin54 = Math.sin(Math.toRadians(-54)); - + //TODO: change code so it uses pedropathing paths public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); - if (!redAlliance){ - sin54 = Math.sin(Math.toRadians(54)); - } else { - sin54 = Math.sin(Math.toRadians(-54)); - } - // TODO: test these values determined from the fmap - double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; - double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; - - // Convert robot coordinates to inches - robotInchesX = rotatedX * unitConversionFactor; - robotInchesY = rotatedY * unitConversionFactor; - - // Find approximate location in the grid - int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); - - int remX = Math.floorMod((int) robotInchesX, tileSize); - int remY = Math.floorMod((int) robotInchesY, tileSize); - - //clamp - - //if (redAlliance) { - robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - //} else { + int gridX; + int gridY; + int remX = 0; + int remY = 0; + // Old code +// if (!redAlliance){ +// sin54 = Math.sin(Math.toRadians(54)); +// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; +// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; +// +// // Convert robot coordinates to inches +// robotInchesX = rotatedX * unitConversionFactor + 20; +// robotInchesY = rotatedY * unitConversionFactor + 20; +// +// // Find approximate location in the grid +// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize)); +// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); +// } else { +// sin54 = Math.sin(Math.toRadians(-54)); +// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; +// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; +// +// // Convert robot coordinates to inches +// robotInchesX = rotatedX * unitConversionFactor; +// robotInchesY = rotatedY * unitConversionFactor; +// +// // Find approximate location in the grid +// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); +// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); +// } +// +// + remX = Math.floorMod((int) robotX, tileSize); + remY = Math.floorMod((int) robotY, tileSize); +// +// //clamp +// +// if (redAlliance) { // robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); // robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - //} +// } else { +// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); +// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); +// } + + // New code + if (redAlliance){ + gridY = Math.round((float) (((144-robotX)-12) / 24)); + } else { + gridY = Math.round((float) ((robotX-12) / 24)); + } + gridX = Math.round((float) (((144-robotY)-12) / 24)); + + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. @@ -192,7 +219,7 @@ public class Targeting { if (true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX < 6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + hoodOffset; } return recommendedSettings; } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 55df38d..d76ab5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -13,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.teleop.TeleopV3; import java.util.ArrayList; import java.util.List; @@ -23,10 +25,10 @@ public class Turret { public static double turretTolerance = 0.02; public static double turrPosScalar = 0.00011264432; - public static double turret180Range = 0.54; + public static double turret180Range = 0.55; public static double turrDefault = 0.35; public static double turrMin = 0; - public static double turrMax = 1; + public static double turrMax = 0.69; public static boolean limelightUsed = true; public static double limelightPosOffset = 5; public static double manualOffset = 0.0; @@ -79,7 +81,7 @@ public class Turret { } public double getTurrPos() { - return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; + return robot.turr1.getPosition(); } private double prevTurrPos = 0; @@ -107,38 +109,24 @@ public class Turret { private void limelightRead() { // only for tracking purposes, not general reads Double xPos = null; Double yPos = null; - Double zPos = null; + double zPos; Double hPos = null; result = webcam.getLatestResult(); if (result != null) { if (result.isValid()) { tx = result.getTx(); ty = result.getTy(); - // MegaTag1 code for receiving position - Pose3D botpose = result.getBotpose(); - if (botpose != null) { - limelightPosX = botpose.getPosition().x; - limelightPosY = botpose.getPosition().y; - } - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - limelightTagPose = fiducial.getRobotPoseTargetSpace(); - if (limelightTagPose != null){ - xPos = limelightTagPose.getPosition().x; - yPos = limelightTagPose.getPosition().y; - zPos = limelightTagPose.getPosition().z; - hPos = limelightTagPose.getOrientation().getYaw(); + if (TeleopV3.relocalize){ + zPos = result.getBotpose().getPosition().z; + if (zPos < 0.15){ + xPos = result.getBotpose().getPosition().x; + yPos = result.getBotpose().getPosition().y; + hPos = result.getBotpose().getOrientation().getYaw(); + limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); + limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); + limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH); } } - - } - } - if (xPos != null){ - if (zPos<0) { - limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); - limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); - limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ); - limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH); } } } @@ -153,7 +141,6 @@ public class Turret { return ty; } - Pose3D limelightTagPose; double limelightTagX = 0.0; double limelightTagY = 0.0; double limelightTagZ = 0.0; @@ -246,17 +233,31 @@ public class Turret { return bearingOffset; } - public void trackGoal(Pose2d deltaPos) { + double targetTurretPos; + public void trackGoal(Pose deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ + double posX; + if (Color.redAlliance){ + posX = 134 - deltaPos.getX(); + } else { + posX = deltaPos.getX() - 10; + } + double posY = 140 - deltaPos.getY(); + double posH = Math.toDegrees(deltaPos.getHeading()); + while (posH > 180) posH -= 360; + while (posH < -180) posH += 360; // Angle from robot to goal in robot frame - double desiredTurretAngleDeg = Math.toDegrees( - Math.atan2(deltaPos.position.y, deltaPos.position.x) - ); + double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45; // Robot heading (field → robot) - double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + double robotHeadingDeg; + if (Color.redAlliance){ + robotHeadingDeg = posH + 135; + } else { + robotHeadingDeg = posH + 45; + } // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; @@ -277,7 +278,7 @@ public class Turret { limelightRead(); // Active correction if we see the target - if (result.isValid() && !lockOffset && limelightUsed) { + if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) { currentTrackOffset += bearingAlign(result); currentTrackCount++; @@ -333,7 +334,7 @@ public class Turret { /* ---------------- ANGLE → SERVO POSITION ---------------- */ - double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); // Clamp to physical servo limits targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax)); @@ -358,13 +359,14 @@ public class Turret { // TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); // TELE.addData("Target Pos", "%.3f", targetTurretPos); -// TELE.addData("Current Pos", "%.3f", currentPos); +// TELE.addData("Current Localization Pos", deltaPos); // TELE.addData("Commanded Pos", "%.3f", turretPos); // TELE.addData("LL Valid", result.isValid()); // TELE.addData("LL getTx", result.getTx()); // TELE.addData("LL Offset", offset); // TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); // TELE.addData("Learned Offset", "%.2f", offset); +// TELE.update(); } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 70755bd..bd7c756 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -21,9 +21,9 @@ dependencies { implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.pedropathing:ftc:2.0.6' //PedroCore - implementation 'com.pedropathing:telemetry:1.0.0' //PedroTele - implementation 'com.bylazar:fullpanels:1.0.2' //Panels + implementation 'com.pedropathing:ftc:2.1.1' + implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB implementation 'com.rowanmcalpin.nextftc:core:0.6.2' //NEXT FTC