diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index a454bdb146db..c8ca6c598cbd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -8,6 +8,7 @@ import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.skeletonarmy.marrow.TimerEx; import com.skeletonarmy.marrow.prompts.OptionPrompt; import com.skeletonarmy.marrow.prompts.Prompter; @@ -20,6 +21,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import dev.nextftc.core.commands.delays.Delay; +import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; @@ -29,6 +31,7 @@ @Autonomous(name = "Auto_Main_", group = "Main", preselectTeleOp="Teleop_Main_") public class Auto_Main_ extends NextFTCOpMode { + TimerEx timer25Sec = new TimerEx(25); // Variable to store the selected auto program Auto selectedAuto = null; // Create the prompter object for selecting Alliance and Auto @@ -61,10 +64,14 @@ public void onInit() { prompter.prompt("auto", new OptionPrompt<>("Select Auto", new MOE_365_FAR(), + new Near_15Ball(), + new Near_15Ball2(), new Near_12Ball(), new Near_9Ball(), new Far_9Ball(), - new Far_6Ball() + new Far_6Ball(), + new Far_3Ball() + //new test() )); prompter.onComplete(() -> { selectedAlliance = prompter.get("alliance"); @@ -97,10 +104,11 @@ public void onWaitForStart() { @Override public void onStartButtonPressed() { - Robot.lights.setAllianceDisplay(selectedAlliance); + Robot.lights.setSolidAllianceDisplay(selectedAlliance); // Schedule the proper auto selectedAuto.runAuto(); turretOn = selectedAuto.getTurretEnabled(); + timer25Sec.restart(); // Indicate that initialization is done telemetry.addLine("Initialized"); @@ -113,11 +121,7 @@ public void onUpdate() { telemetry.addData("Alliance", selectedAlliance); Robot.turret.updateAimbot(turretOn, true, 0); Robot.turret.updateFlywheel(); - MecanumDrivetrainClass.robotPose = Robot.drivetrain.follower.getPose(); - - telemetry.addLine(); - telemetry.addData("Mirror -90", Math.toDegrees(paths.autoMirror(new Pose(0,0,-90)).getHeading())); telemetry.update(); } @@ -151,7 +155,7 @@ abstract static class Auto{ public abstract String toString(); } - private class Far_6Ball extends Auto{ + private class Far_9Ball extends Auto{ @Override public Pose getStartPose(){ return paths.farStart; @@ -171,16 +175,20 @@ public void runAuto(){ Commands.runPath(paths.farShoot_to_farPreload, true, 1), Commands.runPath(paths.farPreload_to_farShoot, true, 1), Commands.shootBalls(), + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1), + Commands.runPath(paths.hpPreload_to_farShoot, true, 1), + Commands.shootBalls(), Commands.runPath(paths.farShoot_to_farLeave, true, 1) ).schedule(); } @NonNull @Override - public String toString(){return "Far Zone 6 Ball";} + public String toString(){return "Far 9 Ball";} } - - private class Far_9Ball extends Auto{ + private class Far_6Ball extends Auto{ @Override public Pose getStartPose(){ return paths.farStart; @@ -200,20 +208,44 @@ public void runAuto(){ Commands.runPath(paths.farShoot_to_farPreload, true, 1), Commands.runPath(paths.farPreload_to_farShoot, true, 1), Commands.shootBalls(), - Commands.setFlywheelState(Turret.flywheelState.ON), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_midPreload, true, 1), - Commands.runPath(paths.midPreload_to_farShoot, true, 1), - Commands.shootBalls(), Commands.runPath(paths.farShoot_to_farLeave, true, 1) ).schedule(); } @NonNull @Override - public String toString(){return "Far Zone 9 Ball";} + public String toString(){return "Far 6 Ball";} } + private class Far_3Ball extends Auto{ + @Override + Pose getStartPose() { + return paths.farStart; + } + + @Override + boolean getTurretEnabled() { + return true; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.farStart_to_farShoot, true, 1), + Commands.shootBalls(), + new WaitUntil(() -> timer25Sec.isDone()), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Far 3 Ball + HP Intake"; + } + } private class Near_9Ball extends Auto{ @Override public Pose getStartPose(){ @@ -247,9 +279,8 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "Near Zone 9 Ball";} + public String toString(){return "Near 9 Ball";} } - private class Near_12Ball extends Auto{ @Override public Pose getStartPose(){ @@ -285,9 +316,100 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "Near Zone 12 Ball";} + public String toString(){return "Near 12 Ball";} + } + private class Near_15Ball extends Auto{ + + @Override + Pose getStartPose() { + return paths.nearStart; + } + + @Override + boolean getTurretEnabled() { + return true; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.nearStart_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_midPreload, true, 1), + Commands.runPath(paths.midPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_farPreload, true, 1), + Commands.setIntakeMode(Intake.intakeMode.OFF), + Commands.runPath(paths.farPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_nearPreload, true, 1), + Commands.runPath(paths.nearPreload_to_midShoot, false, 1), + Commands.shootBalls(), + Commands.runPath(paths.midShoot_to_nearLeave, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Near 15 Ball (With Far Spike Mark)"; + } } + private class Near_15Ball2 extends Auto{ + @Override + Pose getStartPose() { + return paths.nearStart; + } + + @Override + boolean getTurretEnabled() { + return true; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.nearStart_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_midPreload, true, 1), + Commands.runPath(paths.midPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_nearPreload, true, 1), + Commands.runPath(paths.nearPreload_to_midShoot, false, 1), + Commands.shootBalls(), + Commands.runPath(paths.midShoot_to_nearLeave, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Near 15 Ball (No Far Spike Mark)"; + } + } private class MOE_365_FAR extends Auto{ @Override public Pose getStartPose(){ @@ -341,6 +463,6 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "MOE 365 Far Zone Auto";} + public String toString(){return "MOE 365 Far";} } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java index f0a2d149add2..293263251198 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java @@ -16,10 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; -import org.firstinspires.ftc.teamcode.Prism.Color; -import org.firstinspires.ftc.teamcode.Prism.Direction; import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver; -import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; import dev.nextftc.control.ControlSystem; import dev.nextftc.control.KineticState; @@ -364,40 +361,45 @@ public static class GoBildaPrismBarClass { // Maximum length of 4 daisy chained strips is 36 (12 + 12 + 6 + 6) // Scrimmage length of 2 daisy chained strips is 24 (12 + 12) + // Artboards current status: + GoBildaPrismDriver.Artboard solidRED = GoBildaPrismDriver.Artboard.ARTBOARD_0; + GoBildaPrismDriver.Artboard solidBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_1; + GoBildaPrismDriver.Artboard blinkingRED = GoBildaPrismDriver.Artboard.ARTBOARD_2; + GoBildaPrismDriver.Artboard blinkingBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_3; + GoBildaPrismDriver.Artboard rainbow = GoBildaPrismDriver.Artboard.ARTBOARD_4; + GoBildaPrismDriver prism; - int stripBrightness = 50; + int stripBrightness = 25; public void init(@NonNull OpMode opmode, int stripLength){ prism = opmode.hardwareMap.get(GoBildaPrismDriver.class, "prism"); prism.setStripLength(stripLength); + prism.clearAllAnimations(); } - public void setStripSolidColor(Color color){ - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Solid(color, stripBrightness)); + public void setStripBrightness(int brightness){ + stripBrightness = brightness; } - public void setStripRainbow(){ - PrismAnimations.AnimationBase colorA = new PrismAnimations.Rainbow(Direction.Backward); - colorA.setIndexes(0, 12); - PrismAnimations.AnimationBase colorB = new PrismAnimations.Rainbow(Direction.Forward); - colorB.setIndexes(13, 24); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, colorA); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_2, colorB); + public void setDisplayedArtboard(GoBildaPrismDriver.Artboard board){ + prism.loadAnimationsFromArtboard(board); } - public void clearStripAnimations(){ - prism.clearAllAnimations(); + public void setStripRainbow(){ + setDisplayedArtboard(rainbow); } - public void setStripBrightness(int brightness){ - stripBrightness = brightness; + public void setSolidAllianceDisplay(LoadHardwareClass.Alliance alliance){ + if (alliance == LoadHardwareClass.Alliance.RED){ + setDisplayedArtboard(solidRED); + }else if (alliance == LoadHardwareClass.Alliance.BLUE){ + setDisplayedArtboard(solidBLUE); + } } - - public void setAllianceDisplay(LoadHardwareClass.Alliance alliance){ - clearStripAnimations(); + public void setBlinkingAllianceDisplay(LoadHardwareClass.Alliance alliance){ if (alliance == LoadHardwareClass.Alliance.RED){ - setStripSolidColor(Color.RED); + setDisplayedArtboard(blinkingRED); }else if (alliance == LoadHardwareClass.Alliance.BLUE){ - setStripSolidColor(Color.BLUE); + setDisplayedArtboard(blinkingBLUE); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java index a4a9398bbbac..18e21908da50 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java @@ -23,6 +23,7 @@ public enum intakeMode { SHOOTING, NO_BELT, REVERSING, + REVERSE_NOBELT, OFF } @@ -74,7 +75,7 @@ public void setMode(intakeMode direction) { intake.setPower(1); belt.setPower(1); }else if (direction == intakeMode.SHOOTING){ - intake.setPower(0); //TODO change this to 0 if using servo powered intake belt + intake.setPower(0); belt.setPower(1); }else if (direction == intakeMode.REVERSING){ intake.setPower(-1); @@ -82,6 +83,9 @@ public void setMode(intakeMode direction) { }else if (direction == intakeMode.NO_BELT){ intake.setPower(1); belt.setPower(0); + }else if (direction == intakeMode.REVERSE_NOBELT){ + intake.setPower(-1); + belt.setPower(0); }else{ intake.setPower(0); belt.setPower(0); @@ -112,6 +116,8 @@ public intakeMode getMode(){ return intakeMode.NO_BELT; }else if (intakePower == -1 && beltPower == -1){ return intakeMode.REVERSING; + }else if (intakePower == -1 && beltPower == 0){ + return intakeMode.REVERSE_NOBELT; }else{ return intakeMode.OFF; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java index 8a0f1ff92317..1824240fc6e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java @@ -30,20 +30,20 @@ public class Turret { public final Devices.REVHallEffectSensorClass hall = new Devices.REVHallEffectSensorClass(); // Turret PID coefficients - public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.07, 0.00000000001, 0.003); // 223RPM Motor + public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.022, 0.0000000002, 0.0015); // 223RPM Motor public static PIDCoefficients cameraCoefficients = new PIDCoefficients(0, 0, 0); // Flywheel PID coefficients for various speeds //public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.0002, 0, 0); // 4500 RPM public static PIDCoefficients flywheelCoefficients4200 = new PIDCoefficients(0.0004, 0, 0); // 4200 RPM public static PIDCoefficients flywheelCoefficients3500 = new PIDCoefficients(0.00025, 0, 0); // 3500 RPM - //public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.00025, 0, 0); // 3000 RPM + public static PIDCoefficients flywheelCoefficients3000 = new PIDCoefficients(0.00025, 0, 0); // 3000 RPM // Flywheel FF coefficients for various speeds //public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.000026,0,0); // 4500 RPM - public static BasicFeedforwardParameters flywheelFFCoefficients4200 = new BasicFeedforwardParameters(0.000032,0,0); // 4200 RPM - public static BasicFeedforwardParameters flywheelFFCoefficients3500 = new BasicFeedforwardParameters(0.000032,0,0); // 3500 RPM - //public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.000031,0,0); // 3000 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients4200 = new BasicFeedforwardParameters(0.0000328,0,0); // 4200 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients3500 = new BasicFeedforwardParameters(0.0000323,0,0); // 3500 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients3000 = new BasicFeedforwardParameters(0.000031,0,0); // 3000 RPM // Actual Flywheel Coefficients private PIDCoefficients actualFlywheelCoefficients = flywheelCoefficients3500; @@ -64,7 +64,9 @@ public enum flywheelState { public flywheelState flywheelMode = flywheelState.OFF; double targetRPM = 0; /** Controls the target speed of the flywheel when it is on.*/ + public static double flywheelReallyNearSpeed = 2800; public static double flywheelNearSpeed = 3300; + public static double flywheelFarNearSpeed = 3600; public static double flywheelFarSpeed = 4200; /** Controls the upper software limit of the hood.*/ public static double upperHoodLimit = 260; @@ -140,27 +142,31 @@ public void init(OpMode opmode, LoadHardwareClass robot){ // Safety points for LUTs hoodLUTnear.add(0, 0); - hoodLUTfar.add(0, 0); + hoodLUTfar.add(0, 190); // -------------------------------------------------------- // Near zone measurements - hoodLUTnear.add(53.5,108); - hoodLUTnear.add(71,168); - hoodLUTnear.add(77, 181); - hoodLUTnear.add(88,185); - hoodLUTnear.add(94.5,185); - hoodLUTnear.add(96, 180); + hoodLUTnear.add(60, 0); + hoodLUTnear.add(68.5, 140.5); + hoodLUTnear.add(71, 182); + hoodLUTnear.add(84.5, 175); + hoodLUTnear.add(88, 145); + hoodLUTnear.add(90, 145); + hoodLUTnear.add(97.5, 200); + hoodLUTnear.add(102, 187); + hoodLUTnear.add(114, 165); // Far zone measurements - hoodLUTfar.add(103, 200); - hoodLUTfar.add(204, 200); + hoodLUTfar.add(139.5, 190); + hoodLUTfar.add(150, 190); + hoodLUTfar.add(160, 160); // -------------------------------------------------------- // Safety points for LUTs - hoodLUTnear.add(300, 200); - hoodLUTfar.add(300, 200); + hoodLUTnear.add(300, 165); + hoodLUTfar.add(300, 160); // Generate Lookup Table & Initialize servo position hoodLUTnear.createLUT(); @@ -231,9 +237,9 @@ private void updateRotationalAimbot(){ } }else{ if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED){ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()-2), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360), -Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); }else{ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()+2), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360), -Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); } } } @@ -262,6 +268,9 @@ public double rotationalAimbotLocalizer (){ ) - Math.toDegrees(Robot.drivetrain.follower.getPose().getHeading()) + 90)%360; } + public static Pose rotationalNearGoalPose = new Pose(8, 136); + public static Pose rotationalFarGoalPose = new Pose(8, 136); + /** * Calculates the proper goal pose * @return a pose of the rotational aimbot's target position. @@ -270,15 +279,18 @@ public Pose calcGoalPose(){ robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY()); robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading()); - Pose nearGoalPose = new Pose(8,136,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {nearGoalPose = new Pose(136, 136, 0);} - Pose farGoalPose = new Pose(16,140,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {farGoalPose = new Pose(136, 140, 0);} + Pose farPose = rotationalFarGoalPose; + Pose nearPose = rotationalNearGoalPose; + + if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) { + nearPose = nearPose.mirror(); + farPose = farPose.mirror(); + } if(robotZone.isInside(LoadHardwareClass.FarLaunchZone)){ - return farGoalPose; + return farPose; }else{ - return nearGoalPose; + return nearPose; } } @@ -287,7 +299,7 @@ public Pose calcGoalPose(){ */ public void setGateState(gatestate state){ if (state == gatestate.CLOSED){ - gate.setAngle(0.47); + gate.setAngle(0.534); }else if (state == gatestate.OPEN){ gate.setAngle(0.5); } @@ -347,6 +359,9 @@ private void setFlywheelRPM(double rpm){ public double getFlywheelRPM(){ return flywheel.getRPM(); } + public boolean isFlywheelReady(){ + return flywheel.getRPM() > getFlywheelCurrentMaxSpeed()-150; + } /** * Sets the target state of the Flywheel.
@@ -377,17 +392,28 @@ public boolean zeroTurret(){ /** * Updates the flywheel PID. Must be called every loop. */ - public void updateFlywheel(){ + public void updateFlywheel() { robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY()); robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading()); + Pose goalPose = new Pose(0,144,0); + if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {goalPose = new Pose(144, 144, 0);} + opMode.telemetry.addData("In Far Zone", robotZone.isInside(LoadHardwareClass.FarLaunchZone)); opMode.telemetry.addData("In Near Zone", robotZone.isInside(LoadHardwareClass.ReallyNearLaunchZoneRed)); - if (robotZone.isInside(LoadHardwareClass.FarLaunchZone)){ + if (robotZone.isInside(LoadHardwareClass.FarLaunchZone)) { targetRPM = flywheelFarSpeed; actualFlywheelCoefficients = flywheelCoefficients4200; actualFlywheelFFCoefficients = flywheelFFCoefficients4200; + }else if (Robot.drivetrain.distanceFromGoal() < 60){ + targetRPM = flywheelReallyNearSpeed; + actualFlywheelCoefficients = flywheelCoefficients3000; + actualFlywheelFFCoefficients = flywheelFFCoefficients3000; + }else if (Robot.drivetrain.distanceFromGoal() > 90){ + targetRPM = flywheelFarNearSpeed; + actualFlywheelCoefficients = flywheelCoefficients3500; + actualFlywheelFFCoefficients = flywheelFFCoefficients3500; }else{ targetRPM = flywheelNearSpeed; actualFlywheelCoefficients = flywheelCoefficients3500; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java index 6f5d165ebbce..a261d738d23a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java @@ -10,9 +10,6 @@ import dev.nextftc.core.commands.Command; import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.delays.WaitUntil; -import dev.nextftc.core.commands.groups.ParallelRaceGroup; -import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; import dev.nextftc.core.commands.utility.LambdaCommand; import dev.nextftc.extensions.pedro.FollowPath; @@ -25,9 +22,13 @@ public Commands(@NonNull LoadHardwareClass robot){ Robot = robot; } + public static int shootingState = 0; + public static boolean isDoneShooting = false; + // Delay timer for shooting sequence private static final TimerEx shootingTimerFifthSec = new TimerEx(0.2); private static final TimerEx shootingTimerHalfSec = new TimerEx(0.5); + private static final TimerEx shootingTimer1sec = new TimerEx(1); private static final TimerEx shootingTimer2sec = new TimerEx(2); private static final TimerEx shootingTimer5sec = new TimerEx(5); private static Command resetShootingTimerFifthsec() { @@ -36,6 +37,9 @@ private static Command resetShootingTimerFifthsec() { private static Command resetShootingTimerHalfsec() { return new LambdaCommand("resetShootingTimer0.5sec").setStart(shootingTimerHalfSec::restart); } + private static Command resetShootingTimer1sec() { + return new LambdaCommand("resetShootingTimer1sec").setStart(shootingTimer1sec::restart); + } private static Command resetShootingTimer2sec() { return new LambdaCommand("resetShootingTimer2sec").setStart(shootingTimer2sec::restart); } @@ -70,84 +74,71 @@ private Command setGateState(Turret.gatestate state){ public Command setIntakeMode(Intake.intakeMode state) { return new InstantCommand(new LambdaCommand("setIntakeMode()") .setStart(() -> Robot.intake.setMode(state)) - .setIsDone(() -> true) ); } public Command setTransferState(Intake.transferState state) { return new InstantCommand(new LambdaCommand("setIntakeMode()") .setStart(() -> Robot.intake.setTransfer(state)) - .setIsDone(() -> true) ); } + /** + * Waits until both proximity sensors are activated at the same time or until 2 seconds have passed + */ public Command waitForArtifacts(){ - return new SequentialGroup( - setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer5sec(), - new WaitUntil(() -> Robot.intake.getTopSensorState() || Robot.intake.getBottomSensorState()), - new ParallelRaceGroup( - new WaitUntil(() -> Robot.intake.getTopSensorState() && Robot.intake.getBottomSensorState()), - new WaitUntil(shootingTimer5sec::isDone) - ) - ); + return new Delay(0.7); } public Command shootBalls(){ - return new SequentialGroup( - // Ensure the flywheel is up to speed, if not, spin up first - setFlywheelState(Turret.flywheelState.ON), - - // Shoot the first two balls - setGateState(Turret.gatestate.OPEN), - resetShootingTimerFifthsec(), - new WaitUntil(shootingTimerFifthSec::isDone), - setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer2sec(), - new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer2sec.isDone())), - - // Shoot the last ball - setIntakeMode(Intake.intakeMode.SHOOTING), - setTransferState(Intake.transferState.UP), - resetShootingTimerHalfsec(), - new WaitUntil(shootingTimerHalfSec::isDone), - - // Reset the systems - setIntakeMode(Intake.intakeMode.OFF), - setGateState(Turret.gatestate.CLOSED), - setTransferState(Intake.transferState.DOWN) - ); - } - - public Command shootTestBalls(){ - return new ParallelRaceGroup( - new SequentialGroup( - // Ensure the flywheel is up to speed, if not, spin up first - setFlywheelState(Turret.flywheelState.ON), - - // Shoot the first two balls - setGateState(Turret.gatestate.OPEN), - resetShootingTimerFifthsec(), - new WaitUntil(shootingTimerFifthSec::isDone), - setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer2sec(), - new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer2sec.isDone())), - - // Shoot the last ball - setIntakeMode(Intake.intakeMode.SHOOTING), - setTransferState(Intake.transferState.UP), - resetShootingTimerHalfsec(), - new WaitUntil(shootingTimerHalfSec::isDone), - - // Reset the systems - setIntakeMode(Intake.intakeMode.OFF), - setGateState(Turret.gatestate.CLOSED), - setTransferState(Intake.transferState.DOWN) - ), - new SequentialGroup( - new Delay(8) - ) - ); + return new LambdaCommand() + .setStart(() -> { + shootingState = 1; + isDoneShooting = false; + shootingTimer5sec.restart(); + }) + .setUpdate(() -> { + switch (shootingState) { + case 1: + Robot.turret.setFlywheelState(Turret.flywheelState.ON); + if (Robot.turret.isFlywheelReady()){ + shootingState++; + shootingTimerFifthSec.restart(); + } + case 2: + Robot.turret.setGateState(Turret.gatestate.OPEN); + if (shootingTimerFifthSec.isDone()){ + shootingState++; + shootingTimerHalfSec.restart(); + } + return; + case 3: + Robot.intake.setMode(Intake.intakeMode.INTAKING); + if (shootingTimerHalfSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ + shootingState++; + shootingTimerHalfSec.restart(); + return; + } + if (shootingTimer5sec.isDone()){ + shootingState = 5; + } + return; + case 4: + Robot.intake.setMode(Intake.intakeMode.SHOOTING); + Robot.intake.setTransfer(Intake.transferState.UP); + if (shootingTimerHalfSec.isDone()) { + shootingState++; + } + return; + case 5: + Robot.turret.setGateState(Turret.gatestate.CLOSED); + Robot.intake.setMode(Intake.intakeMode.OFF); + Robot.intake.setTransfer(Intake.transferState.DOWN); + isDoneShooting = true; + } + }) + .setIsDone(() -> isDoneShooting) + .setInterruptible(true); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java index a94d1a2ae09e..a24e432fdc5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java @@ -23,12 +23,12 @@ public class Pedro_Paths { public Pose nearPreload = new Pose(124.000, 83.500, Math.toRadians(0)); public Pose midPreload = new Pose(130.000, 59.500, Math.toRadians(0)); public Pose farPreload = new Pose(130.000, 35.500, Math.toRadians(0)); - public Pose hpPreload = new Pose(135, 8, -90); - public Pose rampIntake = new Pose(135, 40, 80); + public Pose hpPreload = new Pose(136, 9, Math.toRadians(-90)); + public Pose rampIntake = new Pose(135, 40, Math.toRadians(80)); public Pose hpIntake = null; // Shooting Poses public Pose nearShoot = new Pose(115, 120, Math.toRadians(-35)); - public Pose midShoot = new Pose(85, 85, Math.toRadians(-15)); + public Pose midShoot = new Pose(88, 87, Math.toRadians(-15)); public Pose farShoot = new Pose(85, 15, Math.toRadians(60)); public Pose noTurretMidShoot = new Pose(85, 85, Math.toRadians(45)); public Pose noTurretFarShoot = new Pose(85, 15, Math.toRadians(67.3)); @@ -39,8 +39,8 @@ public class Pedro_Paths { // Open Gate Pose public Pose openGateBasic = new Pose(127.5, 72, Math.toRadians(90)); public Pose openGateBasicReversed = new Pose(127.5, 72, Math.toRadians(-90)); - public Pose openGateIntakeGate = new Pose(131, 61.5, Math.toRadians(20)); - public Pose openGateIntakeRamp = new Pose(133, 55, Math.toRadians(40)); + public Pose openGateIntakeGate = new Pose(128, 62, Math.toRadians(20)); + public Pose openGateIntakeRamp = new Pose(127, 55, Math.toRadians(40)); /** *

Define all path variables.


@@ -299,8 +299,7 @@ public void buildShooting2ToPreloads(){ midShoot, autoMirror(new Pose(75, 56)), midPreload - )) - .setLinearHeadingInterpolation(midShoot.getHeading(), midPreload.getHeading()) + )).setLinearHeadingInterpolation(midShoot.getHeading(), midPreload.getHeading()) .build(); midShoot_to_farPreload = follower.pathBuilder() .addPath(new BezierCurve( @@ -490,10 +489,15 @@ public void buildShootingsToHPPreload(){ farShoot_to_hpPreload = follower.pathBuilder() .addPath(new BezierCurve( farShoot, - autoMirror(new Pose(136, 30)), - hpPreload + autoMirror(new Pose(120, 60)), + autoMirror(new Pose(136, 20)) )) .setLinearHeadingInterpolation(farShoot.getHeading(), hpPreload.getHeading()) + .addPath(new BezierLine( + autoMirror(new Pose(136, 20)), + hpPreload + )) + .setConstantHeadingInterpolation(hpPreload.getHeading()) .build(); midShoot_to_hpPreload = follower.pathBuilder() .addPath(new BezierCurve( @@ -511,11 +515,11 @@ public void buildShootingsToRampIntake(){ midShoot, autoMirror(new Pose(85, 40)), autoMirror(new Pose(137, 2)), - autoMirror(new Pose(135, 30, rampIntake.getHeading())) + autoMirror(new Pose(135, 30)) )) .setLinearHeadingInterpolation(midShoot.getHeading(), rampIntake.getHeading()) .addPath(new BezierLine( - autoMirror(new Pose(135, 30, rampIntake.getHeading())), + autoMirror(new Pose(135, 30)), rampIntake )) .setConstantHeadingInterpolation(rampIntake.getHeading()) @@ -524,20 +528,21 @@ public void buildShootingsToRampIntake(){ .addPath(new BezierCurve( farShoot, autoMirror(new Pose(137, 2)), - autoMirror(new Pose(135, 30, rampIntake.getHeading())) + autoMirror(new Pose(135, 30)) )) - .setLinearHeadingInterpolation(farShoot.getHeading(), rampIntake.getHeading()) + .setLinearHeadingInterpolation(farShoot.getHeading(), rampIntake.getHeading()-Math.toRadians(10)) .addPath(new BezierLine( - autoMirror(new Pose(135, 30, rampIntake.getHeading())), + autoMirror(new Pose(135, 30)), rampIntake )) - .setConstantHeadingInterpolation(rampIntake.getHeading()) + .setLinearHeadingInterpolation(rampIntake.getHeading()-Math.toRadians(10), rampIntake.getHeading()) .build(); } public void buildHPPreloadToShootings(){ hpPreload_to_farShoot = follower.pathBuilder() - .addPath(new BezierLine( + .addPath(new BezierCurve( hpPreload, + autoMirror(new Pose(110, 40)), farShoot )) .setLinearHeadingInterpolation(hpPreload.getHeading(), farShoot.getHeading()) @@ -567,6 +572,7 @@ public void buildPaths(Follower follow){ nearPreload = autoMirror(nearPreload); midPreload = autoMirror(midPreload); farPreload = autoMirror(farPreload); + rampIntake = autoMirror(rampIntake); hpPreload = autoMirror(hpPreload); nearShoot = autoMirror(nearShoot); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java index d1c3d3453d67..73c15408e4b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java @@ -110,7 +110,7 @@ public void init(Pose initialPose) { drivetrain.init(myOpMode, initialPose); turret.init(myOpMode, this); intake.init(myOpMode); - lights.init(myOpMode, 24); + lights.init(myOpMode, 36); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); @@ -126,7 +126,7 @@ public void init(Pose initialPose, Follower follower) { drivetrain.init(myOpMode, initialPose, follower); turret.init(myOpMode, this); intake.init(myOpMode); - lights.init(myOpMode, 24); + lights.init(myOpMode, 36); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java index 667aceca18b4..f2bf2ef1fe92 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java @@ -11,7 +11,7 @@ public class LoadPrismLights { public void init(OpMode opMode, String name){ prism = opMode.hardwareMap.get(GoBildaPrismDriver.class, name); - prism.setStripLength(24); + prism.setStripLength(36); } public void setStripsRainbow(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index 3fc8362109ba..e386307d6397 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -37,13 +37,11 @@ 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; import com.qualcomm.robotcore.util.ElapsedTime; import com.skeletonarmy.marrow.TimerEx; import com.skeletonarmy.marrow.prompts.OptionPrompt; import com.skeletonarmy.marrow.prompts.Prompter; -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.intakeMode; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.transferState; @@ -66,6 +64,7 @@ public class Teleop_Main_ extends LinearOpMode { // Declare OpMode members. private final ElapsedTime runtime = new ElapsedTime(); + private final ElapsedTime looptime = new ElapsedTime(); private final TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); public int shootingState = 0; @@ -91,6 +90,14 @@ enum startPoses { NEAR } + enum lightsState { + SOLID, + BLINKING, + RAINBOW + } + private lightsState ledState = lightsState.SOLID; + private lightsState ledStateOld = lightsState.RAINBOW; + // Contains the start Pose of our robot. This can be changed or saved from the autonomous period. private Pose startPose = Paths.farStart; @@ -149,7 +156,7 @@ public void runOpMode() { // Wait for the game to start (driver presses START) waitForStart(); // Initialize all hardware of the robot - if (selectedAlliance == LoadHardwareClass.Alliance.BLUE){ + if (selectedAlliance == LoadHardwareClass.Alliance.BLUE && MecanumDrivetrainClass.robotPose == null){ Robot.init(startPose.mirror()); }else{ Robot.init(startPose); @@ -158,10 +165,11 @@ public void runOpMode() { Paths.buildPaths(Robot.drivetrain.follower); Robot.drivetrain.startTeleOpDrive(); Robot.intake.setTransfer(transferState.DOWN); - Robot.lights.setAllianceDisplay(selectedAlliance); + Robot.lights.setSolidAllianceDisplay(selectedAlliance); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { + looptime.reset(); if (!Turret.zeroed){ while (!isStopRequested() && Robot.turret.zeroTurret()){ sleep(0); @@ -187,11 +195,8 @@ public void runOpMode() { //positional telemetry telemetry.addData("Robot Position [X, Y, H]", "[" + Robot.drivetrain.follower.getPose().getX() + ", " + Robot.drivetrain.follower.getPose().getY() + ", " + Robot.drivetrain.follower.getPose().getHeading() + "]"); telemetry.addData("Distance From Goal", Robot.drivetrain.distanceFromGoal()); - telemetry.addLine(); - panelsTelemetry.addData("FL Wheel Current", hardwareMap.get(DcMotorEx.class, "FL").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("FR Wheel Current", hardwareMap.get(DcMotorEx.class, "FR").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("BL Wheel Current", hardwareMap.get(DcMotorEx.class, "BL").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("BR Wheel Current", hardwareMap.get(DcMotorEx.class, "BR").getCurrent(CurrentUnit.AMPS)); + telemetry.addData("Angular Velocity (Deg/sec)", Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); + telemetry.addData("Turret Angular Velocity (Deg/sec)", Robot.turret.rotation.getDegreesPerSecond()); telemetry.addLine(); // Turret-related Telemetry @@ -229,15 +234,36 @@ public void runOpMode() { // System-related Telemetry telemetry.addLine(); + telemetry.addData("Loop Time", looptime); telemetry.addData("Status", "Run Time: " + runtime); telemetry.addData("Version: ", "2/13/25"); telemetry.update(); panelsTelemetry.update(); if (runtime.time(TimeUnit.SECONDS) > 115){ - Robot.lights.setStripRainbow(); + ledState = lightsState.RAINBOW; + }else if (Robot.turret.isFlywheelReady()){ + ledState = lightsState.BLINKING; + }else{ + ledState = lightsState.SOLID; } + if (ledState != ledStateOld){ + switch (ledState){ + case SOLID: + Robot.lights.setSolidAllianceDisplay(selectedAlliance); + break; + case BLINKING: + Robot.lights.setBlinkingAllianceDisplay(selectedAlliance); + break; + case RAINBOW: + Robot.lights.setStripRainbow(); + } + ledStateOld = ledState; + } + telemetry.addData("lightsState", ledState); } + + selectedAlliance = null; } /** @@ -332,6 +358,15 @@ public void Gamepad1() { if (gamepad1.dpadDownWasPressed()){ hoodOn = !hoodOn; } + if (gamepad1.yWasPressed()){ + turretOn = !turretOn; + } + + if (gamepad1.dpadLeftWasPressed()){ + selectedAlliance = LoadHardwareClass.Alliance.BLUE; + }else if (gamepad1.dpadRightWasPressed()){ + selectedAlliance = LoadHardwareClass.Alliance.RED; + } } /** @@ -378,9 +413,6 @@ public void Gamepad1() { * */ public void Gamepad2() { - if (gamepad1.yWasPressed()){ - turretOn = !turretOn; - } Robot.turret.updateAimbot(turretOn, hoodOn, hoodOffset); Robot.turret.rotation.setOffsetDegrees(Turret.turretOffset + turretOffset); @@ -430,7 +462,7 @@ public void Gamepad2() { } if (gamepad2.dpadLeftWasPressed()){ turretOffset += 10; - }else if (gamepad2.dpadLeftWasPressed()){ + }else if (gamepad2.dpadRightWasPressed()){ turretOffset -= 10; } @@ -461,12 +493,12 @@ public void Gamepad2() { return; case 2: if (Robot.intake.getMode() == intakeMode.OFF){ - stateTimerFullSec.restart(); - stateTimerFullSec.start(); + stateTimerHalfSec.restart(); + stateTimerHalfSec.start(); } Robot.intake.setMode(intakeMode.INTAKING); - telemetry.addData("Shooting State", "SHOOTING 2"); - if (stateTimerFullSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ + telemetry.addData("Shooting State", "SHOOTING FIRST TWO"); + if (stateTimerHalfSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ shootingState = 3; } return; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java index e1cdea2eef6f..4248d61dd108 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java @@ -37,8 +37,10 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.Pedro_Paths; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; -import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadPrismLights; @Configurable @TeleOp(name="Teleop_Tuning_", group="TeleOp") @@ -49,11 +51,9 @@ public class Teleop_Tuning_ extends LinearOpMode { private ElapsedTime loopTimer = new ElapsedTime(); private TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); - // Panels variables - public static double hoodTargetPos = 0; - public static int turretTurnIncrement = 2; - int turretTurnMultiplier = 1; - int turretTurnAngle = 0; + public static double hoodOffset = 0; + + double initial = 0; // Contains the start Pose of our robot. This can be changed or saved from the autonomous period. private final Pose startPose = new Pose(88.5,7.8, Math.toRadians(90)); @@ -63,60 +63,67 @@ public void runOpMode() { // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); - LoadPrismLights prism = new LoadPrismLights(); + Pedro_Paths paths = new Pedro_Paths(); // Initialize all hardware of the robot Robot.init(startPose); telemetry.addData("Status", "Initialized"); telemetry.update(); + Robot.turret.setGateState(Turret.gatestate.CLOSED); + Robot.lights.setStripRainbow(); - while (!isStopRequested() && Robot.turret.zeroTurret()){ - sleep(0); + if (!Turret.zeroed){ + while (!isStopRequested() && Robot.turret.zeroTurret()){ + sleep(0); + } } - Robot.turret.setHood(0); // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); - - // Begin TeleOp driving Robot.drivetrain.startTeleOpDrive(); - prism.init(this, "prism"); - // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - - // Pass the joystick positions to our mecanum drive controller Robot.drivetrain.pedroMecanumDrive( gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, true ); - - if (gamepad1.aWasPressed()){ - prism.setStripsRainbow(); + if (gamepad2.yWasPressed()) { + if (Robot.turret.flywheelMode == Turret.flywheelState.OFF) { + Robot.turret.setFlywheelState(Turret.flywheelState.ON); + } else { + Robot.turret.setFlywheelState(Turret.flywheelState.OFF); + } } - if (gamepad1.bWasPressed()){ - prism.setStripsRed(); - } - if (gamepad1.xWasPressed()){ - prism.setStripsBlue(); - } - if (gamepad1.yWasPressed()){ - prism.clearStrips(); + Robot.turret.updateFlywheel(); + + if (Math.abs(gamepad2.left_stick_y) < 0.1){ + Robot.intake.setMode(Intake.intakeMode.INTAKING); + }else{ + Robot.intake.setMode(Intake.intakeMode.OFF); } + if (gamepad2.dpadUpWasPressed()) hoodOffset += 10; + if (gamepad2.dpadDownWasPressed()) hoodOffset -= 10; + Robot.turret.updateAimbot(false, true, hoodOffset); + + telemetry.addData("Aimbot Hood Angle", Robot.turret.getHood()); + telemetry.addData("Hood Offset", hoodOffset); + telemetry.addData("Distance From Goal", Robot.drivetrain.distanceFromGoal()); + + // System-related Telemetry telemetry.addLine(); telemetry.addLine("SYSTEM DATA"); telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Loop Time", loopTimer.toString()); - panelsTelemetry.addData("Loop Time", loopTimer.toString()); telemetry.addData("Version: ", "12/26/25"); telemetry.update(); panelsTelemetry.update(); loopTimer.reset(); + Robot.turret.updatePIDs(); } } }