From fab0d6346d097e3c449805c055f2e1eade57af21 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Tue, 10 Feb 2026 20:44:12 -0600 Subject: [PATCH] Lights work --- .../ftc/teamcode/constants/Color.java | 5 + .../constants/{Types.java => StateEnums.java} | 11 +- .../ftc/teamcode/teleop/TeleopV3.java | 36 +- .../ftc/teamcode/utils/Light.java | 108 ++++++ .../ftc/teamcode/utils/Spindexer.java | 338 ++++++++++-------- .../ftc/teamcode/utils/Turret.java | 16 +- 6 files changed, 349 insertions(+), 165 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/{Types.java => StateEnums.java} (55%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java index fea5d76..ccded35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java @@ -6,6 +6,11 @@ import com.acmerobotics.dashboard.config.Config; public class Color { public static boolean redAlliance = true; public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5; + public static double LightGreen = 0.5; + public static double LightPurple = 0.715; + public static double LightOrange = 0.33; + public static double LightRed = 0.28; + public static double LightBlue = 0.6; public static double colorFilterAlpha = 1; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/StateEnums.java similarity index 55% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/StateEnums.java index c619eb1..a3f48bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/StateEnums.java @@ -1,10 +1,19 @@ package org.firstinspires.ftc.teamcode.constants; -public class Types { +public class StateEnums { public enum Motif { NONE, GPP, // Green, Purple, Purple PGP, // Purple, Green, Purple PPG // Purple, Purple, Green } + + public enum LightState { + BALL_COUNT, + BALL_COLOR, + GOAL_LOCK, + MANUAL, + DISABLED, + OFF + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index d406445..ffcbf0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -16,9 +16,12 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Light; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; @@ -47,6 +50,7 @@ public class TeleopV3 extends LinearOpMode { boolean fixedTurret = false; Robot robot; MultipleTelemetry TELE; + Light light; Servos servo; Flywheel flywheel; MecanumDrive drive; @@ -63,7 +67,6 @@ public class TeleopV3 extends LinearOpMode { double headingOffset = 0.0; int ticker = 0; - boolean autoSpintake = false; boolean enableSpindexerManager = true; @@ -99,18 +102,26 @@ public class TeleopV3 extends LinearOpMode { tController.setTolerance(0.001); Turret turret = new Turret(robot, TELE, robot.limelight); - robot.light.setPosition(1); + + light = Light.getInstance(); + light.init(robot.light, spindexer, turret); + + light.setState(StateEnums.LightState.MANUAL); while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { robot.limelight.pipelineSwitch(4); + light.setManualLightColor(Color.LightRed); } else { robot.limelight.pipelineSwitch(2); + light.setManualLightColor(Color.LightBlue); + } + + light.update(); } - limelightUsed= true; - + limelightUsed = true; waitForStart(); if (isStopRequested()) return; @@ -121,9 +132,6 @@ public class TeleopV3 extends LinearOpMode { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); - // LIGHT COLORS - spindexer.ballCounterLight(); - //DRIVETRAIN: drivetrain.drive( @@ -134,9 +142,17 @@ public class TeleopV3 extends LinearOpMode { ); if (gamepad1.right_bumper) { + shootAll = false; robot.transferServo.setPosition(transferServo_out); + light.setState(StateEnums.LightState.BALL_COUNT); + + } else if (gamepad2.triangle){ + light.setState(StateEnums.LightState.BALL_COLOR); + + } else { + light.setState(StateEnums.LightState.GOAL_LOCK); } robot.transfer.setPower(1); @@ -227,8 +243,6 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); } - - if (enableSpindexerManager) { //if (!shootAll) { spindexer.processIntake(); @@ -332,7 +346,7 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("robotX", robotX); TELE.addData("robotY", robotY); TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData( "robotInchesY", targeting.robotInchesY); + TELE.addData("robotInchesY", targeting.robotInchesY); TELE.addData("Targeting Interpolate", turretInterpolate); TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridY", targeting.robotGridY); @@ -342,6 +356,8 @@ public class TeleopV3 extends LinearOpMode { TELE.update(); + light.update(); + ticker++; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java new file mode 100644 index 0000000..dc77b07 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.StateEnums.LightState; + + +@Config +public final class Light { + + private static Light instance; + public static double ballColorCycleTime = 1000; //in ms + public static double restingTime = 150; //in ms + + private Servo lightServo; + private LightState state = LightState.DISABLED; + + // References to other systems (NOT static) + private Spindexer spindexer; + private Turret turret; + private double manualLightColor = Color.Light0; + + private double lightColor = Color.Light0; + private double previousLightColor = lightColor; + + private Light() { + } + + public static synchronized Light getInstance() { + if (instance == null) { + instance = new Light(); + } + return instance; + } + + // Call once in OpMode init() + public void init( + Servo servo, + Spindexer spin, + Turret turr + ) { + this.lightServo = servo; + this.spindexer = spin; + this.turret = turr; + } + + public void setManualLightColor(double value) { + this.manualLightColor = value; + } + + public void setState(LightState newState) { + state = newState; + } + + public void update() { + if (lightServo == null) return; + + switch (state) { + + case BALL_COUNT: + lightColor = spindexer.ballCounterLight(); + break; + + case BALL_COLOR: + + if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { + lightColor = spindexer.getRearCenterLight(); + } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) { + lightColor = 0; + } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { + lightColor = spindexer.getDriverLight(); + + } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { + lightColor = 0; + } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { + lightColor = spindexer.getPassengerLight(); + + } else { + lightColor = 0; + } + break; + + case GOAL_LOCK: + lightColor = turret.getLightColor(); + break; + + case MANUAL: + lightColor = manualLightColor; + break; + + case DISABLED: + break; + + case OFF: + lightColor = 0; + break; + } + + if (lightColor != previousLightColor) { + lightServo.setPosition(lightColor); + } + + previousLightColor = lightColor; + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index ea5aa6e..30ea571 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -1,10 +1,13 @@ package org.firstinspires.ftc.teamcode.utils; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.controller.PIDFController; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import static org.firstinspires.ftc.teamcode.constants.Color.*; +import static org.firstinspires.ftc.teamcode.constants.Color.Light0; +import static org.firstinspires.ftc.teamcode.constants.Color.Light1; +import static org.firstinspires.ftc.teamcode.constants.Color.Light2; +import static org.firstinspires.ftc.teamcode.constants.Color.Light3; +import static org.firstinspires.ftc.teamcode.constants.Color.LightGreen; +import static org.firstinspires.ftc.teamcode.constants.Color.LightOrange; +import static org.firstinspires.ftc.teamcode.constants.Color.LightPurple; +import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; @@ -19,89 +22,51 @@ import static org.firstinspires.ftc.teamcode.utils.Servos.spinF; import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; import static org.firstinspires.ftc.teamcode.utils.Servos.spinP; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.hardware.HardwareMap; + import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.constants.Types; +import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import java.util.Objects; + public class Spindexer { + // Array of commandedIntakePositions with contents + // {RearCenter, FrontDriver, FrontPassenger} + static final int[][] RotatedBallPositions = {{0, 2, 1}, {1, 0, 2}, {2, 1, 0}}; + public int commandedIntakePosition = 0; + public double distanceRearCenter = 0.0; + public double distanceFrontDriver = 0.0; + public double distanceFrontPassenger = 0.0; + public double spindexerWiggle = 0.01; + public double spindexerOuttakeWiggle = 0.01; + public double spindexerPosOffset = 0.00; + public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE; + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; + public int unknownColorDetect = 0; + public int counter = 0; Robot robot; Servos servos; Flywheel flywheel; MecanumDrive drive; double lastKnownSpinPos = 0.0; MultipleTelemetry TELE; - PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF); - double spinCurrentPos = 0.0; - - public int commandedIntakePosition = 0; - - public double distanceRearCenter = 0.0; - public double distanceFrontDriver = 0.0; - public double distanceFrontPassenger = 0.0; - - public double spindexerWiggle = 0.01; - - public double spindexerOuttakeWiggle = 0.01; - private double prevPos = 0.0; - public double spindexerPosOffset = 0.00; - public Types.Motif desiredMotif = Types.Motif.NONE; - // For Use - enum RotatedBallPositionNames { - REARCENTER, - FRONTDRIVER, - FRONTPASSENGER - } - // Array of commandedIntakePositions with contents - // {RearCenter, FrontDriver, FrontPassenger} - static final int[][] RotatedBallPositions = {{0,2,1}, {1,0,2}, {2,1,0}}; - class spindexerBallRoatation { - int rearCenter = 0; // aka commanded Position - int frontDriver = 0; - int frontPassenger = 0; - } - - enum IntakeState { - UNKNOWN_START, - UNKNOWN_MOVE, - UNKNOWN_DETECT, - INTAKE, - FINDNEXT, - MOVING, - FULL, - SHOOTNEXT, - SHOOTMOVING, - SHOOTWAIT, - SHOOT_ALL_PREP, - SHOOT_ALL_READY, - SHOOT_PREP_CONTINOUS, - SHOOT_CONTINOUS - } - int shootWaitCount = 0; - - public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; - public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; - public int unknownColorDetect = 0; - public enum BallColor { - UNKNOWN, - GREEN, - PURPLE - } - - class BallPosition { - boolean isEmpty = true; - int foundEmpty = 0; - BallColor ballColor = BallColor.UNKNOWN; - } - BallPosition[] ballPositions = new BallPosition[3]; - - public boolean init () { - return true; - } + double[] outakePositions = + {spindexer_outtakeBall1 + spindexerPosOffset, + spindexer_outtakeBall2 + spindexerPosOffset, + spindexer_outtakeBall3 + spindexerPosOffset}; + double[] intakePositions = + {spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3}; + private double prevPos = 0.0; + private double intakeTicker = 0; public Spindexer(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); @@ -115,36 +80,13 @@ public class Spindexer { ballPositions[1] = new BallPosition(); ballPositions[2] = new BallPosition(); - } + public boolean init() { + return true; + } - double[] outakePositions = - {spindexer_outtakeBall1+spindexerPosOffset, - spindexer_outtakeBall2+spindexerPosOffset, - spindexer_outtakeBall3+spindexerPosOffset}; - - double[] intakePositions = - {spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3}; - - public int counter = 0; - -// private double getTimeSeconds () -// { -// return (double) System.currentTimeMillis()/1000.0; -// } - -// public double getPos() { -// robot.spin1Pos.getVoltage(); -// robot.spin1Pos.getMaxVoltage(); -// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage()); -// } - -// public void manageSpindexer() { -// -// } - - public void resetBallPosition (int pos) { + public void resetBallPosition(int pos) { ballPositions[pos].isEmpty = true; ballPositions[pos].foundEmpty = 0; ballPositions[pos].ballColor = BallColor.UNKNOWN; @@ -153,7 +95,7 @@ public class Spindexer { distanceFrontPassenger = 61; } - public void resetSpindexer () { + public void resetSpindexer() { for (int i = 0; i < 3; i++) { resetBallPosition(i); } @@ -170,11 +112,11 @@ public class Spindexer { // Read Distances double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); - distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter); + distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1 - colorFilterAlpha) * distanceRearCenter); double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM); - distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver); + distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1 - colorFilterAlpha) * distanceFrontDriver); double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM); - distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); + distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1 - colorFilterAlpha) * distanceFrontPassenger); // Position 1 if (distanceRearCenter < 60) { @@ -196,7 +138,7 @@ public class Spindexer { } else { ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple } - } + } } // Position 2 // Find which ball position this is in the spindexer @@ -267,10 +209,11 @@ public class Spindexer { return newPos1Detection; } + // Has code to unjam spindexer private void moveSpindexerToPos(double pos) { robot.spin1.setPosition(pos); - robot.spin2.setPosition(1-pos); + robot.spin2.setPosition(1 - pos); // double currentPos = servos.getSpinPos(); // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ @@ -284,41 +227,89 @@ public class Spindexer { // prevPos = currentPos; } +// private double getTimeSeconds () +// { +// return (double) System.currentTimeMillis()/1000.0; +// } + +// public double getPos() { +// robot.spin1Pos.getVoltage(); +// robot.spin1Pos.getMaxVoltage(); +// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage()); +// } + +// public void manageSpindexer() { +// +// } + public void stopSpindexer() { } - public void ballCounterLight(){ - int counter = 0; - if (!ballPositions[0].isEmpty){ - counter++; - } - if (!ballPositions[1].isEmpty){ - counter++; - } - if (!ballPositions[2].isEmpty){ - counter++; - } - if (counter == 3){ - robot.light.setPosition(Light3); - } else if (counter == 2){ - robot.light.setPosition(Light2); - } else if (counter == 1){ - robot.light.setPosition(Light1); + public double getRearCenterLight() { + BallColor color = GetRearCenterColor(); + if (Objects.equals(color, BallColor.GREEN)) { + return LightGreen; + } else if (Objects.equals(color, BallColor.PURPLE)) { + return LightPurple; } else { - robot.light.setPosition(Light0); + return LightOrange; } } - public boolean slotIsEmpty(int slot){ + + public double getDriverLight() { + BallColor color = GetFrontDriverColor(); + if (Objects.equals(color, BallColor.GREEN)) { + return LightGreen; + } else if (Objects.equals(color, BallColor.PURPLE)) { + return LightPurple; + } else { + return LightOrange; + } + } + + public double getPassengerLight() { + BallColor color = GetFrontPassengerColor(); + if (Objects.equals(color, BallColor.GREEN)) { + return LightGreen; + } else if (Objects.equals(color, BallColor.PURPLE)) { + return LightPurple; + } else { + return LightOrange; + } + } + public double ballCounterLight() { + int counter = 0; + if (!ballPositions[0].isEmpty) { + counter++; + } + if (!ballPositions[1].isEmpty) { + counter++; + } + if (!ballPositions[2].isEmpty) { + counter++; + } + + if (counter == 3) { + return Light3; + } else if (counter == 2) { + return Light2; + } else if (counter == 1) { + return Light1; + } else { + return Light0; + } + } + + public boolean slotIsEmpty(int slot) { return !ballPositions[slot].isEmpty; } - public boolean isFull () { + public boolean isFull() { return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); } - private double intakeTicker = 0; public boolean processIntake() { switch (currentIntakeState) { @@ -340,7 +331,7 @@ public class Spindexer { } break; case UNKNOWN_DETECT: - if (unknownColorDetect >5) { + if (unknownColorDetect > 5) { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } else { //detectBalls(true, true); @@ -355,7 +346,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle); } break; case FINDNEXT: @@ -393,7 +384,7 @@ public class Spindexer { case MOVING: // Stopping when we get to the new position if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - if (intakeTicker > 1){ + if (intakeTicker > 1) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); intakeTicker = 0; @@ -416,7 +407,7 @@ public class Spindexer { } // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle); break; case SHOOT_ALL_PREP: @@ -475,7 +466,7 @@ public class Spindexer { double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { - if (commandedIntakePosition==2) { + if (commandedIntakePosition == 2) { shootWaitMax = 5; } shootWaitCount = 0; @@ -492,11 +483,11 @@ public class Spindexer { } // Keep moving the spindexer spindexerOuttakeWiggle *= -1.0; - moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); + moveSpindexerToPos(outakePositions[commandedIntakePosition] + spindexerOuttakeWiggle); break; case SHOOT_PREP_CONTINOUS: - if (servos.spinEqual(spinStartPos)){ + if (servos.spinEqual(spinStartPos)) { currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { moveSpindexerToPos(spinStartPos); @@ -507,11 +498,11 @@ public class Spindexer { ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; - if (servos.getSpinPos() > spinEndPos){ + if (servos.getSpinPos() > spinEndPos) { currentIntakeState = IntakeState.FINDNEXT; } else { double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; - if (spinPos > spinEndPos + 0.03){ + if (spinPos > spinEndPos + 0.03) { spinPos = spinEndPos + 0.03; } moveSpindexerToPos(spinPos); @@ -529,12 +520,12 @@ public class Spindexer { return false; } - public void setDesiredMotif (Types.Motif newMotif) { + public void setDesiredMotif(StateEnums.Motif newMotif) { desiredMotif = newMotif; } // Returns the best fit for the motiff - public int bestFitMotif () { + public int bestFitMotif() { switch (desiredMotif) { case GPP: if (ballPositions[0].ballColor == BallColor.GREEN) { @@ -565,36 +556,38 @@ public class Spindexer { //break; case NONE: return 0; - //break; + //break; } return 0; } - void prepareToShootMotif () { + void prepareToShootMotif() { commandedIntakePosition = bestFitMotif(); } - public void prepareShootAll(){ + public void prepareShootAll() { currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; } - public void prepareShootAllContinous(){ + + public void prepareShootAllContinous() { currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS; } - public void shootAll () { + + public void shootAll() { ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } - public void shootAllContinous(){ + + public void shootAllContinous() { ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } - public boolean shootAllComplete () - { + public boolean shootAllComplete() { return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) && (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) && (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) && @@ -604,23 +597,64 @@ public class Spindexer { (currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS)); } - void shootAllToIntake () { + void shootAllToIntake() { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } - public void update() - { + public void update() { } - public BallColor GetFrontDriverColor () { + public BallColor GetFrontDriverColor() { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor; } - public BallColor GetFrontPassengerColor () { + public BallColor GetFrontPassengerColor() { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor; } - public BallColor GetRearCenterColor () { + public BallColor GetRearCenterColor() { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; } + + // For Use + enum RotatedBallPositionNames { + REARCENTER, + FRONTDRIVER, + FRONTPASSENGER + } + + enum IntakeState { + UNKNOWN_START, + UNKNOWN_MOVE, + UNKNOWN_DETECT, + INTAKE, + FINDNEXT, + MOVING, + FULL, + SHOOTNEXT, + SHOOTMOVING, + SHOOTWAIT, + SHOOT_ALL_PREP, + SHOOT_ALL_READY, + SHOOT_PREP_CONTINOUS, + SHOOT_CONTINOUS + } + + public enum BallColor { + UNKNOWN, + GREEN, + PURPLE + } + + class spindexerBallRoatation { + int rearCenter = 0; // aka commanded Position + int frontDriver = 0; + int frontPassenger = 0; + } + + class BallPosition { + boolean isEmpty = true; + int foundEmpty = 0; + BallColor ballColor = BallColor.UNKNOWN; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 9f01c43..157ace4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -9,10 +9,10 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.teamcode.constants.Color; import java.util.List; @@ -51,6 +51,7 @@ public class Turret { private int obeliskID = 0; private double offset = 0.0; private double currentTrackOffset = 0.0; + private double lightColor = Color.LightRed; private int currentTrackCount = 0; private double permanentOffset = 0.0; private PIDController bearingPID; @@ -62,6 +63,10 @@ public class Turret { bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); } + public double getLightColor() { + return lightColor; + } + public void zeroTurretEncoder() { robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -153,11 +158,17 @@ public class Turret { // LL has 54.5 degree total Horizontal FOV; very edges are not useful. final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/- final double DRIVE_POWER_REDUCTION = 2.0; + final double COLOR_OK_TOLERANCE = 2.5; if (abs(targetTx) < TARGET_POSITION_TOLERANCE) { bearingAligned = true; - } else { + lightColor = Color.LightBlue; + } else if (abs(targetTx) < COLOR_OK_TOLERANCE) { bearingAligned = false; + lightColor = Color.LightPurple; + } else { + bearingAligned = false; + lightColor = Color.LightOrange; } // Only with valid data and if too far off target @@ -246,6 +257,7 @@ public class Turret { // if (currentTrackCount > 20) { // offset = currentTrackOffset; // } + lightColor = Color.LightRed; currentTrackOffset = 0.0; currentTrackCount = 0; }