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 946037c..77d4d41 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,11 +18,10 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; +import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; -import org.firstinspires.ftc.teamcode.utilsv2.Robot; -import org.firstinspires.ftc.teamcode.utilsv2.Turret; import java.util.List; @@ -33,45 +32,52 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { MultipleTelemetry TELE; Follower follower; MeasuringLoopTimes loopTimes; + Shooter shooter; + Turret turret; + Flywheel flywheel; + VelocityCommander commander; + SpindexerTransferIntake spindexer; // Wait Times - public static double shootTime = 2; - public static double openGateTime = 1.5; + public static double sortedShootTime = 2; + public static double rapidWaitTime = 0.25; + public static double rapidShootTime = 1; + public static double openGateTime = 2.5; + public static double pushTime = 2; // Extra Variables - public static double intakePower = 0.3; + public static double intakePower = 0.5; double shootX, shootY, shootH; // Initialize path state machine private enum PathState { PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, - PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, + PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 } PathState pathState = PathState.PUSHBOT; // Poses - public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; - public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; - public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454; - public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; - public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031; + public static double startPoseX = 91.5, startPoseY = 15, startPoseH = 90; + public static double pushBotX = 97, pushBotY = 18, pushBotH = 100; + public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454; + public static double shoot0X = 95, shoot0Y = 83, shoot0H = 0; public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; - public static double openGateX = 129, openGateY = 74, openGateH = 0; + public static double openGateX = 131, openGateY = 74, openGateH = 0; public static double shoot1ControlX = 112, shoot1ControlY = 75; - public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; - public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; + public static double shoot1X = 95, shoot1Y = 83, shoot1H = 0; + public static double drivePickup2X = 98, drivePickup2Y = 58.5, drivePickup2H = 0; public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; public static double shoot2ControlX = 102, shoot2ControlY = 63; - public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; - public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; + public static double shoot2X = 95, shoot2Y = 83, shoot2H = 0; + public static double drivePickup3X = 98, drivePickup3Y = 34.5, drivePickup3H = 0; public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; - public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; + public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90; Pose startPose, pushBot, shoot0Control, shoot0, - pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1, + pickup1, openGateControl, openGate, shoot1Control, shoot1, drivePickup2, pickup2, shoot2Control, shoot2, drivePickup3, pickup3, shoot3Control, shoot3; private void initializePoses(){ @@ -79,7 +85,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); shoot0Control = new Pose(shoot0ControlX, shoot0ControlY); shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); - pickup1Control = new Pose(pickup1ControlX, pickup1ControlY); pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); openGateControl = new Pose(openGateControlX, openGateControlY); openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); @@ -112,7 +117,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { .build(); shoot0_pickup1 = follower.pathBuilder() - .addPath(new BezierCurve(shoot0, pickup1Control, pickup1)) + .addPath(new BezierLine(shoot0, pickup1)) .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading()) .build(); @@ -164,42 +169,46 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { double currentTime = (double) System.currentTimeMillis() / 1000; switch(pathState){ case PUSHBOT: + spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + shooter.setFlywheelVelocity(2400); + robot.setHoodPos(0.64); if (startAuto){ - follower.followPath(startPose_pushBot, true); + follower.followPath(startPose_pushBot, false); startAuto = false; + timeStamp = currentTime; shootX = shoot0X; shootY = shoot0Y; shootH = shoot0H; } - if (!follower.isBusy()){ + if (!follower.isBusy() || currentTime - timeStamp > pushTime){ follower.followPath(pushBot_shoot0, true); pathState = PathState.DRIVE_SHOOT0; + timeStamp = currentTime; } break; case DRIVE_SHOOT0: - if (!follower.isBusy()){ + if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT0; + spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); } break; case WAIT_SHOOT0: - if (currentTime - timeStamp > shootTime){ + if (currentTime - timeStamp > rapidShootTime && + (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE && + spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){ follower.followPath(shoot0_pickup1, intakePower, false); pathState = PathState.PICKUP1; + spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED); } break; case PICKUP1: if (!follower.isBusy()){ - follower.followPath(pickup1_openGate, true); + follower.followPath(pickup1_openGate, false); pathState = PathState.OPENGATE; shootX = shoot1X; shootY = shoot1Y; shootH = shoot1H; - } - break; - case DRIVE_OPENGATE: - if (!follower.isBusy()){ - pathState = PathState.OPENGATE; timeStamp = currentTime; } break; @@ -213,10 +222,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (!follower.isBusy()){ pathState = PathState.WAIT_SHOOT1; timeStamp = currentTime; + spindexer.startSortedShoot(); } break; case WAIT_SHOOT1: - if (currentTime - timeStamp > shootTime){ + if (currentTime - timeStamp > sortedShootTime){ follower.followPath(shoot1_drivePickup2, true); pathState = PathState.DRIVE_PICKUP2; } @@ -240,16 +250,19 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (!follower.isBusy()){ pathState = PathState.WAIT_SHOOT2; timeStamp = currentTime; + spindexer.startSortedShoot(); } break; case WAIT_SHOOT2: - if (currentTime - timeStamp > shootTime){ + if (currentTime - timeStamp > sortedShootTime){ follower.followPath(shoot2_drivePickup3, true); pathState = PathState.DRIVE_PICKUP3; } break; case DRIVE_PICKUP3: if (!follower.isBusy()){ + shooter.setFlywheelVelocity(2300); + robot.setHoodPos(0.68); follower.followPath(drivePickup3_pickup3, intakePower, false); pathState = PathState.PICKUP3; } @@ -266,6 +279,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { case DRIVE_SHOOT3: if (!follower.isBusy()){ pathState = PathState.WAIT_SHOOT3; + spindexer.startSortedShoot(); } break; case WAIT_SHOOT3: @@ -296,19 +310,33 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { } TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); follower = Constants.createFollower(hardwareMap); + sleep(1000); follower.setStartingPose(new Pose(72,72,0)); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); + turret = new Turret(robot); + flywheel = new Flywheel(robot); + commander = new VelocityCommander(); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); + spindexer = new SpindexerTransferIntake(robot, TELE, commander); + ParkTilter park = new ParkTilter(robot); boolean initializeRobot = false; while (opModeInInit()){ follower.update(); + if (gamepad1.squareWasPressed()){ + robot.setSpinPos(ServoPositions.spindexer_A2); + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed); + robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); + } + if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; + shooter.setRedAlliance(Color.redAlliance); double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X, - pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X, + pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; @@ -326,14 +354,26 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { initializePoses(); follower.setPose(startPose); buildPaths(); -// turret.switchPipeline(Turret.PipelineMode.OBELISK); - robot.limelight.start(); sleep(2000); + turret.switchPipeline(Turret.PipelineMode.OBELISK); + robot.limelight.start(); + limelightUsed = true; + park.unpark(); + } + + if (initializeRobot){ + //add obelisk read here + shooter.setState(Shooter.ShooterState.READ_OBELISK); + int ID = turret.getObeliskID(); + spindexer.setDesiredPatternAuto(ID); + TELE.addData("ID", ID); + shooter.update(robot.voltage.getVoltage()); } TELE.addData("Red Alliance?", Color.redAlliance); TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); TELE.addData("Start Pose", follower.getPose()); + TELE.addData("Current LL Pipeline", turret.pipeline()); TELE.update(); } @@ -341,9 +381,10 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (isStopRequested()) return; - limelightUsed = false; - while (opModeIsActive()){ + shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); + shooter.update(robot.voltage.getVoltage()); + follower.update(); pathStateMachine(); Pose currentPose = follower.getPose(); @@ -351,6 +392,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { teleStartPoseY = currentPose.getY(); teleStartPoseH = Math.toDegrees(currentPose.getHeading()); + spindexer.update(); + for (LynxModule hub : allHubs) { hub.clearBulkCache(); } 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 7aa9159..556c7ef 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 @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double rapidFireBlocker_Closed = 0.35; + public static double rapidFireBlocker_Closed = 0.32; public static double rapidFireBlocker_Open = 0.5; public static double spindexBlocker_Closed = 0.31; @@ -34,9 +34,9 @@ public class ServoPositions { public static double shootAllSpindexerSpeedIncrease = 0.01; - public static double transferServo_out = 0.57; + public static double transferServo_out = 0.28; - public static double transferServo_in = 0.77; + public static double transferServo_in = 0.54; public static double hoodAuto = 0.27; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 268c78c..d7a8aad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -5,7 +5,7 @@ import com.pedropathing.control.FilteredPIDFCoefficients; import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.FollowerBuilder; import com.pedropathing.ftc.drivetrains.MecanumConstants; import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; @@ -22,7 +22,7 @@ public class Constants { .lateralZeroPowerAcceleration(-60.876) .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02)) .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) - .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.013, 0, 0.00001, 0.6, 0.015)) .centripetalScaling(0.0005); public static MecanumConstants driveConstants = new MecanumConstants() @@ -51,10 +51,16 @@ public class Constants { .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD); public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) + FollowerBuilder followerBuilder; + followerBuilder = new FollowerBuilder(followerConstants, hardwareMap) .pathConstraints(pathConstraints) .mecanumDrivetrain(driveConstants) - .pinpointLocalizer(localizerConstants) - .build(); + .pinpointLocalizer(localizerConstants); + + followerBuilder.resetPinpoint(); + followerBuilder.recalibrateIMU(); + + return followerBuilder.build(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java new file mode 100644 index 0000000..8e04447 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java @@ -0,0 +1,106 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.drivetrain.Drivetrain; +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.drivetrains.*; +import com.pedropathing.ftc.localization.constants.DriveEncoderConstants; +import com.pedropathing.ftc.localization.constants.OTOSConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; +import com.pedropathing.ftc.localization.constants.ThreeWheelConstants; +import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants; +import com.pedropathing.ftc.localization.constants.TwoWheelConstants; +import com.pedropathing.ftc.localization.localizers.DriveEncoderLocalizer; +import com.pedropathing.ftc.localization.localizers.OTOSLocalizer; +import com.pedropathing.ftc.localization.localizers.PinpointLocalizer; +import com.pedropathing.ftc.localization.localizers.ThreeWheelIMULocalizer; +import com.pedropathing.ftc.localization.localizers.ThreeWheelLocalizer; +import com.pedropathing.ftc.localization.localizers.TwoWheelLocalizer; +import com.pedropathing.localization.Localizer; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.robotcore.hardware.HardwareMap; + +/** This is the FollowerBuilder. + * It is used to create Followers with a specific drivetrain + localizer without having to use a full constructor + * + * @author Baron Henderson - 20077 The Indubitables + */ +public class FollowerBuilder { + private final FollowerConstants constants; + private PathConstraints constraints; + private final HardwareMap hardwareMap; + private Localizer localizer; + private Drivetrain drivetrain; + + public FollowerBuilder(FollowerConstants constants, HardwareMap hardwareMap) { + this.constants = constants; + this.hardwareMap = hardwareMap; + constraints = PathConstraints.defaultConstraints; + } + + public FollowerBuilder setLocalizer(Localizer localizer) { + this.localizer = localizer; + return this; + } + + public FollowerBuilder driveEncoderLocalizer(DriveEncoderConstants lConstants) { + return setLocalizer(new DriveEncoderLocalizer(hardwareMap, lConstants)); + } + + public FollowerBuilder OTOSLocalizer(OTOSConstants lConstants) { + return setLocalizer(new OTOSLocalizer(hardwareMap, lConstants)); + } + + PinpointLocalizer pinpointLocalizer; + public FollowerBuilder pinpointLocalizer(PinpointConstants lConstants) { + pinpointLocalizer = new PinpointLocalizer(hardwareMap, lConstants); + return setLocalizer(pinpointLocalizer); + } + + public void resetPinpoint(){ + pinpointLocalizer.resetIMU(); + } + + public void recalibrateIMU(){ + pinpointLocalizer.recalibrate(); + } + + public FollowerBuilder threeWheelIMULocalizer(ThreeWheelIMUConstants lConstants) { + return setLocalizer(new ThreeWheelIMULocalizer(hardwareMap, lConstants)); + } + + public FollowerBuilder threeWheelLocalizer(ThreeWheelConstants lConstants) { + return setLocalizer(new ThreeWheelLocalizer(hardwareMap, lConstants)); + } + + public FollowerBuilder twoWheelLocalizer(TwoWheelConstants lConstants) { + return setLocalizer(new TwoWheelLocalizer(hardwareMap, lConstants)); + } + + public FollowerBuilder setDrivetrain(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + return this; + } + + public FollowerBuilder mecanumDrivetrain(MecanumConstants mecanumConstants) { + return setDrivetrain(new Mecanum(hardwareMap, mecanumConstants)); + } + + @Deprecated + public FollowerBuilder mecanumExDrivetrain(MecanumConstants mecanumConstants) { + return setDrivetrain(new MecanumEx(hardwareMap, mecanumConstants)); + } + + public FollowerBuilder swerveDrivetrain(SwerveConstants swerveConstants, SwervePod... pods) { + return setDrivetrain(new Swerve(hardwareMap, swerveConstants, pods)); + } + + public FollowerBuilder pathConstraints(PathConstraints pathConstraints) { + this.constraints = pathConstraints; + PathConstraints.setDefaultConstraints(pathConstraints); + return this; + } + + public Follower build() { + return new Follower(constants, localizer, drivetrain, constraints); + } +} 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 7b942f9..80459f3 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 @@ -161,6 +161,8 @@ public class TeleopV4 extends LinearOpMode { 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/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 0830331..188f88e 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 @@ -34,7 +34,7 @@ public class NewShooterTest extends LinearOpMode { public static int flywheelVelo = 0; public static double hoodPos = 0.5; - public static double transferPower = -0.7; + public static double transferPower = -0.8; // public static double turretPos = 0.51; @Override 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 78a244c..45d5fdc 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 @@ -140,11 +140,9 @@ public class Shooter { break; case READ_OBELISK: manualFlywheel = false; - turr.trackObelisk( - (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), - (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), - follow.getHeading() - ); + robot.setTurretPos(Turret.neutralPosition); + + turr.detectObelisk(); commander.getVeloPredictive( (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), @@ -158,7 +156,7 @@ public class Shooter { flywheelVelocity = commander.getPredictedRPM(); - fly.manageFlywheel(flywheelVelocity); + fly.manageFlywheel(0); fly.setF(voltage); break; 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 1522488..a939e92 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 @@ -216,6 +216,16 @@ public class SpindexerTransferIntake { desiredPattern = pattern; } + public void setDesiredPatternAuto(int id) { + if (id == 22){ + desiredPattern = DesiredPattern.PGP; + } else if (id == 23){ + desiredPattern = DesiredPattern.PPG; + } else { + desiredPattern = DesiredPattern.GPP; + } + } + public void startSortedShoot() { shootOrder = buildShootOrder( @@ -404,6 +414,8 @@ public class SpindexerTransferIntake { case SORTED: + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + switch (sortedIntakeStates) { case NOTHING: break; @@ -424,14 +436,14 @@ public class SpindexerTransferIntake { ServoPositions.transferServo_out ); robot.setIntakePower(1); - robot.setTransferPower(-1); + robot.setTransferPower(-0.7); if (sortedStateTime() > 200) { setSortedIntakeMode(SortedIntakeStates.INTAKE_1); } break; case INTAKE_1: robot.setIntakePower(1); - robot.setTransferPower(-1); + robot.setTransferPower(-0.7); if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) { @@ -458,8 +470,6 @@ public class SpindexerTransferIntake { } break; case INTAKE_2: - robot.setIntakePower(1); - robot.setTransferPower(-1); robot.setIntakePower(1); robot.setTransferPower(-1); if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { @@ -470,9 +480,9 @@ public class SpindexerTransferIntake { ballTicks++; if (ballTicks > 15) { if (greenTicks > 10){ - ballColors[0] = BallStates.GREEN; + ballColors[1] = BallStates.GREEN; } else { - ballColors[0] = BallStates.PURPLE; + ballColors[1] = BallStates.PURPLE; } robot.setSpinPos(ServoPositions.spindexer_A3); setSortedIntakeMode(SortedIntakeStates.DELAY_2); @@ -505,9 +515,9 @@ public class SpindexerTransferIntake { ballTicks++; if (ballTicks > 15) { if (greenTicks > 10){ - ballColors[0] = BallStates.GREEN; + ballColors[2] = BallStates.GREEN; } else { - ballColors[0] = BallStates.PURPLE; + ballColors[2] = BallStates.PURPLE; } setSortedIntakeMode(SortedIntakeStates.REVERSE); ballTicks = 0; @@ -525,7 +535,12 @@ public class SpindexerTransferIntake { break; case SHOOT_SORTED: - robot.setTransferPower(commander.getTransferPow()); + robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open); + if (Shooter.manualFlywheel) { + robot.setTransferPower(NewShooterTest.transferPower); + } else { + robot.setTransferPower(commander.getTransferPow()); + } switch (shootState) { @@ -697,7 +712,7 @@ public class SpindexerTransferIntake { ServoPositions.transferServo_out ); robot.setIntakePower(1); - robot.setTransferPower(-1); + robot.setTransferPower(-0.7); if (shootStateTime() > 250) { 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 f239aac..40197ee 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 @@ -16,7 +16,7 @@ public class Turret { Robot robot; private final double servoTicksPer180 = 0.58; - private final double neutralPosition = 0.51; + public static double neutralPosition = 0.51; private final double turretMin = 0.05; private final double turretMax = 0.95; public static boolean limelightUsed = true; @@ -83,11 +83,12 @@ public class Turret { pipeline = 2; } } - if (pipeline == 0){prevPipeline = 0;} if (pipeline != prevPipeline){ robot.limelight.pipelineSwitch(pipeline); } + prevPipeline = pipeline; } + public int pipeline(){return prevPipeline;} public void trackObelisk(double dx, double dy, double h) { @@ -107,7 +108,6 @@ public class Turret { robot.setTurretPos(servoAngle); - detectObelisk(); } @@ -116,20 +116,14 @@ public class Turret { return obeliskID; } - private int detectObelisk() { - switchPipeline(PipelineMode.OBELISK); + public void detectObelisk() { result = robot.limelight.getLatestResult(); if (result != null && result.isValid()) { List fiducials = result.getFiducialResults(); - double prevTx = -1000; for (LLResultTypes.FiducialResult fiducial : fiducials) { - double currentTx = fiducial.getTargetXDegrees(); - if (currentTx > prevTx){ - obeliskID = fiducial.getFiducialId(); - } + obeliskID = fiducial.getFiducialId(); } } - return obeliskID; } public void manual (double pos) {