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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-27 23:34:22 -06:00
parent c42fce2e78
commit f9013f4d79
11 changed files with 251 additions and 316 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -180,6 +180,7 @@ public class Auto_LT_Close extends LinearOpMode {
servos.setTransferPos(transferServo_out); servos.setTransferPos(transferServo_out);
limelightUsed = false; limelightUsed = false;
Spindexer.teleop = false;
robot.light.setPosition(1); robot.light.setPosition(1);

268
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -1,6 +1,11 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAY;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBX;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBY;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
@@ -12,9 +17,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos;
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_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -60,19 +62,14 @@ public class Auto_LT_Far extends LinearOpMode {
AutoActions autoActions; AutoActions autoActions;
Light light; Light light;
double xShoot, yShoot, hShoot; double xShoot, yShoot, hShoot;
double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0; double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0;
double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0;
public static double flywheel0Time = 1.5; public static double flywheel0Time = 1.5;
boolean gatePickup = true; boolean gatePickup = true;
boolean stack3 = true; boolean stack3 = true;
boolean stack2 = true; double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA; double xStackPickupB, yStackPickupB, hStackPickupB;
double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB; public static int pickupStackSpeed = 12;
double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA; public static int pickupGateSpeed = 30;
double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB;
public static int pickupStackSpeed = 17;
public static int pickupGateSpeed = 25;
int prevMotif = 0; int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.014; public static double spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 2; public static double shootAllTime = 2;
@@ -82,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode {
public static double shootStackTime = 2; public static double shootStackTime = 2;
public static double shootGateTime = 2.5; public static double shootGateTime = 2.5;
public static double colorSenseTime = 1; public static double colorSenseTime = 1;
public static double intakeStackTime = 4.5; public static double intakeStackTime = 2.5;
public static double intakeGateTime = 8; public static double intakeGateTime = 2;
double obeliskTurrPos1 = 0.0; double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0; double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0; double obeliskTurrPos3 = 0.0;
@@ -96,7 +93,6 @@ public class Auto_LT_Far extends LinearOpMode {
TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder shoot3 = null;
TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder pickupGate = null;
TrajectoryActionBuilder shootGate = null; TrajectoryActionBuilder shootGate = null;
TrajectoryActionBuilder pickup2 = null;
Pose2d autoStart = new Pose2d(0,0,0); Pose2d autoStart = new Pose2d(0,0,0);
@Override @Override
@@ -127,46 +123,16 @@ public class Auto_LT_Far extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault); Spindexer.teleop = false;
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
limelightUsed = false;
while (opModeInInit()) { while (opModeInInit()) {
if (limelightUsed){
Actions.runBlocking(
autoActions.detectObelisk(
0.1,
0.501,
0.501,
0.501,
0.501,
turrDefault
)
);
motif = turret.getObeliskID();
if (motif == 21){ if (gamepad2.leftBumperWasPressed()){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall1;
} else if (motif == 22){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b;
} else {
AutoActions.firstSpindexShootPos = spindexer_outtakeBall2;
}
}
if (gamepad2.triangleWasPressed()){
gatePickup = !gatePickup; gatePickup = !gatePickup;
} }
if (gamepad2.rightBumperWasPressed()){ if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3; stack3 = !stack3;
} }
if (gamepad2.leftBumperWasPressed()){
stack2 = !stack2;
}
turret.setTurret(turretShootPos); turret.setTurret(turretShootPos);
@@ -199,33 +165,17 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = rShootY; yShoot = rShootY;
hShoot = rShootH; hShoot = rShootH;
xStackPickupFarA = rStackPickupAX; xStackPickupA = rStackPickupFarAX;
yStackPickupFarA = rStackPickupAY; yStackPickupA = rStackPickupFarAY;
hStackPickupFarA = rStackPickupAH; hStackPickupA = rStackPickupFarAH;
xStackPickupFarB = rStackPickupBX; xStackPickupB = rStackPickupFarBX;
yStackPickupFarB = rStackPickupBY; yStackPickupB = rStackPickupFarBY;
hStackPickupFarB = rStackPickupBH; hStackPickupB = rStackPickupFarBH;
xStackPickupMiddleA = rStackPickupAX; pickupGateX = rPickupGateXA;
yStackPickupMiddleA = rStackPickupAY; pickupGateY = rPickupGateYA;
hStackPickupMiddleA = rStackPickupAH; pickupGateH = rPickupGateHA;
xStackPickupMiddleB = rStackPickupBX;
yStackPickupMiddleB = rStackPickupBY;
hStackPickupMiddleB = rStackPickupBH;
pickupGateXA = rPickupGateXA;
pickupGateYA = rPickupGateYA;
pickupGateHA = rPickupGateHA;
pickupGateXB = rPickupGateXB;
pickupGateYB = rPickupGateYB;
pickupGateHB = rPickupGateHB;
pickupGateXC = rPickupGateXC;
pickupGateYC = rPickupGateYC;
pickupGateHC = rPickupGateHC;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -233,10 +183,15 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + redTurretShootPos; turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){ if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(1); turret.pipelineSwitch(4);
robot.limelight.start(); robot.limelight.start();
limelightUsed = true;
gamepad2.rumble(500); gamepad2.rumble(500);
sleep(1000);
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
} }
} else { } else {
robot.light.setPosition(0.6); robot.light.setPosition(0.6);
@@ -253,33 +208,17 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = bShootY; yShoot = bShootY;
hShoot = bShootH; hShoot = bShootH;
xStackPickupFarA = bStackPickupAX; xStackPickupA = bStackPickupFarAX;
yStackPickupFarA = bStackPickupAY; yStackPickupA = bStackPickupFarAY;
hStackPickupFarA = bStackPickupAH; hStackPickupA = bStackPickupFarAH;
xStackPickupFarB = bStackPickupBX; xStackPickupB = bStackPickupFarBX;
yStackPickupFarB = bStackPickupBY; yStackPickupB = bStackPickupFarBY;
hStackPickupFarB = bStackPickupBH; hStackPickupB = bStackPickupFarBH;
xStackPickupMiddleA = bStackPickupAX; pickupGateX = bPickupGateXA;
yStackPickupMiddleA = bStackPickupAY; pickupGateY = bPickupGateYA;
hStackPickupMiddleA = bStackPickupAH; pickupGateH = bPickupGateHA;
xStackPickupMiddleB = bStackPickupBX;
yStackPickupMiddleB = bStackPickupBY;
hStackPickupMiddleB = bStackPickupBH;
pickupGateXA = bPickupGateXA;
pickupGateYA = bPickupGateYA;
pickupGateHA = bPickupGateHA;
pickupGateXB = bPickupGateXB;
pickupGateYB = bPickupGateYB;
pickupGateHB = bPickupGateHB;
pickupGateXC = bPickupGateXC;
pickupGateYC = bPickupGateYC;
pickupGateHC = bPickupGateHC;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -287,10 +226,15 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + blueTurretShootPos; turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){ if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(5); turret.pipelineSwitch(2);
robot.limelight.start(); robot.limelight.start();
limelightUsed = true;
gamepad2.rumble(500); gamepad2.rumble(500);
sleep(1000);
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
} }
} }
@@ -301,33 +245,26 @@ public class Auto_LT_Far extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA)) .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB), .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
new TranslationalVelConstraint(pickupStackSpeed)); new TranslationalVelConstraint(pickupStackSpeed));
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA))
.strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB),
new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA)) .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH),
.waitSeconds(0.2)
.strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
.strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
new TranslationalVelConstraint(pickupGateSpeed)); new TranslationalVelConstraint(pickupGateSpeed));
shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC))) shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
limelightUsed = true;
TELE.addData("Red?", redAlliance); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault); TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup); TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack Far?", stack3); TELE.addData("Pickup Stack?", stack3);
TELE.addData("Pickup Stack Middle?", stack2);
TELE.addData("Start Position", autoStart); TELE.addData("Start Position", autoStart);
TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update(); TELE.update();
@@ -351,11 +288,6 @@ public class Auto_LT_Far extends LinearOpMode {
shoot(); shoot();
} }
if (stack2){
cycleStackMiddle();
shoot();
}
while (gatePickup && getRuntime() - stamp < endAutoTime){ while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickupBalls(); cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){ if (getRuntime() - stamp > endAutoTime){
@@ -432,79 +364,16 @@ public class Auto_LT_Far extends LinearOpMode {
pickup3.build(), pickup3.build(),
autoActions.intake( autoActions.intake(
intakeStackTime, intakeStackTime,
xStackPickupFarB, xStackPickupB,
yStackPickupFarB, yStackPickupB,
hStackPickupFarB hStackPickupB
),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupFarB,
yStackPickupFarB,
posXTolerance,
posYTolerance,
obeliskTurrPos3
) )
) )
); );
servos.setSpinPos(spinStartPos);
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot3.build(), shoot3.build()
autoActions.prepareShootAll(
colorSenseTime,
shootStackTime,
motif,
xShoot,
yShoot,
hShoot)
)
);
}
void cycleStackMiddle(){
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
autoActions.intake(
intakeStackTime,
xStackPickupMiddleB,
yStackPickupMiddleB,
hStackPickupMiddleB
),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupMiddleB,
yStackPickupMiddleB,
posXTolerance,
posYTolerance,
obeliskTurrPos2
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootStackTime,
motif,
xShoot,
yShoot,
hShoot)
) )
); );
} }
@@ -515,25 +384,12 @@ public class Auto_LT_Far extends LinearOpMode {
pickupGate.build(), pickupGate.build(),
autoActions.intake( autoActions.intake(
intakeStackTime, intakeStackTime,
pickupGateXC, pickupGateX,
pickupGateYC, pickupGateY,
pickupGateHC pickupGateH
),
autoActions.detectObelisk(
intakeGateTime,
pickupGateXC,
pickupGateYC,
posXTolerance,
posYTolerance,
obeliskTurrPos3
) )
) )
); );
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
} }
void cycleGatePrepareShoot(){ void cycleGatePrepareShoot(){

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
View File

@@ -55,7 +55,7 @@ public class AutoActions {
private boolean shootForward = true; private boolean shootForward = true;
public int motif = 0; public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos; double spinEndPos = ServoPositions.spinEndPos;
private boolean intaking = false;
public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) {
this.robot = rob; this.robot = rob;
this.drive = dri; this.drive = dri;
@@ -427,8 +427,10 @@ public class AutoActions {
if ((System.currentTimeMillis() - stamp) > (time * 1000)) { if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
servos.setSpinPos(spindexer_intakePos1); servos.setSpinPos(spindexer_intakePos1);
intaking = false;
return false; return false;
} else { } else {
intaking = true;
return true; return true;
} }
} }
@@ -672,6 +674,44 @@ public class AutoActions {
} }
}; };
} }
public Action ShakeDrivetrain(
double time
){
return new Action() {
int ticker = 0;
double stamp = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0){
stamp = System.currentTimeMillis();
}
ticker++;
double currentStamp = System.currentTimeMillis();
if (currentStamp - stamp < time*1000 && (intaking || ticker < 50)) {
if (ticker % 10000 < 5000) {
robot.frontLeft.setPower(0.5);
robot.backLeft.setPower(0.5);
robot.frontRight.setPower(0.5);
robot.backRight.setPower(0.5);
} else {
robot.frontLeft.setPower(-0.5);
robot.backLeft.setPower(-0.5);
robot.frontRight.setPower(-0.5);
robot.backRight.setPower(-0.5);
}
return true;
} else {
robot.frontLeft.setPower(0);
robot.backLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backRight.setPower(0);
return false;
}
}
};
}
} }

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java
View File

