KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Lights work

Browse Source
This commit is contained in:
Keshav Anand
2026-02-10 20:44:12 -06:00
parent 127995c5fe
commit fab0d6346d
6 changed files with 349 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java
View File

@@ -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;
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/StateEnums.java
View File

@@ -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
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -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);
}
limelightUsed= true;
light.update();
}
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++;
}
}

108
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java Normal file
View File

@@ -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;
}
}

334
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -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) {
@@ -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) {
@@ -570,31 +561,33 @@ public class Spindexer {
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;
}
}

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -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;
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;
}