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

Working Spindexer prototype with original shoot all functionality.

Browse Source
This commit is contained in:
Matt9150
2026-01-19 11:11:22 -06:00
parent 59796154bd
commit cfd09df8a0
3 changed files with 338 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -7,9 +7,9 @@ public class ServoPositions {
public static double spindexer_intakePos1 = 0.39;
public static double spindexer_intakePos2 = 0.5;
public static double spindexer_intakePos2 = 0.55;//0.5;
public static double spindexer_intakePos3 = 0.66;
public static double spindexer_intakePos3 = 0.71;//0.66;
public static double spindexer_outtakeBall3 = 0.47;

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

@@ -32,6 +32,7 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import java.util.ArrayList;
import java.util.List;
@@ -70,6 +71,7 @@ public class TeleopV3 extends LinearOpMode {
Servos servo;
Flywheel flywheel;
MecanumDrive drive;
Spindexer spindexer;
double autoHoodOffset = 0.0;
int shooterTicker = 0;
@@ -94,7 +96,8 @@ public class TeleopV3 extends LinearOpMode {
boolean shootA = true;
boolean shootB = true;
boolean shootC = true;
boolean autoSpintake = true;
boolean autoSpintake = false;
boolean enableSpindexerManager = true;
List<Integer> shootOrder = new ArrayList<>();
boolean outtake1 = false;
boolean outtake2 = false;
@@ -142,6 +145,7 @@ public class TeleopV3 extends LinearOpMode {
servo = new Servos(hardwareMap);
flywheel = new Flywheel(hardwareMap);
drive = new MecanumDrive(hardwareMap, teleStart);
spindexer = new Spindexer(hardwareMap);
PIDFController tController = new PIDFController(tp, ti, td, tf);
@@ -529,7 +533,7 @@ public class TeleopV3 extends LinearOpMode {
// }
// }
if (gamepad1.left_bumper) {
if (gamepad1.left_bumper && !enableSpindexerManager) {
robot.transferServo.setPosition(transferServo_out);
@@ -560,14 +564,14 @@ public class TeleopV3 extends LinearOpMode {
}
if (gamepad1.leftBumperWasReleased()) {
if (gamepad1.leftBumperWasReleased() && !enableSpindexerManager) {
shootStamp = getRuntime();
shootAll = true;
shooterTicker = 0;
}
if (shootAll) {
if (shootAll && !enableSpindexerManager) {
TELE.addData("100% works", shootOrder);
@@ -765,7 +769,57 @@ public class TeleopV3 extends LinearOpMode {
// }
//EXTRA STUFFINESS:
if (enableSpindexerManager) {
if (!shootAll) {
spindexer.processIntake();
}
// RIGHT_BUMPER
if (gamepad1.right_bumper) {
robot.intake.setPower(1);
} else {
robot.intake.setPower(0);
}
// LEFT_BUMPER
if (gamepad1.leftBumperWasReleased()) {
shootStamp = getRuntime();
shootAll = true;
shooterTicker = 0;
}
if (shootAll) {
TELE.addData("100% works", shootOrder);
intake = false;
reject = false;
shooterTicker++;
spindexPos = spindexer_intakePos1;
if (getRuntime() - shootStamp < 3.5) {
robot.transferServo.setPosition(transferServo_in);
robot.spin1.setPower(-spinPow);
robot.spin2.setPower(spinPow);
} else {
robot.transferServo.setPosition(transferServo_out);
spindexPos = spindexer_intakePos1;
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
spindexer.resetSpindexer();
}
}
}
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
@@ -786,9 +840,11 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("oddColor", oddBallColor);
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update();
ticker++;

275
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java Normal file
View File

@@ -0,0 +1,275 @@
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.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
public class Spindexer {
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;
enum IntakeState {
UNKNOWN,
INTAKE,
FINDNEXT,
MOVING,
FULL
};
public IntakeState currentIntakeState = IntakeState.UNKNOWN;
enum BallColor {
UNKNOWN,
GREEN,
PURPLE
};
class BallPosition {
boolean isEmpty = true;
BallColor ballColor = BallColor.UNKNOWN;
}
BallPosition[] ballPositions = new BallPosition[3];
public boolean init () {
return true;
}
public Spindexer(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
servos = new Servos(hardwareMap);
flywheel = new Flywheel(hardwareMap);
//TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
lastKnownSpinPos = servos.getSpinPos();
ballPositions[0] = new BallPosition();
ballPositions[1] = new BallPosition();
ballPositions[2] = new BallPosition();
}
double[] outakePositions =
{spindexer_outtakeBall1, spindexer_outtakeBall2, spindexer_outtakeBall3};
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 resetSpindexer () {
for (int i = 0; i < 3; i++) {
ballPositions[i].isEmpty = true;
ballPositions[i].ballColor = BallColor.UNKNOWN;
}
currentIntakeState = IntakeState.UNKNOWN;
}
// Detects if a ball is found and what color.
// Returns true is there was a new ball found in Position 1
public boolean detectBalls() {
boolean newPos1Detection = false;
// Read Distances
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
// Position 1
if (s1D < 43) {
// Mark Ball Found
newPos1Detection = ballPositions[commandedIntakePosition].isEmpty;
// Detect which color
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.4) {
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
} else {
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple
}
}
// FIXIT - Need to find Position base on rotation
// // Position 2
// if (s2D < 60) {
//
// double green = robot.color2.getNormalizedColors().green;
// double red = robot.color2.getNormalizedColors().red;
// double blue = robot.color2.getNormalizedColors().blue;
//
// double gP = green / (green + red + blue);
//
// if (gP >= 0.4) {
// b2 = 2; // purple
// } else {
// b2 = 1; // green
// }
// }
//
// // Position 3
// if (s3D < 33) {
//
// double green = robot.color3.getNormalizedColors().green;
// double red = robot.color3.getNormalizedColors().red;
// double blue = robot.color3.getNormalizedColors().blue;
//
// double gP = green / (green + red + blue);
//
// if (gP >= 0.4) {
// b3 = 2; // purple
// } else {
// b3 = 1; // green
// }
// }
// TELE.addData("Velocity", velo);
// TELE.addLine("Detecting");
// TELE.addData("Distance 1", s1D);
// TELE.addData("Distance 2", s2D);
// TELE.addData("Distance 3", s3D);
// TELE.addData("B1", b1);
// TELE.addData("B2", b2);
// TELE.addData("B3", b3);
// TELE.update();
return newPos1Detection;
}
public void moveSpindexerToPos(double pos) {
spinCurrentPos = servos.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, pos);
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
}
public void stopSpindexer() {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
}
public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
}
public boolean processIntake() {
switch (currentIntakeState) {
case UNKNOWN:
// For now just set position ONE if UNKNOWN
commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.MOVING;
break;
case INTAKE:
// Ready for intake and Detecting a New Ball
if (detectBalls()) {
ballPositions[commandedIntakePosition].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} else {
// Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
}
break;
case FINDNEXT:
// Find Next Open Position and start movement
if (ballPositions[0].isEmpty) {
// Position 1
commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
} else if (ballPositions[1].isEmpty) {
// Position 2
commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
} else if (ballPositions[2].isEmpty) {
// Position 3
commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
} else {
// Full
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
break;
case MOVING:
// Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
detectBalls();
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
}
break;
case FULL:
// Double Check Colors?
// Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
default:
// Statements to execute if no case matches
}
//TELE.addData("commandedIntakePosition", commandedIntakePosition);
//TELE.update();
// Signal a successful intake
return false;
}
public void update()
{
}
}