@@ -7,23 +7,29 @@ public class Back_Poses {
public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1; public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
public static double rShootX = 100, rShootY = 55, rShootH = 90; public static double rShootX = 100, rShootY = 60, rShootH = 145.2;
public static double bShootX = 100, bShootY = -55, bShootH = -90; public static double bShootX = 100, bShootY = -55, bShootH = -145.2;
public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140; public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145;
public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1;
public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1;
public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140; public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145;
public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140; public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145;
public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140;
public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140; public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1;
public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1;
public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145;
public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -145;
public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145;
public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145;
public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190;
public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54;
public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -54;
} }

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

@@ -19,7 +19,7 @@ public class ServoPositions {
public static double spinStartPos = 0.10; public static double spinStartPos = 0.10;
public static double spinEndPos = 0.95; public static double spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.014; public static double shootAllSpindexerSpeedIncrease = 0.013;
public static double transferServo_out = 0.15; public static double transferServo_out = 0.15;

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

@@ -39,7 +39,7 @@ public class TeleopV3 extends LinearOpMode {
public static double spinPow = 0.09; public static double spinPow = 0.09;
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static double spinSpeedIncrease = 0.03; public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4; public static int resetSpinTicks = 0;
public static double hoodSpeedOffset = 0.01; public static double hoodSpeedOffset = 0.01;
public static double turretSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01;
public double vel = 3000; public double vel = 3000;
@@ -76,7 +76,7 @@ public class TeleopV3 extends LinearOpMode {
int intakeTicker = 0; int intakeTicker = 0;
private boolean shootAll = false; private boolean shootAll = false;
boolean relocalize = false; public static boolean relocalize = false;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -112,6 +112,7 @@ public class TeleopV3 extends LinearOpMode {
light.setState(StateEnums.LightState.MANUAL); light.setState(StateEnums.LightState.MANUAL);
limelightUsed = true; limelightUsed = true;
Spindexer.teleop = true;
while (opModeInInit()) { while (opModeInInit()) {
robot.limelight.start(); robot.limelight.start();
@@ -154,13 +155,13 @@ public class TeleopV3 extends LinearOpMode {
shootAll = false; shootAll = false;
servo.setTransferPos(transferServo_out); servo.setTransferPos(transferServo_out);
light.setState(StateEnums.LightState.BALL_COUNT); //light.setState(StateEnums.LightState.BALL_COUNT);
} else if (gamepad2.triangle){ } else if (gamepad2.triangle){
light.setState(StateEnums.LightState.BALL_COLOR); //light.setState(StateEnums.LightState.BALL_COLOR);
} else { } else {
light.setState(StateEnums.LightState.GOAL_LOCK); //light.setState(StateEnums.LightState.GOAL_LOCK);
} }
//TURRET TRACKING //TURRET TRACKING
@@ -354,14 +355,14 @@ public class TeleopV3 extends LinearOpMode {
// //
// TELE.addData("shootall commanded", shootAll); // TELE.addData("shootall commanded", shootAll);
// Targeting Debug // Targeting Debug
TELE.addData("robotX", robotX); // TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY); // TELE.addData("robotY", robotY);
TELE.addData("robot H", robotHeading); // TELE.addData("robot H", robotHeading);
// TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("robotInchesX", targeting.robotInchesX);
// TELE.addData("robotInchesY", targeting.robotInchesY); // TELE.addData("robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate); // TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX); // TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
@@ -369,13 +370,13 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); // TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); // TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); // TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
TELE.update(); TELE.update();
light.update(); //light.update();
ticker++; ticker++;
} }

85
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java
View File

@@ -65,46 +65,51 @@ public class SortingTest extends LinearOpMode {
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()){ if (opModeIsActive()){
spindexer.setIntakePower(1); Actions.runBlocking(
robot.transfer.setPower(1); autoActions.ShakeDrivetrain(
100
if (gamepad1.crossWasPressed()){ )
motif = 21; );
} else if (gamepad1.squareWasPressed()){ // spindexer.setIntakePower(1);
motif = 22; // robot.transfer.setPower(1);
} else if (gamepad1.triangleWasPressed()){ //
motif = 23; // if (gamepad1.crossWasPressed()){
} // motif = 21;
flywheel.manageFlywheel(2500); // } else if (gamepad1.squareWasPressed()){
// motif = 22;
if (gamepad1.leftBumperWasPressed()){ // } else if (gamepad1.triangleWasPressed()){
intaking = false; // motif = 23;
Actions.runBlocking( // }
autoActions.prepareShootAll( // flywheel.manageFlywheel(2500);
3, //
5, // if (gamepad1.leftBumperWasPressed()){
motif, // intaking = false;
0.501, // Actions.runBlocking(
0.501, // autoActions.prepareShootAll(
0.501 // 3,
) // 5,
); // motif,
} else if (gamepad1.rightBumperWasPressed()){ // 0.501,
intaking = false; // 0.501,
Actions.runBlocking( // 0.501
autoActions.shootAllAuto( // )
3.5, // );
0.014, // } else if (gamepad1.rightBumperWasPressed()){
0.501, // intaking = false;
0.501, // Actions.runBlocking(
0.501 // autoActions.shootAllAuto(
) // 3.5,
); // 0.014,
intaking = true; // 0.501,
} else if (intaking){ // 0.501,
spindexer.processIntake(); // 0.501
} // )
// );
// intaking = true;
// } else if (intaking){
// spindexer.processIntake();
// }
} }
} }
} }

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java
View File

