diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index 35ede8f..9f1b731 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.teleop.TeleopV4; +import org.firstinspires.ftc.teamcode.tests.NewShooterTest; import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; @@ -369,6 +370,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { park.unpark(); } + NewShooterTest.transferPower = -0.8; + if (initializeRobot){ //add obelisk read here shooter.setState(Shooter.ShooterState.READ_OBELISK); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java index 88a104b..1794294 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java @@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.teleop.TeleopV4; +import org.firstinspires.ftc.teamcode.tests.NewShooterTest; import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; @@ -37,8 +38,8 @@ public class Auto15Ball_Back extends LinearOpMode { SpindexerTransferIntake spindexer; // Wait Times - public static double rapidWaitTime = 0.5; - public static double rapidShootTime = 0.8; + public static double rapidWaitTime = 0.7; + public static double rapidShootTime = 2.6; public static double loadPickupTime = 3; // Initialize path state machine @@ -51,20 +52,20 @@ public class Auto15Ball_Back extends LinearOpMode { PathState pathState = PathState.WAIT_VELOCITY; // Poses - public static double startPoseX = 12, startPoseY = -64, startPoseH = 90; + public static double startPoseX = 16, startPoseY = -64, startPoseH = 90; public static double pickup3ControlX = 12, pickup3ControlY = -37; public static double pickup3X = 61, pickup3Y = -37, pickup3H = 0; - public static double shoot3X = 16, shoot3Y = -57, shoot3H = 0; + public static double shoot3X = 16, shoot3Y = -55, shoot3H = 0; public static double pickupLoadControlX = 21.23654066437572, pickupLoadControlY = -62.311626575028637; public static double pickupLoadX = 63, pickupLoadY = -63, pickupLoadH = 0; public static double shootLoadControlX = 21.23654066437572, shootLoadControlY = -62.311626575028637; - public static double shootLoadX = 16, shootLoadY = -57, shootLoadH = 0; + public static double shootLoadX = 16, shootLoadY = -55, shootLoadH = 0; public static double intakeGateControl1X = 51.9656357388316, intakeGateControl1Y = -65.506277205040073; public static double intakeGateControl2X = 60.13459335624285, intakeGateControl2Y = -67.300458190148911; - public static double intakeGateX = 59, intakeGateY = -12, intakeGateH = 90; + public static double intakeGateX = 63, intakeGateY = -35, intakeGateH = 90; public static double shootGateControlX = 53.8705612829324, shootGateControlY = -35.14501718213059; - public static double shootGateX = 16, shootGateY = -57, shootGateH = 0; - public static double leaveX = 40, leaveY = 14, leaveH = 0; + public static double shootGateX = 16, shootGateY = -55, shootGateH = 0; + public static double leaveX = 36, leaveY = -58, leaveH = 0; double[] xPoses = {startPoseX, pickup3ControlX, pickup3X, shoot3X, pickupLoadControlX, pickupLoadX, shootLoadControlX, shootLoadX, intakeGateControl1X, intakeGateControl2X, intakeGateX, shootGateControlX, shootGateX, leaveX}; @@ -146,29 +147,27 @@ public class Auto15Ball_Back extends LinearOpMode { double currentTime = (double) System.currentTimeMillis() / 1000; switch(pathState){ case WAIT_VELOCITY: - spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); - shooter.setFlywheelVelocity(2400); - robot.setHoodPos(0.64); + shooter.setFlywheelVelocity(3250); + robot.setHoodPos(0.65); if (flywheel.getSteady()){ startAuto++; } - if (startAuto > 5){ - spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + if (startAuto > 6){ + spindexer.startBackShooting(); timeStamp = currentTime; pathState = PathState.WAIT_SHOOT0; } + timeStamp = currentTime; break; case WAIT_SHOOT0: - if (currentTime - timeStamp > rapidShootTime || - (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE && - spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){ + if (currentTime - timeStamp > rapidShootTime){ follower.followPath(startPose_pickup3, false); pathState = PathState.PICKUP3; } break; case PICKUP3: if (!follower.isBusy()){ - follower.followPath(pickup3_shoot3, false); + follower.followPath(pickup3_shoot3, true); pathState = PathState.DRIVE_SHOOT3; timeStamp = currentTime; } @@ -177,15 +176,13 @@ public class Auto15Ball_Back extends LinearOpMode { if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT3; - spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + spindexer.startBackShooting(); } else if (follower.isBusy()){ timeStamp = currentTime; } break; case WAIT_SHOOT3: - if (currentTime - timeStamp > rapidShootTime || - (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE && - spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){ + if (currentTime - timeStamp > rapidShootTime){ follower.followPath(shoot3_pickupLoad, false); pathState = PathState.PICKUP_LOAD; timeStamp = currentTime; @@ -193,7 +190,7 @@ public class Auto15Ball_Back extends LinearOpMode { break; case PICKUP_LOAD: if (currentTime - timeStamp > loadPickupTime){ - follower.followPath(pickupLoad_shootLoad, false); + follower.followPath(pickupLoad_shootLoad, true); pathState = PathState.DRIVE_SHOOT_LOAD; timeStamp = currentTime; } @@ -202,15 +199,13 @@ public class Auto15Ball_Back extends LinearOpMode { if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT_LOAD; - spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + spindexer.startBackShooting(); } else if (follower.isBusy()){ timeStamp = currentTime; } break; case WAIT_SHOOT_LOAD: - if (currentTime - timeStamp > rapidShootTime || - (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE && - spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){ + if (currentTime - timeStamp > rapidShootTime){ follower.followPath(shootLoad_intakeGate, false); pathState = PathState.INTAKE_GATE; timeStamp = currentTime; @@ -218,7 +213,7 @@ public class Auto15Ball_Back extends LinearOpMode { break; case INTAKE_GATE: if (!follower.isBusy()){ - follower.followPath(intakeGate_shootGate, false); + follower.followPath(intakeGate_shootGate, true); pathState = PathState.DRIVE_SHOOT_GATE; timeStamp = currentTime; } @@ -227,19 +222,17 @@ public class Auto15Ball_Back extends LinearOpMode { if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT_GATE; - spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + spindexer.startBackShooting(); } else if (follower.isBusy()){ timeStamp = currentTime; } break; case WAIT_SHOOT_GATE: - if (currentTime - timeStamp > rapidShootTime || - (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE && - spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){ - follower.followPath(shootGate_intakeGate, false); - pathState = PathState.INTAKE_GATE; + if (currentTime - timeStamp > rapidShootTime){ +// follower.followPath(shootGate_intakeGate, false); + follower.followPath(shootGate_leave, true); + pathState = PathState.LEAVE; timeStamp = currentTime; - // TODO: add logic for leave } break; case LEAVE: @@ -285,14 +278,14 @@ public class Auto15Ball_Back extends LinearOpMode { while (opModeInInit()){ park.unpark(); - robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); follower.update(); if (gamepad1.squareWasPressed()){ robot.setSpinPos(ServoPositions.spindexer_A2); - robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed); - robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed); + robot.setTurretPos(Turret.neutralPosition); } if (gamepad1.crossWasPressed() && !initializeRobot){ @@ -320,6 +313,8 @@ public class Auto15Ball_Back extends LinearOpMode { park.unpark(); } + NewShooterTest.transferPower = -0.6; + 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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java index 8bf096f..7172845 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java @@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.teleop.TeleopV4; +import org.firstinspires.ftc.teamcode.tests.NewShooterTest; import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; @@ -413,6 +414,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { park.unpark(); } + NewShooterTest.transferPower = -0.8; + 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()); 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 556c7ef..10b2e4d 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,6 +16,7 @@ public class ServoPositions { public static double spindexer_A3 = 0.54; public static double spindexer_B1 = 0.73; public static double spindexer_B2 = 0.92; + public static double spindexer_StopShooting = 0.44; public static double spindexer_intakePos1 = 0.18; //0.13; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 9cb6d9c..07a9cd8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utilsv2.*; @@ -36,6 +37,8 @@ public class TeleopV4 extends LinearOpMode { private boolean firstTickFull = true; private boolean intakeFull = true; private boolean shooting = false; + public static int flywheelOffset = 0; + public static double hoodOffset = 0; @Override public void runOpMode() throws InterruptedException { @@ -85,12 +88,15 @@ public class TeleopV4 extends LinearOpMode { if (gamepad1.triangleWasPressed()){ VelocityCommander.lockFront = true; VelocityCommander.lockBack = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); } else if (gamepad1.squareWasPressed()){ VelocityCommander.lockBack = true; VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK); } else if (gamepad1.circleWasPressed()){ VelocityCommander.lockBack = false; VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); } TELE.addLine("Initialization is done"); TELE.addData("Starting Position", follower.getPose()); @@ -137,23 +143,29 @@ public class TeleopV4 extends LinearOpMode { } if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){ - shooter.setFlywheelVelocity(2500); - robot.setHoodPos(0.6); + shooter.setFlywheelVelocity(2500 + flywheelOffset); + robot.setHoodPos(0.6 + hoodOffset); } if (gamepad1.triangleWasPressed()){ VelocityCommander.lockFront = true; VelocityCommander.lockBack = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE); TELE.addData("Front?:", true); TELE.addData("Back?:", false); } else if (gamepad1.squareWasPressed()){ VelocityCommander.lockBack = true; VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK); + spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE); TELE.addData("Front?:", false); TELE.addData("Back?:", true); } else if (gamepad1.circleWasPressed()){ VelocityCommander.lockBack = false; VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE); TELE.addData("Front?:", false); TELE.addData("Back?:", true); } @@ -161,50 +173,65 @@ public class TeleopV4 extends LinearOpMode { shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); - SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + if (VelocityCommander.lockBack){ + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + if (gamepad1.leftBumperWasPressed()){ + spindexerTransferIntake.startBackShooting(); + firstTickFull = true; + } - if (gamepad1.leftBumperWasPressed() && - (state == SpindexerTransferIntake.RapidMode.INTAKE || - state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || - state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || - state == SpindexerTransferIntake.RapidMode.PULSE_OUT || - state == SpindexerTransferIntake.RapidMode.PULSE_IN || - state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { - - spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); - intakeFull = false; - firstTickFull = true; - shooting = true; - } - - if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){ - shooting = false; - } - - if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){ - intakeFull = true; + if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){ + gamepad1.rumble(100); + firstTickFull = false; + } + TELE.addData("Intake State", spindexerTransferIntake.getSortedIntakeStates()); } else { - intakeFull = false; - firstTickFull = true; - } + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); - if (intakeFull && firstTickFull){ - gamepad1.rumble(100); - firstTickFull = false; - } + if (gamepad1.leftBumperWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { - if (gamepad1.right_trigger > 0.5 && - (state == SpindexerTransferIntake.RapidMode.INTAKE || - state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + intakeFull = false; + firstTickFull = true; + shooting = true; + } - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.HOLD_BALLS - ); - } + if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){ + shooting = false; + } - if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { - robot.setIntakePower(1); - robot.setTransferPower(-0.7); + if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){ + intakeFull = true; + } else { + intakeFull = false; + firstTickFull = true; + } + + if (intakeFull && firstTickFull){ + gamepad1.rumble(100); + firstTickFull = false; + } + + if (gamepad1.right_trigger > 0.5 && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + + if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { + robot.setIntakePower(1); + robot.setTransferPower(-0.7); + } } if (gamepad2.leftBumperWasPressed()){ @@ -219,6 +246,29 @@ public class TeleopV4 extends LinearOpMode { parkTilter.unpark(); } + if (gamepad2.leftBumperWasPressed()){ + limelightUsed = false; + } else if (gamepad2.rightBumperWasPressed()){ + limelightUsed = true; + } + + if (gamepad2.dpadUpWasPressed()){ + hoodOffset+=0.02; + } else if (gamepad2.dpadDownWasPressed()){ + hoodOffset-=0.02; + } + + if (gamepad2.dpadRightWasPressed()){ + flywheelOffset+=100; + } else if (gamepad2.dpadLeftWasPressed()){ + flywheelOffset-=100; + } + + if (gamepad2.crossWasPressed()){ + flywheelOffset = 0; + hoodOffset = 0; + } + loopTimes.loop(); // TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime()); // TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 446584c..1cfe2c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode.tests; -import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -13,8 +11,11 @@ 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.teleop.TeleopV4; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utilsv2.*; +import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed; + @TeleOp @Config public class NewShooterTest extends LinearOpMode { @@ -28,11 +29,14 @@ public class NewShooterTest extends LinearOpMode { Flywheel flywheel; VelocityCommander commander; ParkTilter parkTilter; + MeasuringLoopTimes loopTimes; + private boolean firstTickFull = true; + private boolean intakeFull = true; + private boolean shooting = false; public static int flywheelVelo = 0; public static double hoodPos = 0.5; public static double transferPower = -0.8; - public static boolean overrideTransferPower = false; @Override public void runOpMode() throws InterruptedException { @@ -51,6 +55,7 @@ public class NewShooterTest extends LinearOpMode { follower.setStartingPose(new Pose(0,0,0)); sleep(500); follower.setPose(new Pose(0,0,0)); + follower.update(); flywheel = new Flywheel(robot); turret = new Turret(robot); @@ -58,6 +63,9 @@ public class NewShooterTest extends LinearOpMode { parkTilter = new ParkTilter(robot); parkTilter.unpark(); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); shooter.setRedAlliance(Color.redAlliance); @@ -69,9 +77,30 @@ public class NewShooterTest extends LinearOpMode { limelightUsed = true; - TELE.addLine("Initialization Complete"); + TELE.addLine("Initialization is done"); TELE.update(); + while (opModeInInit()){ + if (gamepad1.triangleWasPressed()){ + VelocityCommander.lockFront = true; + VelocityCommander.lockBack = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + } else if (gamepad1.squareWasPressed()){ + VelocityCommander.lockBack = true; + VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK); + spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE); + } else if (gamepad1.circleWasPressed()){ + VelocityCommander.lockBack = false; + VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + } + TELE.addLine("Initialization is done"); + TELE.addData("Front?:", VelocityCommander.lockFront); + TELE.addData("Back?:", VelocityCommander.lockBack); + TELE.update(); + } + waitForStart(); if (isStopRequested()) return; @@ -97,6 +126,7 @@ public class NewShooterTest extends LinearOpMode { } follower.update(); + Pose currentPose = follower.getPose(); if (gamepad1.dpadLeftWasPressed()){ shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); @@ -111,56 +141,114 @@ public class NewShooterTest extends LinearOpMode { robot.setHoodPos(hoodPos); } + if (gamepad1.triangleWasPressed()){ + VelocityCommander.lockFront = true; + VelocityCommander.lockBack = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + TELE.addData("Front?:", true); + TELE.addData("Back?:", false); + } else if (gamepad1.squareWasPressed()){ + VelocityCommander.lockBack = true; + VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK); + TELE.addData("Front?:", false); + TELE.addData("Back?:", true); + } else if (gamepad1.circleWasPressed()){ + VelocityCommander.lockBack = false; + VelocityCommander.lockFront = false; + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + TELE.addData("Front?:", false); + TELE.addData("Back?:", true); + } -// shooter.setTurretPosition(turretPos); shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); + if (VelocityCommander.lockBack){ + if (gamepad1.leftBumperWasPressed() && spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE){ + spindexerTransferIntake.startBackShooting(); + firstTickFull = true; + } + + if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){ + gamepad1.rumble(100); + firstTickFull = false; + } + } else { + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + + if (gamepad1.leftBumperWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { + + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + intakeFull = false; + firstTickFull = true; + shooting = true; + } + + if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){ + shooting = false; + } + + if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){ + intakeFull = true; + } else { + intakeFull = false; + firstTickFull = true; + } + + if (intakeFull && firstTickFull){ + gamepad1.rumble(100); + firstTickFull = false; + } + + if (gamepad1.right_trigger > 0.5 && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + + if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { + robot.setIntakePower(1); + robot.setTransferPower(-0.7); + } + } + if (gamepad2.leftBumperWasPressed()){ limelightUsed = false; } else if (gamepad2.rightBumperWasPressed()){ limelightUsed = true; } - SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); - - if (gamepad1.leftBumperWasPressed() && - (state == SpindexerTransferIntake.RapidMode.INTAKE || - state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || - state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || - state == SpindexerTransferIntake.RapidMode.PULSE_OUT || - state == SpindexerTransferIntake.RapidMode.PULSE_IN || - state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { - - spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); - } - - if (gamepad1.right_trigger > 0.5 && - (state == SpindexerTransferIntake.RapidMode.INTAKE || - state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { - - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.HOLD_BALLS - ); - } - if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { - robot.setIntakePower(1); - } - if (gamepad1.dpad_down){ parkTilter.park(); } else if (gamepad1.dpad_up) { parkTilter.unpark(); } + loopTimes.loop(); +// TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime()); +// TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin()); +// TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin()); +// TELE.addData("Distance From Goal", commander.getDistance()); - TELE.addData("Hood Position", commander.getHoodPredicted()); - TELE.addData("Transfer Power", commander.getTransferPow()); +// TELE.addData("Hood Position", commander.getHoodPredicted()); +// TELE.addData("Transfer Power", robot.transfer.getPower()); TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); - TELE.addData("Manuel Velocity RPM", flywheelVelo); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); - TELE.addData("TX:", turret.getTX()); - +// +// TELE.addData("Current Position", currentPose); +// +// TELE.addData("Current LL Pipeline", turret.pipeline()); +// TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 1eef705..6a3c35f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.teleop.TeleopV4; @Config public class Shooter { @@ -138,8 +139,8 @@ public class Shooter { flywheelVelocity = commander.getPredictedRPM(); - robot.setHoodPos(commander.getHoodPredicted()); - fly.manageFlywheel(flywheelVelocity); + robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset); + fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset); fly.setF(voltage); break; case READ_OBELISK: @@ -180,9 +181,8 @@ public class Shooter { ); flywheelVelocity = commander.getPredictedRPM(); - robot.setHoodPos(commander.getHoodPredicted()); - - fly.manageFlywheel(flywheelVelocity); + robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset); + fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset); break; case MANUAL_FLYWHEEL_TRACK_TURR: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index c0cd3da..cc2de76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -179,7 +179,9 @@ public class SpindexerTransferIntake { public enum SpindexerMode { RAPID, SORTED, - SHOOT_SORTED + SHOOT_SORTED, + SPINDEXER_BACK, + SHOOT_BACK } public enum BallStates { @@ -241,6 +243,15 @@ public class SpindexerTransferIntake { SpindexerMode.SHOOT_SORTED ); } + public void startBackShooting(){ + setShootState( + SortedShootState.IDLE + ); + + setSpindexerMode( + SpindexerMode.SHOOT_BACK + ); + } public void setRapidMode(RapidMode newMode) { if (rapidMode != newMode) { rapidMode = newMode; @@ -263,6 +274,10 @@ public class SpindexerTransferIntake { return this.rapidMode; } + public SpindexerMode getSpindexerMode(){return this.mode;} + + public SortedIntakeStates getSortedIntakeStates(){return this.sortedIntakeStates;} + private long stateTime() { return System.currentTimeMillis() - stateStartTime; } @@ -538,20 +553,130 @@ public class SpindexerTransferIntake { } break; case REVERSE: - robot.setTransferPower(-0.3); - robot.setIntakePower(-0.1); + if (ballTicks > 4){ + robot.setTransferPower(0); + } else if (ballTicks > 1){ + robot.setTransferPower(1); + } else { + robot.setTransferPower(-1); + } + ballTicks++; + robot.setIntakePower(-0.7); + robot.setSpinPos(ServoPositions.spindexer_StopShooting); + break; + } + + + break; + case SPINDEXER_BACK: + + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + + switch (sortedIntakeStates) { + case NOTHING: + break; + case IDLE: + ballColors[0] = BallStates.UNKNOWN; + ballColors[1] = BallStates.UNKNOWN; + ballColors[2] = BallStates.UNKNOWN; + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Closed + ); + robot.setSpinPos( + ServoPositions.spindexer_A1 + ); + robot.setTransferServoPos( + ServoPositions.transferServo_out + ); + robot.setIntakePower(1); + robot.setTransferPower(-0.7); + if (sortedStateTime() > 200) { + setSortedIntakeMode(SortedIntakeStates.INTAKE_1); + } + break; + case INTAKE_1: + robot.setIntakePower(1); + robot.setTransferPower(-0.7); + if (robot.insideBeam.isPressed()) { + ballTicks++; + if (ballTicks > 0) { + robot.setSpinPos(ServoPositions.spindexer_A2); + setSortedIntakeMode(SortedIntakeStates.DELAY_1); + ballTicks = 0; + greenTicks = 0; + } + } + break; + case DELAY_1: + robot.setSpinPos(ServoPositions.spindexer_A2); + if (sortedStateTime() > spinMovementTime) { + setSortedIntakeMode(SortedIntakeStates.INTAKE_2); + } + break; + case INTAKE_2: + robot.setIntakePower(1); + robot.setTransferPower(-1); + if (robot.insideBeam.isPressed()) { + ballTicks++; + if (ballTicks > 0) { + robot.setSpinPos(ServoPositions.spindexer_A3); + setSortedIntakeMode(SortedIntakeStates.DELAY_2); + ballTicks = 0; + greenTicks = 0; + } + } + break; + case DELAY_2: + + robot.setSpinPos( + ServoPositions.spindexer_A3 + ); + + if (sortedStateTime() > spinMovementTime) { + setSortedIntakeMode( + SortedIntakeStates.INTAKE_3 + ); + } + + break; + case INTAKE_3: + robot.setIntakePower(1); + robot.setTransferPower(-1); + if (robot.insideBeam.isPressed()) { + ballTicks++; + if (ballTicks > 0) { + setSortedIntakeMode(SortedIntakeStates.REVERSE); + ballTicks = 0; + greenTicks = 0; + } + } + break; + case REVERSE: + if (ballTicks > 3){ + robot.setTransferPower(0); + } else if (ballTicks > 1){ + robot.setTransferPower(1); + } else { + robot.setTransferPower(-1); + } + ballTicks++; + robot.setIntakePower(-0.7); + robot.setSpinPos(ServoPositions.spindexer_StopShooting); break; } break; case SHOOT_SORTED: - + ballTicks = 0; robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); if (Shooter.manualFlywheel) { robot.setTransferPower(NewShooterTest.transferPower); } else { - robot.setTransferPower(commander.getTransferPow()); + robot.setTransferPower(-0.8); } @@ -740,6 +865,203 @@ public class SpindexerTransferIntake { } + break; + case SHOOT_BACK: + ballTicks = 0; + + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + if (Shooter.manualFlywheel) { + robot.setTransferPower(NewShooterTest.transferPower); + } else { + robot.setTransferPower(-0.8); + } + + + switch (shootState) { + case IDLE: + shootOrder = buildShootOrder( + ballColors, + desiredPattern + ); + + setShootState(SortedShootState.MOVE_TO_1); + mode = SpindexerMode.SHOOT_SORTED; + break; + case MOVE_TO_1: + + robot.setSpinPos(ServoPositions.spindexer_A3); + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Closed + ); + + + setShootState( + SortedShootState.WAIT_FOR_1 + ); + + break; + case WAIT_FOR_1: + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.SHOOT_1 + ); + } + + break; + + case SHOOT_1: + + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + robot.setTransferServoPos(ServoPositions.transferServo_in); + + + if (shootStateTime() > 300) { + + setShootState( + SortedShootState.RETRACT_1 + ); + } + + break; + case RETRACT_1: + + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed); + robot.setTransferServoPos(ServoPositions.transferServo_out); + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.MOVE_TO_2 + ); + } + + break; + case MOVE_TO_2: + + robot.setSpinPos(ServoPositions.spindexer_A2); + + setShootState( + SortedShootState.WAIT_FOR_2 + ); + + break; + case WAIT_FOR_2: + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.SHOOT_2 + ); + } + + break; + case SHOOT_2: + + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + robot.setTransferServoPos(ServoPositions.transferServo_in); + + if (shootStateTime() > 300) { + + setShootState( + SortedShootState.RETRACT_2 + ); + } + + break; + case RETRACT_2: + + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed); + robot.setTransferServoPos(ServoPositions.transferServo_out); + + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.MOVE_TO_3 + ); + } + + break; + case MOVE_TO_3: + + robot.setSpinPos(ServoPositions.spindexer_A1); + + setShootState( + SortedShootState.WAIT_FOR_3 + ); + + break; + case WAIT_FOR_3: + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.SHOOT_3 + ); + } + + break; + case SHOOT_3: + + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + robot.setTransferServoPos(ServoPositions.transferServo_in); + + if (shootStateTime() > 300) { + + setShootState( + SortedShootState.RETRACT_3 + ); + } + + break; + case RETRACT_3: + + robot.setTransferServoPos(ServoPositions.transferServo_out); + + if (shootStateTime() > 150) { + + setShootState( + SortedShootState.DONE + ); + } + + break; + case DONE: + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Closed + ); + robot.setSpinPos( + ServoPositions.spindexer_A1 + ); + robot.setTransferServoPos( + ServoPositions.transferServo_out + ); + robot.setIntakePower(1); + robot.setTransferPower(-0.7); + + if (shootStateTime() > 250) { + + setSortedIntakeMode( + SortedIntakeStates.IDLE + ); + + mode = SpindexerMode.SPINDEXER_BACK; + } + + break; + + } + + break; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index c2031ad..e09ed8d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -25,8 +25,8 @@ public class Turret { PIDController bearingPID; boolean bearingAligned = false; public int LL_COAST_TICKS = 5; - public static double TARGET_POSITION_TOLERANCE = 0.65; - public static double alphaTX = 0.5; + public static double TARGET_POSITION_TOLERANCE = 1.5; + public static double alphaTX = 0.3; private double targetTx = 0; private double currentTrackOffset = 0; private double llCoast = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java index 09bc369..eb15989 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -10,8 +10,8 @@ public class VelocityCommander { public static double yAccK = 0.025; // TODO: Tune public static boolean lockFront = false; public static boolean lockBack = false; - public static int farBound = 140; - public static int closeBound = 110; + public static int farBound = 138; + public static int closeBound = 112; public static double errorHoodAdjustment = 0.0005; private double hoodPos = 0.88; private double transferPow = -1; @@ -24,6 +24,10 @@ public class VelocityCommander { final double veloC = -0.0739531; final double veloD = 5.16759; final double veloE = 62.45781; + final double veloF = -0.0000833333; + final double veloG = 0.0377857; + final double veloH = -4.55067; + final double veloI = 456.97486; private double distToRPM (double dist){ double currentVelo; if (lockFront && dist > closeBound){ @@ -33,8 +37,8 @@ public class VelocityCommander { } if (dist < 54) { velo = 2000; - } else if (dist > 181){ - velo = 3600; + } else if (dist > 174){ + velo = 3700; } else { currentVelo = veloA*Math.pow(dist, 4) + veloB*Math.pow(dist, 3) + @@ -50,6 +54,8 @@ public class VelocityCommander { final double hoodB = -0.0000204165; final double hoodC = -0.00252089; final double hoodD = 1.06154; + final double hoodE = -0.002; + final double hoodF = 0.918; private void distToHood (double dist){ if (dist > 174){ hoodPos = 0.48; @@ -69,9 +75,9 @@ public class VelocityCommander { private void distToTransferPow(double dist, double voltage){ if (dist < 140){ - transferPow = -0.8; + transferPow = -0.75; } else { - transferPow = -0.5; + transferPow = -0.6; } } public double getTransferPow(){return transferPow;} @@ -94,7 +100,7 @@ public class VelocityCommander { distToTransferPow(predictedDist, voltage); distToRPM(predictedDist); - hoodPos += adjustHood(predictedDist, velocity, velo); +// hoodPos += adjustHood(predictedDist, velocity, velo); } public double adjustHood(double dist, double currentVelocity, double targetVelocity){