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

@Matt please take a look at this code

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-27 18:38:41 -06:00
parent 1715fa96cb
commit 2fd87c9093
3 changed files with 65 additions and 57 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

@@ -11,11 +11,11 @@ public class ServoPositions {
public static double spindexer_intakePos3 = 0.53;//0.66; public static double spindexer_intakePos3 = 0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.8; public static double spindexer_outtakeBall3 = 0.24;
public static double spindexer_outtakeBall2 = 0.6; public static double spindexer_outtakeBall2 = 0.6;
public static double spindexer_outtakeBall1 = 0.4; public static double spindexer_outtakeBall1 = 0.4;
public static double spinStartPos = spindexer_outtakeBall1 - 0.08; public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double transferServo_out = 0.15; public static double transferServo_out = 0.15;

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

@@ -3,6 +3,9 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; 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_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -123,8 +126,8 @@ public class TeleopV3 extends LinearOpMode {
private int tickerA = 1; private int tickerA = 1;
private boolean transferIn = false; private boolean transferIn = false;
boolean turretInterpolate = false; boolean turretInterpolate = false;
public static double spinSpeedIncrease = 0.04; public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 20; public static int resetSpinTicks = 4;
public static double velPrediction(double distance) { public static double velPrediction(double distance) {
if (distance < 30) { if (distance < 30) {
@@ -584,20 +587,18 @@ public class TeleopV3 extends LinearOpMode {
// } // }
if (enableSpindexerManager) { if (enableSpindexerManager) {
// Gives some time to reset spindexer // RIGHT_BUMPER
if (!shootAll && intakeTicker > resetSpinTicks) { if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1);
spindexer.processIntake(); spindexer.processIntake();
} else {
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1-spindexer_intakePos1);
}
// RIGHT_BUMPER
if (gamepad1.right_bumper) {
robot.intake.setPower(1);
} else { } else {
robot.intake.setPower(0); robot.intake.setPower(0);
if (spindexer.isFull() && intakeTicker > resetSpinTicks*5){
robot.spin1.setPosition(spinStartPos);
robot.spin2.setPosition(1-spinStartPos);
} else {
spindexer.processIntake();
}
} }
// LEFT_BUMPER // LEFT_BUMPER
@@ -609,7 +610,6 @@ public class TeleopV3 extends LinearOpMode {
shootAll = true; shootAll = true;
shooterTicker = 0; shooterTicker = 0;
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
} }
intakeTicker++; intakeTicker++;
@@ -618,18 +618,23 @@ public class TeleopV3 extends LinearOpMode {
intake = false; intake = false;
reject = false; reject = false;
if (getRuntime() - shootStamp < 3.5 && servo.getSpinPos() < spindexer_outtakeBall3 + 0.1) { if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4 || shooterTicker == 0) {
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ if (shooterTicker == 0){
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexPos); robot.spin1.setPosition(spinStartPos);
robot.spin2.setPosition(1-spindexPos); robot.spin2.setPosition(1-spinStartPos);
if (servo.spinEqual(spinStartPos)){
shooterTicker++;
}
TELE.addLine("starting to shoot");
} else { } else {
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = servo.getSpinPos();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
TELE.addLine("shooting");
} }
} else { } else {
@@ -640,6 +645,7 @@ public class TeleopV3 extends LinearOpMode {
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
TELE.addLine("stop shooting");
} }
} }
@@ -825,42 +831,40 @@ public class TeleopV3 extends LinearOpMode {
hub.clearBulkCache(); hub.clearBulkCache();
} }
// //
TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); // TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
//
TELE.addData("pose", drive.localizer.getPose()); // TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); // TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal); // TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition()); // TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel); // TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo()); // TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velo1", flywheel.velo1); // TELE.addData("Velo1", flywheel.velo1);
TELE.addData("Velo2", flywheel.velo2); // TELE.addData("Velo2", flywheel.velo2);
TELE.addData("shootOrder", shootOrder); // TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); // TELE.addData("oddColor", oddBallColor);
//
// Spindexer Debug // // Spindexer Debug
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); // TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); // TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState); // TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake); // TELE.addData("autoSpintake", autoSpintake);
//TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); //
//TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); // TELE.addData("shootall commanded", shootAll);
//TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); // // Targeting Debug
TELE.addData("shootall commanded", shootAll); // TELE.addData("robotX", robotX);
// Targeting Debug // TELE.addData("robotY", robotY);
TELE.addData("robotX", robotX); // TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData("robotY", robotY); // TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData( "robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting Interpolate", turretInterpolate); // TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting GridX", targeting.robotGridX); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update(); TELE.update();

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

@@ -291,6 +291,10 @@ public class Spindexer {
} }
} }
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); return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
} }