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);
limelightUsed = false;
Spindexer.teleop = false;
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;
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.Front_Poses.teleStart;
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.redTurretShootPos;
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.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -60,19 +62,14 @@ public class Auto_LT_Far extends LinearOpMode {
AutoActions autoActions;
Light light;
double xShoot, yShoot, hShoot;
double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0;
double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0;
double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0;
double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
public static double flywheel0Time = 1.5;
boolean gatePickup = true;
boolean stack3 = true;
boolean stack2 = true;
double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA;
double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB;
double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA;
double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB;
public static int pickupStackSpeed = 17;
public static int pickupGateSpeed = 25;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 12;
public static int pickupGateSpeed = 30;
int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 2;
@@ -82,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode {
public static double shootStackTime = 2;
public static double shootGateTime = 2.5;
public static double colorSenseTime = 1;
public static double intakeStackTime = 4.5;
public static double intakeGateTime = 8;
public static double intakeStackTime = 2.5;
public static double intakeGateTime = 2;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
@@ -96,7 +93,6 @@ public class Auto_LT_Far extends LinearOpMode {
TrajectoryActionBuilder shoot3 = null;
TrajectoryActionBuilder pickupGate = null;
TrajectoryActionBuilder shootGate = null;
TrajectoryActionBuilder pickup2 = null;
Pose2d autoStart = new Pose2d(0,0,0);
@Override
@@ -127,46 +123,16 @@ public class Auto_LT_Far extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
limelightUsed = false;
Spindexer.teleop = false;
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){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall1;
} else if (motif == 22){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b;
} else {
AutoActions.firstSpindexShootPos = spindexer_outtakeBall2;
}
}
if (gamepad2.triangleWasPressed()){
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3;
}
if (gamepad2.leftBumperWasPressed()){
stack2 = !stack2;
}
turret.setTurret(turretShootPos);
@@ -199,33 +165,17 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = rShootY;
hShoot = rShootH;
xStackPickupFarA = rStackPickupAX;
yStackPickupFarA = rStackPickupAY;
hStackPickupFarA = rStackPickupAH;
xStackPickupA = rStackPickupFarAX;
yStackPickupA = rStackPickupFarAY;
hStackPickupA = rStackPickupFarAH;
xStackPickupFarB = rStackPickupBX;
yStackPickupFarB = rStackPickupBY;
hStackPickupFarB = rStackPickupBH;
xStackPickupB = rStackPickupFarBX;
yStackPickupB = rStackPickupFarBY;
hStackPickupB = rStackPickupFarBH;
xStackPickupMiddleA = rStackPickupAX;
yStackPickupMiddleA = rStackPickupAY;
hStackPickupMiddleA = rStackPickupAH;
xStackPickupMiddleB = rStackPickupBX;
yStackPickupMiddleB = rStackPickupBY;
hStackPickupMiddleB = rStackPickupBH;
pickupGateXA = rPickupGateXA;
pickupGateYA = rPickupGateYA;
pickupGateHA = rPickupGateHA;
pickupGateXB = rPickupGateXB;
pickupGateYB = rPickupGateYB;
pickupGateHB = rPickupGateHB;
pickupGateXC = rPickupGateXC;
pickupGateYC = rPickupGateYC;
pickupGateHC = rPickupGateHC;
pickupGateX = rPickupGateXA;
pickupGateY = rPickupGateYA;
pickupGateH = rPickupGateHA;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -233,10 +183,15 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(1);
turret.pipelineSwitch(4);
robot.limelight.start();
limelightUsed = true;
gamepad2.rumble(500);
sleep(1000);
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
}
} else {
robot.light.setPosition(0.6);
@@ -253,33 +208,17 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = bShootY;
hShoot = bShootH;
xStackPickupFarA = bStackPickupAX;
yStackPickupFarA = bStackPickupAY;
hStackPickupFarA = bStackPickupAH;
xStackPickupA = bStackPickupFarAX;
yStackPickupA = bStackPickupFarAY;
hStackPickupA = bStackPickupFarAH;
xStackPickupFarB = bStackPickupBX;
yStackPickupFarB = bStackPickupBY;
hStackPickupFarB = bStackPickupBH;
xStackPickupB = bStackPickupFarBX;
yStackPickupB = bStackPickupFarBY;
hStackPickupB = bStackPickupFarBH;
xStackPickupMiddleA = bStackPickupAX;
yStackPickupMiddleA = bStackPickupAY;
hStackPickupMiddleA = bStackPickupAH;
xStackPickupMiddleB = bStackPickupBX;
yStackPickupMiddleB = bStackPickupBY;
hStackPickupMiddleB = bStackPickupBH;
pickupGateXA = bPickupGateXA;
pickupGateYA = bPickupGateYA;
pickupGateHA = bPickupGateHA;
pickupGateXB = bPickupGateXB;
pickupGateYB = bPickupGateYB;
pickupGateHB = bPickupGateHB;
pickupGateXC = bPickupGateXC;
pickupGateYC = bPickupGateYC;
pickupGateHC = bPickupGateHC;
pickupGateX = bPickupGateXA;
pickupGateY = bPickupGateYA;
pickupGateH = bPickupGateHA;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -287,10 +226,15 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(5);
turret.pipelineSwitch(2);
robot.limelight.start();
limelightUsed = true;
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));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA))
.strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB),
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
new TranslationalVelConstraint(pickupStackSpeed));
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.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)))
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA))
.waitSeconds(0.2)
.strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
.strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
.strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH),
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));
limelightUsed = true;
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack Far?", stack3);
TELE.addData("Pickup Stack Middle?", stack2);
TELE.addData("Pickup Stack?", stack3);
TELE.addData("Start Position", autoStart);
TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update();
@@ -351,11 +288,6 @@ public class Auto_LT_Far extends LinearOpMode {
shoot();
}
if (stack2){
cycleStackMiddle();
shoot();
}
while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){
@@ -432,79 +364,16 @@ public class Auto_LT_Far extends LinearOpMode {
pickup3.build(),
autoActions.intake(
intakeStackTime,
xStackPickupFarB,
yStackPickupFarB,
hStackPickupFarB
),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupFarB,
yStackPickupFarB,
posXTolerance,
posYTolerance,
obeliskTurrPos3
xStackPickupB,
yStackPickupB,
hStackPickupB
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
servos.setSpinPos(spinStartPos);
Actions.runBlocking(
new ParallelAction(
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)
shoot3.build()
)
);
}
@@ -515,25 +384,12 @@ public class Auto_LT_Far extends LinearOpMode {
pickupGate.build(),
autoActions.intake(
intakeStackTime,
pickupGateXC,
pickupGateYC,
pickupGateHC
),
autoActions.detectObelisk(
intakeGateTime,
pickupGateXC,
pickupGateYC,
posXTolerance,
posYTolerance,
obeliskTurrPos3
pickupGateX,
pickupGateY,
pickupGateH
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
}
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;
public int motif = 0;
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) {
this.robot = rob;
this.drive = dri;
@@ -427,8 +427,10 @@ public class AutoActions {
if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
servos.setSpinPos(spindexer_intakePos1);
intaking = false;
return false;
} else {
intaking = 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 bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
public static double rShootX = 100, rShootY = 55, rShootH = 90;
public static double bShootX = 100, bShootY = -55, bShootH = -90;
public static double rShootX = 100, rShootY = 60, rShootH = 145.2;
public static double bShootX = 100, bShootY = -55, bShootH = -145.2;
public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145;
public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145;
public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1;
public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1;
public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1;
public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140;
public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140;
public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140;
public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140;
public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145;
public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145;
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 bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54;
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 spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.014;
public static double shootAllSpindexerSpeedIncrease = 0.013;
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 tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
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 turretSpeedOffset = 0.01;
public double vel = 3000;
@@ -76,7 +76,7 @@ public class TeleopV3 extends LinearOpMode {
int intakeTicker = 0;
private boolean shootAll = false;
boolean relocalize = false;
public static boolean relocalize = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -112,6 +112,7 @@ public class TeleopV3 extends LinearOpMode {
light.setState(StateEnums.LightState.MANUAL);
limelightUsed = true;
Spindexer.teleop = true;
while (opModeInInit()) {
robot.limelight.start();
@@ -154,13 +155,13 @@ public class TeleopV3 extends LinearOpMode {
shootAll = false;
servo.setTransferPos(transferServo_out);
light.setState(StateEnums.LightState.BALL_COUNT);
//light.setState(StateEnums.LightState.BALL_COUNT);
} else if (gamepad2.triangle){
light.setState(StateEnums.LightState.BALL_COLOR);
//light.setState(StateEnums.LightState.BALL_COLOR);
} else {
light.setState(StateEnums.LightState.GOAL_LOCK);
//light.setState(StateEnums.LightState.GOAL_LOCK);
}
//TURRET TRACKING
@@ -354,14 +355,14 @@ public class TeleopV3 extends LinearOpMode {
//
// TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
TELE.addData("robot H", robotHeading);
// TELE.addData("robotX", robotX);
// TELE.addData("robotY", robotY);
// TELE.addData("robot H", robotHeading);
// 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 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);
@@ -369,13 +370,13 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
TELE.update();
light.update();
//light.update();
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();
if (isStopRequested()) return;
while (opModeIsActive()){
spindexer.setIntakePower(1);
robot.transfer.setPower(1);
if (gamepad1.crossWasPressed()){
motif = 21;
} else if (gamepad1.squareWasPressed()){
motif = 22;
} else if (gamepad1.triangleWasPressed()){
motif = 23;
}
flywheel.manageFlywheel(2500);
if (gamepad1.leftBumperWasPressed()){
intaking = false;
Actions.runBlocking(
autoActions.prepareShootAll(
3,
5,
motif,
0.501,
0.501,
0.501
)
);
} else if (gamepad1.rightBumperWasPressed()){
intaking = false;
Actions.runBlocking(
autoActions.shootAllAuto(
3.5,
0.014,
0.501,
0.501,
0.501
)
);
intaking = true;
} else if (intaking){
spindexer.processIntake();
}
if (opModeIsActive()){
Actions.runBlocking(
autoActions.ShakeDrivetrain(
100
)
);
// spindexer.setIntakePower(1);
// robot.transfer.setPower(1);
//
// if (gamepad1.crossWasPressed()){
// motif = 21;
// } else if (gamepad1.squareWasPressed()){
// motif = 22;
// } else if (gamepad1.triangleWasPressed()){
// motif = 23;
// }
// flywheel.manageFlywheel(2500);
//
// if (gamepad1.leftBumperWasPressed()){
// intaking = false;
// Actions.runBlocking(
// autoActions.prepareShootAll(
// 3,
// 5,
// motif,
// 0.501,
// 0.501,
// 0.501
// )
// );
// } else if (gamepad1.rightBumperWasPressed()){
// intaking = false;
// Actions.runBlocking(
// autoActions.shootAllAuto(
// 3.5,
// 0.014,
// 0.501,
// 0.501,
// 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){
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
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1);
double frontLeftPower = (prevY + prevX + prevRX) / denominator;
double backLeftPower = (prevY - prevX + prevRX) / denominator;
double frontRightPower = (prevY - prevX - prevRX) / denominator;
double backRightPower = (prevY + prevX - prevRX) / denominator;
robot.frontLeft.setPower(frontLeftPower);
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;
case BALL_COLOR:
if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
double currentTime = System.currentTimeMillis();
if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
lightColor = spindexer.getRearCenterLight();
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) {
} else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) {
lightColor = 0;
} else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
} else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
lightColor = spindexer.getDriverLight();
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
} else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
lightColor = 0;
} else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
} else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
lightColor = spindexer.getPassengerLight();
} 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.
// 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.
public static boolean teleop = false;
public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) {
boolean newPos1Detection = false;
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
double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
@@ -187,7 +200,7 @@ public class Spindexer {
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1
if (distanceRearCenter < 48) {
if (distanceRearCenter < rearDistance) {
// Mark Ball Found
newPos1Detection = true;
@@ -209,7 +222,7 @@ public class Spindexer {
// Position 2
// Find which ball position this is in the spindexer
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
if (distanceFrontDriver < 50) {
if (distanceFrontDriver < frontDriverDistance) {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
@@ -235,7 +248,7 @@ public class Spindexer {
// Position 3
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
if (distanceFrontPassenger < 29) {
if (distanceFrontPassenger < frontPassengerDistance) {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
@@ -512,6 +525,7 @@ public class Spindexer {
break;
case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition==2) {
@@ -535,33 +549,27 @@ public class Spindexer {
break;
case SHOOT_PREP_CONTINOUS:
if (shootTicks > waitFirstBallTicks){
if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
shootTicks++;
} else if (servos.spinEqual(spinStartPos)){
shootTicks++;
servos.setTransferPos(transferServo_in);
} else {
servos.setSpinPos(spinStartPos);
}
break;
case SHOOT_CONTINOUS:
whileShooting = true;
servos.setTransferPos(transferServo_in);
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
if (servos.getSpinPos() > spinEndPos){
whileShooting = false;
servos.setTransferPos(transferServo_out);
shootTicks = 0;
currentIntakeState = IntakeState.FINDNEXT;
servos.setTransferPos(transferServo_out);
} else {
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03;
}
servos.setSpinPos(spinPos);
moveSpindexerToPos(spinPos);
}
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.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.teleop.TeleopV3;
import java.util.ArrayList;
import java.util.List;
@@ -79,7 +80,7 @@ public class Turret {
}
public double getTurrPos() {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
return robot.turr1.getPosition();
}
private double prevTurrPos = 0;
@@ -114,23 +115,18 @@ public class Turret {
if (result.isValid()) {
tx = result.getTx();
ty = result.getTy();
// MegaTag1 code for receiving position
Pose3D botpose = result.getBotpose();
if (botpose != null) {
limelightPosX = botpose.getPosition().x;
limelightPosY = botpose.getPosition().y;
}
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
limelightTagPose = fiducial.getRobotPoseTargetSpace();
if (limelightTagPose != null){
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
if (TeleopV3.relocalize){
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
limelightTagPose = fiducial.getRobotPoseTargetSpace();
if (limelightTagPose != null){
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
}
}
}
}
}
if (xPos != null){