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.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain; import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp @TeleOp
@Config @Config
@@ -18,7 +17,7 @@ public class TeleopV4 extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry( TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(), telemetry FtcDashboard.getInstance().getTelemetry(), telemetry
@@ -26,8 +25,6 @@ public class TeleopV4 extends LinearOpMode {
drivetrain = new Drivetrain(robot, TELE); drivetrain = new Drivetrain(robot, TELE);
drivetrain.setTelemetry(true);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
@@ -42,6 +39,8 @@ public class TeleopV4 extends LinearOpMode {
gamepad1.left_stick_x gamepad1.left_stick_x
); );
TELE.update(); 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.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
public class Drivetrain { 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 turr2;
public Servo spin1; public Servo spin1;
public Servo spin2; public Servo spin2;
public TouchSensor beam1; public TouchSensor insideBeam;
public TouchSensor beam2; public TouchSensor outsideBeam;
public TouchSensor beam3;
public RevColorSensorV3 revSensor; public RevColorSensorV3 revSensor;
public VoltageSensor voltage; public VoltageSensor voltage;
@@ -139,9 +139,8 @@ public class Robot {
tilt1 = hardwareMap.get(Servo.class, "tilt1"); tilt1 = hardwareMap.get(Servo.class, "tilt1");
tilt2 = hardwareMap.get(Servo.class, "tilt2"); tilt2 = hardwareMap.get(Servo.class, "tilt2");
// beam1 = hardwareMap.get(TouchSensor.class, "beam1"); insideBeam = hardwareMap.get(TouchSensor.class, "beam1");
// beam2 = hardwareMap.get(TouchSensor.class, "beam2"); outsideBeam = hardwareMap.get(TouchSensor.class, "beam2");
// beam3 = hardwareMap.get(TouchSensor.class, "beam3");
revSensor = hardwareMap.get(RevColorSensorV3.class, "rev"); 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; package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; 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 { public class SpindexerTransferIntake {
private Servo spin1; private final Robot robot;
private Servo spin2;
public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
this.spin1 = rob.spin1; this.robot = rob;
this.spin2 = rob.spin2;
} }
enum SpindexerMode { private final double sensorDistanceThreshold = 4.0;
private final long pulseTime = 50; // ms
public enum SpindexerMode {
RAPID, RAPID,
SORTED SORTED
} }
enum RapidMode { public enum RapidMode {
INTAKE, // Normal intake operation INTAKE,
TRANSFER_OFF, // Slow transfer, waiting for rings to settle TRANSFER_OFF,
BEFORE_PULSE_OUT, // Brief forward delay before pulse BEFORE_PULSE_OUT,
PULSE_OUT, // Small reverse pulse to unjam/reposition rings PULSE_OUT,
PULSE_IN, // Feed rings back forward PULSE_IN,
HOLD_BALLS, // Maintain ring position HOLD_BALLS,
OPEN_GATE, // Open shooter gate OPEN_GATE,
SHOOT // Feed ring into shooter SHOOT
} }
private SpindexerMode mode = SpindexerMode.RAPID; private SpindexerMode mode = SpindexerMode.RAPID;
private RapidMode rapidMode = RapidMode.INTAKE; 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; this.mode = spindexerMode;
} }
private long stateTime() {
return System.currentTimeMillis() - stateStartTime;
}
public void update() { public void update() {
switch (mode) { switch (mode) {
case RAPID: case RAPID:
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Open
);
switch (rapidMode) { switch (rapidMode) {
case INTAKE: case INTAKE:
// Run front intake robot.setIntakePower(1);
// Run transfer intake robot.setTransferPower(1);
// Keep shooter gate closed robot.setRapidFireBlockerPos(
// Keep kicker retracted ServoPositions.rapidFireBlocker_Closed
// Keep spindexer in default position );
robot.setSpinPos(
ServoPositions.spindexer_A2
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
// If first beam break sees ring: if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
// rapidMode = TRANSFER_OFF;
setRapidMode(RapidMode.TRANSFER_OFF);
}
// If shoot button pressed:
// rapidMode = OPEN_GATE;
break; break;
case TRANSFER_OFF: case TRANSFER_OFF:
// Slow down transfer intake robot.setTransferPower(0.3);
// Allow rings to settle into storage
// If both beam breaks occupied: if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
// rapidMode = BEFORE_PULSE_OUT; setRapidMode(RapidMode.BEFORE_PULSE_OUT);
}
// If shoot button pressed:
// rapidMode = OPEN_GATE;
break; break;
case BEFORE_PULSE_OUT: case BEFORE_PULSE_OUT:
// Continue running intake forward robot.setIntakePower(1.0);
// After ~0.3 sec: if (stateTime() >= 300) {
// rapidMode = PULSE_OUT; setRapidMode(RapidMode.PULSE_OUT);
}
break; break;
case PULSE_OUT: case PULSE_OUT:
// Reverse intake slightly robot.setIntakePower(-0.1);
// Helps separate rings and prevent jams
// After pulse time: if (stateTime() >= pulseTime) {
// rapidMode = PULSE_IN; setRapidMode(RapidMode.PULSE_IN);
}
break; break;
case PULSE_IN: case PULSE_IN:
// Run intake forward again robot.setIntakePower(1.0);
// Re-seat rings after pulse
if (stateTime() >= 200) {
setRapidMode(RapidMode.HOLD_BALLS);
}
// After ~0.2 sec:
// rapidMode = HOLD_BALLS;
break; break;
case HOLD_BALLS: case HOLD_BALLS:
// If both sensors occupied: if (robot.insideBeam.isPressed()
// hold intake at low power && robot.outsideBeam.isPressed()) {
// Else: robot.setIntakePower(0.1);
// run intake forward to pull rings back in
} else {
robot.setIntakePower(1);
}
// If shoot button pressed:
// rapidMode = OPEN_GATE;
// If resume intake button pressed:
// rapidMode = INTAKE;
break; break;
case OPEN_GATE: case OPEN_GATE:
// Open upper gate robot.setRapidFireBlockerPos(
// Keep intake feeding ServoPositions.rapidFireBlocker_Open
);
// After ~0.1 sec: if (stateTime() >= 100) {
// rapidMode = SHOOT; setRapidMode(RapidMode.SHOOT);
}
break; break;
case SHOOT: case SHOOT:
// Activate kicker robot.setTransferServoPos(
// Feed ring into shooter ServoPositions.transferServo_in
);
// After ~0.4 sec: if (stateTime() >= 400) {
// rapidMode = INTAKE; setRapidMode(RapidMode.INTAKE);
}
break; break;
} }
break; break;
case SORTED: case SORTED:
// Future sorted-intake logic // Future sorted-intake logic
// Color sorting
// Alliance filtering
// Different spindexer behavior
break; break;
} }
} }
} }