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 43321d4..d98acde 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 @@ -106,10 +106,10 @@ import java.util.Objects; public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; + public static double shoot0SpinSpeedIncrease = 0.02; public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.025; + public static double finalSpindexerSpeedIncrease = 0.03; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -348,6 +348,8 @@ public class Auto_LT_Close extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); velo = flywheel.getVelo(); @@ -622,6 +624,8 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); robot.hood.setPosition(hoodPos); @@ -693,6 +697,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.hood.setPosition(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -762,6 +768,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.hood.setPosition(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -992,7 +1000,7 @@ public class Auto_LT_Close extends LinearOpMode { robot.transfer.setPower(1); - startCycle(); + startAuto(); if (ballCycles > 0){ cycleStackClose(); @@ -1022,7 +1030,7 @@ public class Auto_LT_Close extends LinearOpMode { } - void startCycle() { + void startAuto() { assert shoot0 != null; Actions.runBlocking( 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 8e9709a..61dcfb8 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 @@ -49,7 +49,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Far extends LinearOpMode { - public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; + public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0XTolerance = 1.0; @@ -69,16 +69,18 @@ public class Auto_LT_Far extends LinearOpMode { Turret turret; Targeting targeting; Targeting.Settings targetingSettings; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; - private double xShoot, yShoot, hShoot; + double firstSpindexShootPos = autoSpinStartPos; + boolean shootForward = true; + double xShoot, yShoot, hShoot; private int driverSlotGreen = 0; private int passengerSlotGreen = 0; - private int rearSlotGreen = 0; - private int mostGreenSlot = 0; - private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; - private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; + int rearSlotGreen = 0; + int mostGreenSlot = 0; + double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; public static double firstShootTime = 0.3; + public static double flywheel0Time = 3.5; + public static double shoot0Time = 2; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -97,12 +99,28 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + 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); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Hood", robot.hood.getPosition()); TELE.addData("motif", motif_id); @@ -214,6 +232,8 @@ public class Auto_LT_Far extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); velo = flywheel.getVelo(); @@ -314,6 +334,24 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + 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); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { @@ -369,6 +407,9 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; @@ -467,6 +508,8 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); robot.hood.setPosition(hoodPos); @@ -538,6 +581,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.hood.setPosition(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -607,6 +652,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.hood.setPosition(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -627,6 +674,9 @@ public class Auto_LT_Far extends LinearOpMode { }; } + // initialize path variables here + TrajectoryActionBuilder leave3Ball = null; + @Override public void runOpMode() throws InterruptedException { @@ -662,8 +712,6 @@ public class Auto_LT_Far extends LinearOpMode { robot.light.setPosition(1); - TrajectoryActionBuilder leave = null; - while (opModeInInit()) { // Recalibration in initialization @@ -673,9 +721,10 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(1000); } - robot.hood.setPosition(shoot0Hood); turret.manualSetTurret(turretShootPos); + robot.hood.setPosition(shoot0Hood); + if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; } @@ -706,10 +755,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; } - leave = drive.actionBuilder(teleEnd) + leave3Ball = drive.actionBuilder(teleEnd) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); - TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Start Position", teleEnd); @@ -726,35 +774,9 @@ public class Auto_LT_Far extends LinearOpMode { robot.transfer.setPower(1); - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shoot0Vel, - shoot0Hood, - 5, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) + startAuto(); - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - assert leave != null; - Actions.runBlocking(leave.build()); + leave3Ball(); // Actual way to end autonomous in to find final position while (opModeIsActive()) { @@ -772,4 +794,30 @@ public class Auto_LT_Far extends LinearOpMode { } } + + void startAuto(){ + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + 0.501, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + } + + void leave3Ball(){ + assert leave3Ball != null; + Actions.runBlocking(leave3Ball.build()); + } } \ No newline at end of file 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 7927364..0488a76 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 @@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class Back_Poses { - public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50; - public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50; + public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50; + public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 18e229a..92e8500 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode { waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ + if (spindexPos != 0.501){ robot.spin1.setPosition(spindexPos); robot.spin2.setPosition(1-spindexPos); } @@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode { if (light !=0.501){ robot.light.setPosition(light); } - // To check configuration of spindexer: - // Set "mode" to 1 and spindexPow to 0.1 - // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE - // Do the previous test again to confirm - // Set "mode" to 0 but keep spindexPos at 0.501 - // Manually turn the spindexer until "spindexer pos" is set close to 0 - // Check each spindexer voltage: - // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done! - // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE - //TODO: @KeshavAnandCode do the above please TELE.addData("spindexer pos", servo.getSpinPos()); TELE.addData("turret pos", robot.turr1.getPosition());