diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java new file mode 100644 index 0000000..69606da --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java @@ -0,0 +1,141 @@ +package org.firstinspires.ftc.teamcode.tests; + +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +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.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.*; + +@TeleOp +@Config +public class SortedSpindexerTest extends LinearOpMode { + Robot robot; + Drivetrain drivetrain; + Shooter shooter; + MultipleTelemetry TELE; + Follower follower; + SpindexerTransferIntake spindexerTransferIntake; + Turret turret; + Flywheel flywheel; + VelocityCommander commander; + + ParkTilter parkTilter; + public static String order = "GPP"; + + @Override + public void runOpMode() throws InterruptedException { + + Robot.resetInstance(); + + robot = Robot.getInstance(hardwareMap); + + TELE = new MultipleTelemetry( + FtcDashboard.getInstance().getTelemetry(), telemetry + ); + + commander = new VelocityCommander(); + drivetrain = new Drivetrain(robot, TELE); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); + + flywheel = new Flywheel(robot); + turret = new Turret(robot); + + parkTilter = new ParkTilter(robot); + + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + shooter.setRedAlliance(Color.redAlliance); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED); + + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + switch(order) { + case "PPG": + spindexerTransferIntake.setDesiredPattern( + SpindexerTransferIntake.DesiredPattern.PPG + ); + break; + + case "PGP": + spindexerTransferIntake.setDesiredPattern( + SpindexerTransferIntake.DesiredPattern.PGP + ); + break; + + default: + spindexerTransferIntake.setDesiredPattern( + SpindexerTransferIntake.DesiredPattern.GPP + ); + } + + + //Drivetrain + drivetrain.drive( + -gamepad1.right_stick_y, + gamepad1.right_stick_x, + gamepad1.left_stick_x + ); + + follower.update(); + + + shooter.update(robot.voltage.getVoltage()); + spindexerTransferIntake.update(); + + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + + if(gamepad1.leftBumperWasPressed()) { + spindexerTransferIntake.startSortedShoot(); + } + + if (gamepad1.right_trigger > 0.5 && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + if (gamepad1.rightBumperWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + if (gamepad1.dpad_down){ + parkTilter.park(); + } else if (gamepad1.dpad_up) { + parkTilter.unpark(); + } + + TELE.addData("Distance From Goal", commander.getDistance()); + TELE.addData("Hood Position", commander.getHoodPredicted()); + TELE.addData("Transfer Power", robot.transfer.getPower()); + TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + + TELE.update(); + } + + } +} 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 901c50b..ea14c5f 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 @@ -17,9 +17,12 @@ public class SpindexerTransferIntake { VelocityCommander commander; - public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE, VelocityCommander com) { + private MultipleTelemetry TELE; + + public SpindexerTransferIntake(Robot rob, MultipleTelemetry tele, VelocityCommander com) { this.robot = rob; this.commander = com; + this.TELE = tele; } public enum DesiredPattern { @@ -200,7 +203,7 @@ public class SpindexerTransferIntake { private RapidMode rapidMode = RapidMode.INTAKE; private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE; private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN}; - private final double greenThresh = 0.40; + private final double greenThresh = 0.39; private final double spinMovementTime = 250; /** @@ -209,6 +212,25 @@ public class SpindexerTransferIntake { private long stateStartTime = System.currentTimeMillis(); private long sortedStateStartTime = System.currentTimeMillis(); + public void setDesiredPattern(DesiredPattern pattern) { + desiredPattern = pattern; + } + + public void startSortedShoot() { + + shootOrder = buildShootOrder( + ballColors, + desiredPattern + ); + + setShootState( + SortedShootState.IDLE + ); + + setSpindexerMode( + SpindexerMode.SHOOT_SORTED + ); + } public void setRapidMode(RapidMode newMode) { if (rapidMode != newMode) { rapidMode = newMode; @@ -241,6 +263,21 @@ public class SpindexerTransferIntake { public void update() { + TELE.addData("Sorted State", sortedIntakeStates); + TELE.addData("Ball0", ballColors[0]); + TELE.addData("Ball1", ballColors[1]); + TELE.addData("Ball2", ballColors[2]); + + TELE.addData("Shoot0", shootOrder[0]); + TELE.addData("Shoot1", shootOrder[1]); + TELE.addData("Shoot2", shootOrder[2]); + + TELE.addData("Color0", ballColors[0]); + TELE.addData("Color1", ballColors[1]); + TELE.addData("Color2", ballColors[2]); + + TELE.addData("Shoot State", shootState); + switch (mode) { case RAPID: @@ -369,6 +406,9 @@ public class SpindexerTransferIntake { case NOTHING: break; case IDLE: + ballColors[0] = BallStates.UNKNOWN; + ballColors[1] = BallStates.UNKNOWN; + ballColors[2] = BallStates.UNKNOWN; robot.setRapidFireBlockerPos( ServoPositions.rapidFireBlocker_Open ); @@ -391,6 +431,7 @@ public class SpindexerTransferIntake { robot.setIntakePower(1); robot.setTransferPower(-1); if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { + //TODO: ADD DELAY OR AVERGE @ DANIEL NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) { ballColors[0] = BallStates.GREEN; @@ -457,7 +498,7 @@ public class SpindexerTransferIntake { break; case SHOOT_SORTED: - robot.setIntakePower(commander.getTransferPow()); + robot.setTransferPower(commander.getTransferPow()); switch (shootState) { @@ -469,6 +510,7 @@ public class SpindexerTransferIntake { setShootState(SortedShootState.MOVE_TO_1); mode = SpindexerMode.SHOOT_SORTED; + break; case MOVE_TO_1: moveToSlot(shootOrder[0]); @@ -630,9 +672,14 @@ public class SpindexerTransferIntake { robot.setIntakePower(1); robot.setTransferPower(-1); - setSortedIntakeMode(SortedIntakeStates.NOTHING); + if (shootStateTime() > 250) { - mode = SpindexerMode.SORTED; + setSortedIntakeMode( + SortedIntakeStates.IDLE + ); + + mode = SpindexerMode.SORTED; + } break;