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

Sorting beta workish

Browse Source
This commit is contained in:
Keshav Anand
2026-06-04 18:47:23 -05:00
parent cca86f3691
commit 9b92a59a75
2 changed files with 193 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

141
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java Normal file
View File

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

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

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