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

auton progress with errors

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-08 23:02:42 -06:00
parent ef1c7b0e6b
commit 4488fabecf
6 changed files with 164 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,67 +1,7 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
@@ -849,6 +789,7 @@ public class Auto_LT_Close extends LinearOpMode {
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (redAlliance) {
@@ -1052,8 +993,8 @@ public class Auto_LT_Close extends LinearOpMode {
shootAllVelocity,
shootAllHood,
intake1Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1113,8 +1054,8 @@ public class Auto_LT_Close extends LinearOpMode {
pickup2.build(),
manageShooterAuto(
intake2Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1171,8 +1112,8 @@ public class Auto_LT_Close extends LinearOpMode {
pickup3.build(),
manageShooterAuto(
intake3Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),

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

@@ -1,13 +1,7 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
@@ -31,6 +25,7 @@ import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -81,6 +76,12 @@ public class Auto_LT_Far extends LinearOpMode {
public static double firstShootTime = 0.3;
public static double flywheel0Time = 3.5;
public static double shoot0Time = 2;
boolean gatePickup = false;
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
@@ -667,6 +668,9 @@ public class Auto_LT_Far extends LinearOpMode {
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
TrajectoryActionBuilder leaveFromShoot = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
@Override
public void runOpMode() throws InterruptedException {
@@ -694,23 +698,34 @@ public class Auto_LT_Far extends LinearOpMode {
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
drive = new MecanumDrive(hardwareMap, autoStart);
servos.setSpinPos(autoSpinStartPos);
servos.setTransferPos(transferServo_out);
robot.light.setPosition(1);
while (opModeInInit()) {
// Recalibration in initialization
drive.updatePoseEstimate();
if (gamepad2.square) {
teleEnd = drive.localizer.getPose(); // use this position as starting position
if (gamepad2.triangle) {
autoStart = drive.localizer.getPose(); // use this position as starting position
gamepad2.rumble(1000);
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3;
}
turret.setTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood);
@@ -734,6 +749,18 @@ public class Auto_LT_Far extends LinearOpMode {
yLeave = rLeaveY;
hLeave = rLeaveH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
xStackPickupA = rStackPickupAX;
yStackPickupA = rStackPickupAY;
hStackPickupA = rStackPickupAH;
xStackPickupB = rStackPickupBX;
yStackPickupB = rStackPickupBY;
hStackPickupB = rStackPickupBH;
turretShootPos = turrDefault + redTurretShootPos;
} else {
robot.light.setPosition(0.6);
@@ -742,15 +769,40 @@ public class Auto_LT_Far extends LinearOpMode {
yLeave = bLeaveY;
hLeave = bLeaveH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
xStackPickupA = bStackPickupAX;
yStackPickupA = bStackPickupAY;
hStackPickupA = bStackPickupAH;
xStackPickupB = bStackPickupBX;
yStackPickupB = bStackPickupBY;
hStackPickupB = bStackPickupBH;
turretShootPos = turrDefault + blueTurretShootPos;
}
leave3Ball = drive.actionBuilder(teleEnd)
leave3Ball = drive.actionBuilder(autoStart)
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Start Position", teleEnd);
TELE.addData("Gate Cycle?", gatePickup);
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();
}
@@ -762,11 +814,21 @@ public class Auto_LT_Far extends LinearOpMode {
// Currently only shoots; keep this start and modify times and then add extra paths
if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1);
startAuto();
if (stack3){
cycleStackFar();
}
if (gatePickup || stack3){
leave();
} else {
leave3Ball();
}
// Actual way to end autonomous in to find final position
while (opModeIsActive()) {
@@ -810,4 +872,67 @@ public class Auto_LT_Far extends LinearOpMode {
assert leave3Ball != null;
Actions.runBlocking(leave3Ball.build());
}
void leave(){
assert leaveFromShoot != null;
Actions.runBlocking(leaveFromShoot.build());
}
void cycleStackFar(){
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
manageShooterAuto(
intake3Time,
0.501,
0.501,
0.501,
0.501
),
intake(intake3Time),
detectObelisk(
intake3Time,
0.501,
0.501,
0.501,
0.501,
obeliskTurrPos3
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shoot3Time,
0.501,
0.501,
0.501,
0.501
),
shoot3.build(),
prepareShootAll(colorSenseTime, shoot3Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
finalShootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
)
);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java
View File

@@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java
View File

@@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;

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

@@ -1,10 +1,23 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Back_Poses {
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50;
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
public static double rShootX = 95, rShootY = 85, rShootH = 90;
public static double bShootX = 95, bShootY = -85, bShootH = -90;
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
}

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -43,5 +43,4 @@ public class Front_Poses {
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
}