@@ -31,18 +31,40 @@ public class Drivetrain {
} }
private double prevY = 0;
private double prevX = 0;
private double prevRX = 0;
private double prevBrake = 0;
public void drive(double y, double x, double rx, double brake){ public void drive(double y, double x, double rx, double brake){
int countConstant = 0;
boolean brakeChange = false;
if (Math.abs(prevY - y) > 0.05){
prevY = y;
countConstant++;
}
if (Math.abs(prevX - x) > 0.05){
prevX = x;
countConstant++;
}
if (Math.abs(prevRX - rx) > 0.05){
prevRX = rx;
countConstant++;
}
if (Math.abs(prevBrake - brake) > 0.05){
prevBrake = brake;
brakeChange = true;
}
if (!autoDrive) { if (!autoDrive && countConstant > 0) {
x = x* 1.1; // Counteract imperfect strafing x = x* 1.1; // Counteract imperfect strafing
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1);
double frontLeftPower = (y + x + rx) / denominator; double frontLeftPower = (prevY + prevX + prevRX) / denominator;
double backLeftPower = (y - x + rx) / denominator; double backLeftPower = (prevY - prevX + prevRX) / denominator;
double frontRightPower = (y - x - rx) / denominator; double frontRightPower = (prevY - prevX - prevRX) / denominator;
double backRightPower = (y + x - rx) / denominator; double backRightPower = (prevY + prevX - prevRX) / denominator;
robot.frontLeft.setPower(frontLeftPower); robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower); robot.backLeft.setPower(backLeftPower);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java
View File

@@ -64,17 +64,17 @@ public final class Light {
break; break;
case BALL_COLOR: case BALL_COLOR:
double currentTime = System.currentTimeMillis();
if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
lightColor = spindexer.getRearCenterLight(); lightColor = spindexer.getRearCenterLight();
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) { } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) {
lightColor = 0; lightColor = 0;
} else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { } else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
lightColor = spindexer.getDriverLight(); lightColor = spindexer.getDriverLight();
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { } else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
lightColor = 0; lightColor = 0;
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
lightColor = spindexer.getPassengerLight(); lightColor = spindexer.getPassengerLight();
} else { } else {

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

@@ -173,10 +173,23 @@ public class Spindexer {
// Detects if a ball is found and what color. // Detects if a ball is found and what color.
// Returns true is there was a new ball found in Position 1 // Returns true is there was a new ball found in Position 1
// FIXIT: Reduce number of times that we read the color sensors for loop times. // FIXIT: Reduce number of times that we read the color sensors for loop times.
public static boolean teleop = false;
public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) {
boolean newPos1Detection = false; boolean newPos1Detection = false;
int spindexerBallPos = 0; int spindexerBallPos = 0;
double rearDistance;
double frontDriverDistance;
double frontPassengerDistance;
if (teleop){
rearDistance = 48;
frontDriverDistance = 50;
frontPassengerDistance = 29;
} else {
rearDistance = 48;
frontDriverDistance = 56;
frontPassengerDistance = 29;
}
// Read Distances // Read Distances
double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
@@ -187,7 +200,7 @@ public class Spindexer {
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1 // Position 1
if (distanceRearCenter < 48) { if (distanceRearCenter < rearDistance) {
// Mark Ball Found // Mark Ball Found
newPos1Detection = true; newPos1Detection = true;
@@ -209,7 +222,7 @@ public class Spindexer {
// Position 2 // Position 2
// Find which ball position this is in the spindexer // Find which ball position this is in the spindexer
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
if (distanceFrontDriver < 50) { if (distanceFrontDriver < frontDriverDistance) {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) { if (detectFrontColor) {
@@ -235,7 +248,7 @@ public class Spindexer {
// Position 3 // Position 3
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
if (distanceFrontPassenger < 29) { if (distanceFrontPassenger < frontPassengerDistance) {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
@@ -512,6 +525,7 @@ public class Spindexer {
break; break;
case SHOOTWAIT: case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position // Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) { if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition==2) { if (commandedIntakePosition==2) {
@@ -535,33 +549,27 @@ public class Spindexer {
break; break;
case SHOOT_PREP_CONTINOUS: case SHOOT_PREP_CONTINOUS:
if (shootTicks > waitFirstBallTicks){ if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
shootTicks++;
} else if (servos.spinEqual(spinStartPos)){
shootTicks++;
servos.setTransferPos(transferServo_in);
} else { } else {
servos.setSpinPos(spinStartPos); servos.setSpinPos(spinStartPos);
} }
break; break;
case SHOOT_CONTINOUS: case SHOOT_CONTINOUS:
whileShooting = true; servos.setTransferPos(transferServo_in);
ballPositions[0].isEmpty = false; ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false; ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false; ballPositions[2].isEmpty = false;
if (servos.getSpinPos() > spinEndPos){ if (servos.getSpinPos() > spinEndPos){
whileShooting = false;
servos.setTransferPos(transferServo_out);
shootTicks = 0;
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
servos.setTransferPos(transferServo_out);
} else { } else {
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){ if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03; spinPos = spinEndPos + 0.03;
} }
servos.setSpinPos(spinPos); moveSpindexerToPos(spinPos);
} }
break; break;

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -13,6 +13,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.teleop.TeleopV3;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
@@ -79,7 +80,7 @@ public class Turret {
} }
public double getTurrPos() { public double getTurrPos() {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; return robot.turr1.getPosition();
} }
private double prevTurrPos = 0; private double prevTurrPos = 0;
@@ -114,23 +115,18 @@ public class Turret {
if (result.isValid()) { if (result.isValid()) {
tx = result.getTx(); tx = result.getTx();
ty = result.getTy(); ty = result.getTy();
// MegaTag1 code for receiving position if (TeleopV3.relocalize){
Pose3D botpose = result.getBotpose(); List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
if (botpose != null) { for (LLResultTypes.FiducialResult fiducial : fiducials) {
limelightPosX = botpose.getPosition().x; limelightTagPose = fiducial.getRobotPoseTargetSpace();
limelightPosY = botpose.getPosition().y; if (limelightTagPose != null){
} xPos = limelightTagPose.getPosition().x;
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults(); yPos = limelightTagPose.getPosition().y;
for (LLResultTypes.FiducialResult fiducial : fiducials) { zPos = limelightTagPose.getPosition().z;
limelightTagPose = fiducial.getRobotPoseTargetSpace(); hPos = limelightTagPose.getOrientation().getYaw();
if (limelightTagPose != null){ }
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
} }
} }
} }
} }
if (xPos != null){ if (xPos != null){