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 e5700d4..b82fa67 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 @@ -40,7 +40,11 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double spindexerSpeedIncrease = 0.014; + public static double velGate0Start = 2700, hoodGate0Start = 0.61; + + public static double velGate0End = 2700, hoodGate0End = 0.4; + + public static double spindexerSpeedIncrease = 0.025; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; @@ -63,12 +67,10 @@ public class Auto_LT_Close extends LinearOpMode { public static double shoot3Time = 2.5; public static double colorSenseTime = 1.2; public int motif = 0; - public static double xShoot0 = 40, yShoot0 = 0.1, hShoot0 = 0.1; - public static double waitToShoot0 = 1.1; - public static double waitToPickupGate2 = 0.5; - public static double pickupStackGateSpeed = 23; + public static double waitToShoot0 = 0.6; + public static double waitToPickupGate2 = 0.1; + public static double pickupStackGateSpeed = 50; public static double intake2TimeGate = 6; - public static double xShootGate = 40, yShootGate = 0.2, hShootGate = 0.2; Robot robot; MultipleTelemetry TELE; @@ -94,9 +96,11 @@ public class Auto_LT_Close extends LinearOpMode { double x4b, y4b, h4b; double xShoot, yShoot, hShoot; + double xShoot0, yShoot0, hShoot0; double xGate, yGate, hGate; - double xPrep, yPrep, hPrep; + double xShootGate, yShootGate, hShootGate; double xLeave, yLeave, hLeave; + double xLeaveGate, yLeaveGate, hLeaveGate; int ballCycles = 3; int prevMotif = 0; @@ -206,9 +210,7 @@ public class Auto_LT_Close extends LinearOpMode { x4b = rx4b; y4b = ry4b; h4b = rh4b; - xPrep = rxPrep; - yPrep = ryPrep; - hPrep = rhPrep; + xShoot = rShootX; yShoot = rShootY; hShoot = rShootH; @@ -216,6 +218,16 @@ public class Auto_LT_Close extends LinearOpMode { yLeave = rLeaveY; hLeave = rLeaveH; + xShoot0 = rShoot0X; + yShoot0 = rShoot0Y; + hShoot0 = rShoot0H; + xShootGate = rShootGateX; + yShootGate = rShootGateY; + hShootGate = rShootGateH; + xLeaveGate = rLeaveGateX; + yLeaveGate = rLeaveGateY; + hLeaveGate = rLeaveGateH; + obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; @@ -247,9 +259,6 @@ public class Auto_LT_Close extends LinearOpMode { y4b = by4b; h4b = bh4b; - xPrep = bxPrep; - yPrep = byPrep; - hPrep = bhPrep; xShoot = bShootX; yShoot = bShootY; hShoot = bShootH; @@ -257,6 +266,16 @@ public class Auto_LT_Close extends LinearOpMode { yLeave = bLeaveY; hLeave = bLeaveH; + xShoot0 = bShoot0X; + yShoot0 = bShoot0Y; + hShoot0 = bShoot0H; + xShootGate = bShootGateX; + yShootGate = bShootGateY; + hShootGate = bShootGateH; + xLeaveGate = bLeaveGateX; + yLeaveGate = bLeaveGateY; + hLeaveGate = bLeaveGateH; + obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; @@ -265,12 +284,22 @@ public class Auto_LT_Close extends LinearOpMode { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - 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(pickup1Speed)); + if (gateCycle){ + pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate))) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), + new TranslationalVelConstraint(pickupStackGateSpeed)); + } else { + 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(pickup1Speed)); + } - if (ballCycles < 2){ + if (gateCycle){ + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + } else if (ballCycles < 2){ shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); } else { @@ -283,12 +312,15 @@ public class Auto_LT_Close extends LinearOpMode { .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickup1Speed)); - if (ballCycles < 3 || gateCycle){ + if (gateCycle){ + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + } else if (ballCycles < 3){ shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); } else { shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave)); + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) @@ -543,14 +575,16 @@ public class Auto_LT_Close extends LinearOpMode { new SequentialAction( new ParallelAction( autoActions.Wait(waitToShoot0), - autoActions.manageShooterAuto( + autoActions.manageShooterManual( waitToShoot0, - xShoot0, - yShoot0, - hShoot0 + velGate0Start, + hoodGate0Start, + velGate0Start, + hoodGate0Start, + 0.501 ) ), - autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease), + autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501), autoActions.intake( intake2TimeGate, xShootGate, @@ -561,4 +595,5 @@ public class Auto_LT_Close extends LinearOpMode { ) ); } + } \ No newline at end of file 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 21900e0..3ebba07 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 @@ -276,6 +276,75 @@ public class AutoActions{ }; } + public Action shootAllManual(double shootTime, double spindexSpeed, double velStart, double hoodStart, double velEnd, double hoodEnd, double turr) { + return new Action() { + int ticker = 1; + + double stamp = 0.0; + + int shooterTicker = 0; + Action manageShooter = null; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + spindexer.setIntakePower(-0.1); + + light.setState(StateEnums.LightState.BALL_COLOR); + light.update(); + + if (ticker == 1) { + stamp = System.currentTimeMillis(); + manageShooter = manageShooterManual(shootTime, velStart, hoodStart, velEnd, hoodEnd, turr); + + } + ticker++; + + manageShooter.run(telemetryPacket); + + double prevSpinPos = servos.getSpinCmdPos(); + + boolean end; + if (shootForward){ + end = prevSpinPos > spinEndPos; + } else { + end = prevSpinPos < spinEndPos; + } + + if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) { + + if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) { + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + Spindexer.whileShooting = true; + if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) { + servos.setSpinPos(prevSpinPos + spindexSpeed); + } else if (shooterTicker > Spindexer.waitFirstBallTicks){ + servos.setSpinPos(prevSpinPos - spindexSpeed); + } + + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + Spindexer.whileShooting = false; + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + } + }; + } + public Action intake( double time, double posX, @@ -305,7 +374,12 @@ public class AutoActions{ manageShooter.run(telemetryPacket); - return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull(); + if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()){ + spindexer.setIntakePower(-0.1); + return false; + } else { + return true; + } } }; } @@ -409,7 +483,7 @@ public class AutoActions{ if (posX != 0.501) { deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); } else { - deltaPose = new Pose2d(robotX, robotY, robotHeading); + deltaPose = new Pose2d(dx, dy, robotHeading); } // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); @@ -456,6 +530,72 @@ public class AutoActions{ } }; } + + public Action manageShooterManual( + double time, + double velStart, + double hoodStart, + double velEnd, + double hoodEnd, + double turr + ){ + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + final boolean timeFallback = (time != 0.501); + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = currentPose.position.x; + double robotY = currentPose.position.y; + + double robotHeading = currentPose.heading.toDouble(); + + 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; + if (turr == 0.501) { + deltaPose = new Pose2d(dx, dy, robotHeading); + if (!detectingObelisk) { + turret.trackGoal(deltaPose); + } + } else { + turret.setTurret(turr); + } + + servos.setHoodPos(hoodStart + (hoodEnd-hoodStart) * ((System.currentTimeMillis() - stamp)/(time*1000))); + double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000)); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(vel); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + + teleStart = currentPose; + + TELE.addData("Steady?", flywheel.getSteady()); + TELE.update(); + + return !timeDone; + } + }; + } } 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 ebffe23..7ebb565 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 @@ -42,5 +42,14 @@ public class Front_Poses { public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55; public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55; + public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1; + public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1; + + public static double rShootGateX = 40, rShootGateY = 0.2, rShootGateH = 90; + public static double bShootGateX = 40, bShootGateY = -0.2, bShootGateH = -90; + + public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55; + public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55; + public static Pose2d teleStart = new Pose2d(0, 0, 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 a6892d8..61edb26 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 @@ -16,7 +16,7 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; - public static double spinStartPos = 0.3; + public static double spinStartPos = 0.25; public static double spinEndPos = 0.85; public static double shootAllSpindexerSpeedIncrease = 0.014;