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

Fixed spidnexer i think

Browse Source
This commit is contained in:
Keshav Anand
2026-06-02 17:08:26 -05:00
parent 1a1c99791d
commit ae25df0393
5 changed files with 98 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -6,8 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.*;
@TeleOp
@Config
@@ -18,7 +17,7 @@ public class TeleopV4 extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(), telemetry
@@ -26,8 +25,6 @@ public class TeleopV4 extends LinearOpMode {
drivetrain = new Drivetrain(robot, TELE);
drivetrain.setTelemetry(true);
waitForStart();
if (isStopRequested()) return;
@@ -42,6 +39,8 @@ public class TeleopV4 extends LinearOpMode {
gamepad1.left_stick_x
);
TELE.update();
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
View File

@@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
public class Drivetrain {

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java
View File

@@ -1,20 +0,0 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
public class Intake {
Robot rob;
public Intake(Robot robot) {
this.rob = robot;
}
public void setIntakePower(double pow){
rob.intake.setPower(pow);
}
public void setTransferPower(double pow){
rob.transfer.setPower(pow);
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java
View File

@@ -63,9 +63,9 @@ public class Robot {
public Servo turr2;
public Servo spin1;
public Servo spin2;
public TouchSensor beam1;
public TouchSensor beam2;
public TouchSensor beam3;
public TouchSensor insideBeam;
public TouchSensor outsideBeam;
public RevColorSensorV3 revSensor;
public VoltageSensor voltage;
@@ -139,9 +139,8 @@ public class Robot {
tilt1 = hardwareMap.get(Servo.class, "tilt1");
tilt2 = hardwareMap.get(Servo.class, "tilt2");
// beam1 = hardwareMap.get(TouchSensor.class, "beam1");
// beam2 = hardwareMap.get(TouchSensor.class, "beam2");
// beam3 = hardwareMap.get(TouchSensor.class, "beam3");
insideBeam = hardwareMap.get(TouchSensor.class, "beam1");
outsideBeam = hardwareMap.get(TouchSensor.class, "beam2");
revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");

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

@@ -1,161 +1,179 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
public class SpindexerTransferIntake {
private Servo spin1;
private Servo spin2;
private final Robot robot;
public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
this.spin1 = rob.spin1;
this.spin2 = rob.spin2;
this.robot = rob;
}
enum SpindexerMode {
private final double sensorDistanceThreshold = 4.0;
private final long pulseTime = 50; // ms
public enum SpindexerMode {
RAPID,
SORTED
}
enum RapidMode {
INTAKE, // Normal intake operation
TRANSFER_OFF, // Slow transfer, waiting for rings to settle
BEFORE_PULSE_OUT, // Brief forward delay before pulse
PULSE_OUT, // Small reverse pulse to unjam/reposition rings
PULSE_IN, // Feed rings back forward
HOLD_BALLS, // Maintain ring position
OPEN_GATE, // Open shooter gate
SHOOT // Feed ring into shooter
public enum RapidMode {
INTAKE,
TRANSFER_OFF,
BEFORE_PULSE_OUT,
PULSE_OUT,
PULSE_IN,
HOLD_BALLS,
OPEN_GATE,
SHOOT
}
private SpindexerMode mode = SpindexerMode.RAPID;
private RapidMode rapidMode = RapidMode.INTAKE;
public void setRapidMode (RapidMode rMode) {
this.rapidMode = rMode;
/**
* Time when current state was entered.
*/
private long stateStartTime = System.currentTimeMillis();
public void setRapidMode(RapidMode newMode) {
if (rapidMode != newMode) {
rapidMode = newMode;
stateStartTime = System.currentTimeMillis();
}
}
public void setSpindexerMode (SpindexerMode spindexerMode){
public void setSpindexerMode(SpindexerMode spindexerMode) {
this.mode = spindexerMode;
}
private long stateTime() {
return System.currentTimeMillis() - stateStartTime;
}
public void update() {
switch (mode) {
case RAPID:
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Open
);
switch (rapidMode) {
case INTAKE:
// Run front intake
// Run transfer intake
// Keep shooter gate closed
// Keep kicker retracted
// Keep spindexer in default position
robot.setIntakePower(1);
robot.setTransferPower(1);
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Closed
);
robot.setSpinPos(
ServoPositions.spindexer_A2
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
// If first beam break sees ring:
// rapidMode = TRANSFER_OFF;
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
setRapidMode(RapidMode.TRANSFER_OFF);
}
// If shoot button pressed:
// rapidMode = OPEN_GATE;
break;
case TRANSFER_OFF:
// Slow down transfer intake
// Allow rings to settle into storage
robot.setTransferPower(0.3);
// If both beam breaks occupied:
// rapidMode = BEFORE_PULSE_OUT;
// If shoot button pressed:
// rapidMode = OPEN_GATE;
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
setRapidMode(RapidMode.BEFORE_PULSE_OUT);
}
break;
case BEFORE_PULSE_OUT:
// Continue running intake forward
robot.setIntakePower(1.0);
// After ~0.3 sec:
// rapidMode = PULSE_OUT;
if (stateTime() >= 300) {
setRapidMode(RapidMode.PULSE_OUT);
}
break;
case PULSE_OUT:
// Reverse intake slightly
// Helps separate rings and prevent jams
robot.setIntakePower(-0.1);
// After pulse time:
// rapidMode = PULSE_IN;
if (stateTime() >= pulseTime) {
setRapidMode(RapidMode.PULSE_IN);
}
break;
case PULSE_IN:
// Run intake forward again
// Re-seat rings after pulse
robot.setIntakePower(1.0);
if (stateTime() >= 200) {
setRapidMode(RapidMode.HOLD_BALLS);
}
// After ~0.2 sec:
// rapidMode = HOLD_BALLS;
break;
case HOLD_BALLS:
// If both sensors occupied:
// hold intake at low power
if (robot.insideBeam.isPressed()
&& robot.outsideBeam.isPressed()) {
// Else:
// run intake forward to pull rings back in
robot.setIntakePower(0.1);
} else {
robot.setIntakePower(1);
}
// If shoot button pressed:
// rapidMode = OPEN_GATE;
// If resume intake button pressed:
// rapidMode = INTAKE;
break;
case OPEN_GATE:
// Open upper gate
// Keep intake feeding
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
// After ~0.1 sec:
// rapidMode = SHOOT;
if (stateTime() >= 100) {
setRapidMode(RapidMode.SHOOT);
}
break;
case SHOOT:
// Activate kicker
// Feed ring into shooter
// After ~0.4 sec:
// rapidMode = INTAKE;
robot.setTransferServoPos(
ServoPositions.transferServo_in
);
if (stateTime() >= 400) {
setRapidMode(RapidMode.INTAKE);
}
break;
}
break;
case SORTED:
// Future sorted-intake logic
// Color sorting
// Alliance filtering
// Different spindexer behavior
break;
}
}
}
}