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

Add functions to get the ball color to spindexer. Attempt to make shoot all in teleop work better.

Browse Source
This commit is contained in:
Matt9150
2026-01-29 09:25:39 -06:00
parent 53290a5982
commit 3f4fee0e24
4 changed files with 119 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java
View File

@@ -309,7 +309,6 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
return false;
}
}
};
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -11,12 +11,15 @@ public class ServoPositions {
public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24;
public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.51; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4;
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double shootAllAutoSpinStartPos = 0.2;
public static double shootAllSpindexerSpeedIncrease = 0.02;
public static double shootAllTime = 1.8;
public static double transferServo_out = 0.15;

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

@@ -31,6 +31,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -250,56 +251,56 @@ public class TeleopV3 extends LinearOpMode {
}
if (autoSpintake) {
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
if (gamepad1.right_bumper) {
shootAll = false;
intakeTicker++;
// if (intakeTicker % 20 < 2) {
// if (autoSpintake) {
//
// robot.spin1.setPower(-1);
// robot.spin2.setPower(1);
// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
//
//
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
//
// } else if (intakeTicker % 20 < 10) {
// robot.spin1.setPower(-0.5);
// robot.spin2.setPower(0.5);
// } else if (intakeTicker % 20 < 12) {
// robot.spin1.setPower(1);
// robot.spin2.setPower(-1);
// } else {
// robot.spin1.setPower(0.5);
// robot.spin2.setPower(-0.5);
// }
robot.intake.setPower(1);
intakeStamp = getRuntime();
TELE.addData("Reverse?", reverse);
TELE.update();
} else {
if (!servo.spinEqual(spindexPos)) {
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
spindexPos = spindexer_intakePos1;
robot.intake.setPower(0);
intakeTicker = 0;
}
}
//
// if (gamepad1.right_bumper) {
//
// shootAll = false;
//
// intakeTicker++;
//
//// if (intakeTicker % 20 < 2) {
////
//// robot.spin1.setPower(-1);
//// robot.spin2.setPower(1);
////
//// } else if (intakeTicker % 20 < 10) {
//// robot.spin1.setPower(-0.5);
//// robot.spin2.setPower(0.5);
//// } else if (intakeTicker % 20 < 12) {
//// robot.spin1.setPower(1);
//// robot.spin2.setPower(-1);
//// } else {
//// robot.spin1.setPower(0.5);
//// robot.spin2.setPower(-0.5);
//// }
//
// robot.intake.setPower(1);
// intakeStamp = getRuntime();
// TELE.addData("Reverse?", reverse);
// TELE.update();
// } else {
// if (!servo.spinEqual(spindexPos)) {
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
//
// }
//
// spindexPos = spindexer_intakePos1;
//
// robot.intake.setPower(0);
//
// intakeTicker = 0;
// }
// }
//COLOR:
@@ -587,7 +588,9 @@ public class TeleopV3 extends LinearOpMode {
// }
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
//}
// RIGHT_BUMPER
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
@@ -598,10 +601,7 @@ public class TeleopV3 extends LinearOpMode {
}
// LEFT_BUMPER
if (!shootAll &&
(gamepad1.leftBumperWasReleased() ||
gamepad1.leftBumperWasPressed() ||
gamepad1.left_bumper)) {
if (!shootAll && gamepad1.leftBumperWasReleased()) {
shootStamp = getRuntime();
shootAll = true;
@@ -632,11 +632,37 @@ public class TeleopV3 extends LinearOpMode {
// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
// TELE.addLine("shooting");
// }
// //robot.intake.setPower(-0.3);
// if (getRuntime() - shootStamp < 3.0) {
//
// if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) {
// robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos);
// robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos);
// } else {
// shooterTicker++;
// //robot.intake.setPower(0.0);
// robot.transferServo.setPosition(transferServo_in);
// double prevSpinPos = robot.spin1.getPosition();
// robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease);
// robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos);
// }
//
// } else {
// robot.transferServo.setPosition(transferServo_out);
// //spindexPos = spindexer_intakePos1;
// shootAll = false;
// spindexer.resetSpindexer();
// spindexer.processIntake();
//
// }
if (shooterTicker == 0) {
spindexer.prepareShootAll();
TELE.addLine("preparing to shoot");
} else if (shooterTicker == 2) {
robot.transferServo.setPosition(transferServo_in);
//robot.transferServo.setPosition(transferServo_in);
spindexer.shootAll();
TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
@@ -865,10 +891,10 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("robotInchesX", targeting.robotInchesX);
// TELE.addData( "robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate);
// TELE.addData("Targeting GridX", targeting.robotGridX);
// TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update();

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

@@ -77,7 +77,7 @@ public class Spindexer {
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
enum BallColor {
public enum BallColor {
UNKNOWN,
GREEN,
PURPLE
@@ -397,6 +397,7 @@ public class Spindexer {
case SHOOT_ALL_PREP:
// We get here with function call to prepareToShootMotif
// Stopping when we get to the new position
commandedIntakePosition = 0;
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
@@ -416,17 +417,17 @@ public class Spindexer {
case SHOOTNEXT:
// Find Next Open Position and start movement
if (!ballPositions[1].isEmpty) {
if (!ballPositions[0].isEmpty) {
// Position 1
commandedIntakePosition = 0;
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (!ballPositions[1].isEmpty) {
// Position 2
commandedIntakePosition = 1;
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (!ballPositions[2].isEmpty) {
// Position 2
commandedIntakePosition = 2;
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (!ballPositions[0].isEmpty) {
// Position 3
commandedIntakePosition = 0;
commandedIntakePosition = 2;
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else {
// Empty return to intake state
@@ -446,14 +447,18 @@ public class Spindexer {
break;
case SHOOTWAIT:
double shootWaitMax = 3;
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition==2) {
shootWaitMax = 5;
}
shootWaitCount = 0;
} else {
shootWaitCount++;
}
// wait 10 cycles
if (shootWaitCount > 2) {
if (shootWaitCount > shootWaitMax) {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
ballPositions[commandedIntakePosition].isEmpty = true;
shootWaitCount = 0;
@@ -461,7 +466,7 @@ public class Spindexer {
//detectBalls(true, false);
}
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01));
//moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.02));
break;
default:
@@ -524,7 +529,10 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void shootAll () {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
public boolean shootAllComplete ()
@@ -543,4 +551,16 @@ public class Spindexer {
public void update()
{
}
public BallColor GetFrontDriverColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
}
public BallColor GetFrontPassengerColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
}
public BallColor GetRearCenterColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
}
}