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

spindex progress: not good

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-28 15:23:17 -06:00
parent f767e82e31
commit 5d0a569f82
2 changed files with 17 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -635,11 +635,12 @@ public class TeleopV3 extends LinearOpMode {
if (shooterTicker == 0) { if (shooterTicker == 0) {
shooterTicker++; shooterTicker++;
spindexer.prepareShootAll(); spindexer.prepareShootAll();
TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) { } else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
shooterTicker++; shooterTicker++;
spindexer.shootAll(); spindexer.shootAll();
TELE.addLine("starting to shoot"); TELE.addLine("shoot");
} else { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
@@ -851,11 +852,11 @@ public class TeleopV3 extends LinearOpMode {
// 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("shootall commanded", shootAll); // TELE.addData("shootall commanded", shootAll);
// // Targeting Debug // // Targeting Debug

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

@@ -258,11 +258,11 @@ public class Spindexer {
double currentPos = servos.getSpinPos(); double currentPos = servos.getSpinPos();
if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
if (currentPos > pos){ if (currentPos > pos){
robot.spin1.setPosition(1); robot.spin1.setPosition(servos.getSpinPos() + 0.05);
robot.spin2.setPosition(0); robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
} else { } else {
robot.spin1.setPosition(0); robot.spin1.setPosition(servos.getSpinPos() - 0.05);
robot.spin2.setPosition(1); robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
} }
} }
prevPos = currentPos; prevPos = currentPos;
@@ -307,7 +307,7 @@ public class Spindexer {
case UNKNOWN_START: case UNKNOWN_START:
// For now just set position ONE if UNKNOWN // For now just set position ONE if UNKNOWN
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[0]); moveSpindexerToPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break; break;
case UNKNOWN_MOVE: case UNKNOWN_MOVE:
@@ -348,7 +348,6 @@ public class Spindexer {
if (ballPositions[0].isEmpty) { if (ballPositions[0].isEmpty) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
@@ -356,14 +355,12 @@ public class Spindexer {
if (ballPositions[1].isEmpty) { if (ballPositions[1].isEmpty) {
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) { if (ballPositions[2].isEmpty) {
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
if (currentIntakeState != Spindexer.IntakeState.MOVING) { if (currentIntakeState != Spindexer.IntakeState.MOVING) {
@@ -422,17 +419,14 @@ public class Spindexer {
if (!ballPositions[0].isEmpty) { if (!ballPositions[0].isEmpty) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? } else if (!ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty?
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? } else if (!ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty?
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else { } else {
// Empty return to intake state // Empty return to intake state
@@ -452,21 +446,21 @@ public class Spindexer {
break; break;
case SHOOTWAIT: case SHOOTWAIT:
// Stopping when we get to the new position] // Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) { if (prevIntakeState != currentIntakeState) {
shootWaitCount = 0; shootWaitCount = 0;
} else { } else {
shootWaitCount++; shootWaitCount++;
} }
// wait 3 cycles // wait 3 cycles
if ((shootWaitCount > 3) && (servos.spinEqual(intakePositions[commandedIntakePosition]))) { if ((shootWaitCount > 15) && (servos.spinEqual(outakePositions[commandedIntakePosition]))) {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
ballPositions[commandedIntakePosition].isEmpty = true; ballPositions[commandedIntakePosition].isEmpty = true;
//stopSpindexer(); //stopSpindexer();
//detectBalls(true, false); //detectBalls(true, false);
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(outakePositions[commandedIntakePosition]);
} }
break; break;