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

front gate auto still in progress

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-19 21:19:04 -06:00
parent 44caad767b
commit 3268d5cd02
4 changed files with 187 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,17 +40,13 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double velGate0Start = 2700, hoodGate0Start = 0.61;
public static double velGate0Start = 2700, hoodGate0Start = 0.6;
public static double velGate0End = 2700, hoodGate0End = 0.4;
public static double velGate0End = 2700, hoodGate0End = 0.35;
public static double hood0MoveTime = 2;
public static double spindexerSpeedIncrease = 0.02;
public static double spindexerSpeedIncrease = 0.025;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double shootAllTime = 2;
public static double shootAllTime = 4;
public static double intake1Time = 3.3;
public static double intake2Time = 3.8;
@@ -67,10 +63,16 @@ public class Auto_LT_Close extends LinearOpMode {
public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2;
public int motif = 0;
public static double waitToShoot0 = 0.6;
public static double waitToPickupGate2 = 0.1;
public static double waitToShoot0 = 0.5;
public static double waitToPickupGate2 = 0.3;
public static double pickupStackGateSpeed = 50;
public static double intake2TimeGate = 6;
public static double intake2TimeGate = 3;
public static double shoot2GateTime = 3;
public static double endGateTime = 25;
public static double waitToPickupGateWithPartner = 2;
public static double waitToPickupGateSolo = 1;
public static double intakeGateTime = 5;
public static double shootGateTime = 3;
Robot robot;
MultipleTelemetry TELE;
@@ -97,7 +99,8 @@ public class Auto_LT_Close extends LinearOpMode {
double xShoot, yShoot, hShoot;
double xShoot0, yShoot0, hShoot0;
double xGate, yGate, hGate;
double pickupGateAX, pickupGateAY, pickupGateAH;
double pickupGateBX, pickupGateBY, pickupGateBH;
double xShootGate, yShootGate, hShootGate;
double xLeave, yLeave, hLeave;
double xLeaveGate, yLeaveGate, hLeaveGate;
@@ -105,7 +108,11 @@ public class Auto_LT_Close extends LinearOpMode {
int ballCycles = 3;
int prevMotif = 0;
boolean gateCycle = true;
boolean withPartner = false;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
double waitToPickupGate = 0;
// initialize path variables here
TrajectoryActionBuilder shoot0 = null;
@@ -116,6 +123,8 @@ public class Auto_LT_Close extends LinearOpMode {
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
TrajectoryActionBuilder shoot0ToPickup2 = null;
TrajectoryActionBuilder gateCyclePickup = null;
TrajectoryActionBuilder gateCycleShoot = null;
@Override
public void runOpMode() throws InterruptedException {
@@ -155,7 +164,12 @@ public class Auto_LT_Close extends LinearOpMode {
while (opModeInInit()) {
servos.setHoodPos(shoot0Hood);
if (gateCycle){
servos.setHoodPos(hoodGate0Start);
} else {
servos.setHoodPos(shoot0Hood);
}
turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) {
@@ -228,6 +242,13 @@ public class Auto_LT_Close extends LinearOpMode {
yLeaveGate = rLeaveGateY;
hLeaveGate = rLeaveGateH;
pickupGateAX = rPickupGateAX;
pickupGateAY = rPickupGateAY;
pickupGateAH = rPickupGateAH;
pickupGateBX = rPickupGateBX;
pickupGateBY = rPickupGateBY;
pickupGateBH = rPickupGateBH;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -276,6 +297,13 @@ public class Auto_LT_Close extends LinearOpMode {
yLeaveGate = bLeaveGateY;
hLeaveGate = bLeaveGateH;
pickupGateAX = bPickupGateAX;
pickupGateAY = bPickupGateAY;
pickupGateAH = bPickupGateAH;
pickupGateBX = bPickupGateBX;
pickupGateBY = bPickupGateBY;
pickupGateBH = bPickupGateBH;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -338,6 +366,20 @@ public class Auto_LT_Close extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
if (withPartner){
waitToPickupGate = waitToPickupGateWithPartner;
} else {
waitToPickupGate = waitToPickupGateSolo;
}
gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
.strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
.waitSeconds(waitToPickupGate)
.strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH)))
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Ball Cycles", ballCycles);
@@ -350,11 +392,19 @@ public class Auto_LT_Close extends LinearOpMode {
if (isStopRequested()) return;
if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1);
if (gateCycle){
shoot0Gate();
cycleStackMiddleGate();
while (getRuntime() - stamp < endGateTime){
cycleGateIntake();
cycleGateShoot();
}
} else {
startAuto();
shoot();
@@ -574,9 +624,9 @@ public class Auto_LT_Close extends LinearOpMode {
shoot0ToPickup2.build(),
new SequentialAction(
new ParallelAction(
autoActions.Wait(waitToShoot0),
autoActions.manageShooterManual(
waitToShoot0,
0.501,
velGate0Start,
hoodGate0Start,
velGate0Start,
@@ -584,7 +634,15 @@ public class Auto_LT_Close extends LinearOpMode {
0.501
)
),
autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501),
autoActions.shootAllManual(
shootAllTime,
hood0MoveTime,
spindexerSpeedIncrease,
velGate0Start,
hoodGate0Start,
velGate0End,
hoodGate0End,
0.501),
autoActions.intake(
intake2TimeGate,
xShootGate,
@@ -596,4 +654,59 @@ public class Auto_LT_Close extends LinearOpMode {
);
}
void cycleStackMiddleGate(){
servos.setSpinPos(spinStartPos);
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
new SequentialAction(
new ParallelAction(
autoActions.manageShooterAuto(
shoot2GateTime,
xShootGate,
yShootGate,
hShootGate
),
autoActions.Wait(shoot2GateTime)
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
)
);
}
void cycleGateIntake(){
Actions.runBlocking(
new ParallelAction(
gateCyclePickup.build(),
autoActions.intake(
intakeGateTime,
xShootGate,
yShootGate,
hShootGate
)
)
);
}
void cycleGateShoot(){
servos.setSpinPos(spinStartPos);
Actions.runBlocking(
new ParallelAction(
gateCycleShoot.build(),
new SequentialAction(
new ParallelAction(
autoActions.manageShooterAuto(
shootGateTime,
xShootGate,
yShootGate,
hShootGate
),
autoActions.Wait(shootGateTime)
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
)
);
}
}

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
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.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -32,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Config
public class AutoActions{
public class AutoActions {
Robot robot;
MultipleTelemetry TELE;
Servos servos;
@@ -47,12 +48,12 @@ public class AutoActions{
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double firstSpindexShootPos = spindexer_outtakeBall1;
private double firstSpindexShootPos = spinStartPos;
private boolean shootForward = true;
public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos;
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.drive = dri;
this.TELE = tel;
@@ -81,37 +82,38 @@ public class AutoActions{
boolean decideGreenSlot = false;
void spin1PosFirst(){
void spin1PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = spindexer_outtakeBall3 + 0.1;
}
void spin2PosFirst(){
void spin2PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = spindexer_outtakeBall3b - 0.1;
}
void reverseSpin2PosFirst(){
void reverseSpin2PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
spinEndPos = 0.95;
}
void spin3PosFirst(){
void spin3PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = spindexer_outtakeBall1 - 0.1;
}
void oddSpin3PosFirst(){
void oddSpin3PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = spindexer_outtakeBall2 + 0.1;
}
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
@@ -215,6 +217,7 @@ public class AutoActions{
int shooterTicker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
@@ -238,16 +241,15 @@ public class AutoActions{
double prevSpinPos = servos.getSpinCmdPos();
boolean end;
if (shootForward){
if (shootForward) {
end = prevSpinPos > spinEndPos;
} else {
end = prevSpinPos < spinEndPos;
}
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
@@ -255,7 +257,7 @@ public class AutoActions{
Spindexer.whileShooting = true;
if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else if (shooterTicker > Spindexer.waitFirstBallTicks){
} else if (shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
@@ -276,7 +278,15 @@ public class AutoActions{
};
}
public Action shootAllManual(double shootTime, double spindexSpeed, double velStart, double hoodStart, double velEnd, double hoodEnd, double turr) {
public Action shootAllManual(
double shootTime,
double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal
double spindexSpeed,
double velStart,
double hoodStart,
double velEnd,
double hoodEnd,
double turr) {
return new Action() {
int ticker = 1;
@@ -284,6 +294,7 @@ public class AutoActions{
int shooterTicker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
@@ -297,7 +308,7 @@ public class AutoActions{
if (ticker == 1) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterManual(shootTime, velStart, hoodStart, velEnd, hoodEnd, turr);
manageShooter = manageShooterManual(shootTime, hoodMoveTime, velStart, hoodStart, velEnd, hoodEnd, turr);
}
ticker++;
@@ -307,27 +318,20 @@ public class AutoActions{
double prevSpinPos = servos.getSpinCmdPos();
boolean end;
if (shootForward){
if (shootForward) {
end = prevSpinPos > spinEndPos;
} else {
end = prevSpinPos < spinEndPos;
}
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
Spindexer.whileShooting = true;
if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else if (shooterTicker > Spindexer.waitFirstBallTicks){
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
if (System.currentTimeMillis() - stamp < shootTime * 1000 && !end) {
servos.setTransferPos(transferServo_in);
shooterTicker++;
Spindexer.whileShooting = true;
if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else if (shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
return true;
@@ -355,6 +359,7 @@ public class AutoActions{
double stamp = 0.0;
int ticker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
@@ -374,7 +379,7 @@ public class AutoActions{
manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()){
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
spindexer.setIntakePower(-0.1);
return false;
} else {
@@ -385,6 +390,7 @@ public class AutoActions{
}
private boolean detectingObelisk = false;
public Action detectObelisk(
double time,
double posX,
@@ -427,8 +433,8 @@ public class AutoActions{
teleStart = currentPose;
if (shouldFinish){
if (redAlliance){
if (shouldFinish) {
if (redAlliance) {
turret.pipelineSwitch(4);
} else {
turret.pipelineSwitch(2);
@@ -514,13 +520,14 @@ public class AutoActions{
};
}
public Action Wait(double time){
public Action Wait(double time) {
return new Action() {
boolean ticker = false;
double stamp = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (!ticker){
if (!ticker) {
stamp = System.currentTimeMillis();
ticker = true;
}
@@ -532,19 +539,20 @@ public class AutoActions{
}
public Action manageShooterManual(
double time,
double maxTime,
double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal
double velStart,
double hoodStart,
double velEnd,
double hoodEnd,
double turr
){
) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
final boolean timeFallback = (time != 0.501);
final boolean timeFallback = (maxTime != 0.501);
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -578,14 +586,14 @@ public class AutoActions{
turret.setTurret(turr);
}
servos.setHoodPos(hoodStart + (hoodEnd-hoodStart) * ((System.currentTimeMillis() - stamp)/(time*1000)));
double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000));
servos.setHoodPos(hoodStart + ((hoodEnd - hoodStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1)));
double vel = velStart + (velEnd - velStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > maxTime * 1000;
teleStart = currentPose;

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

@@ -51,5 +51,11 @@ public class Front_Poses {
public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
public static double rPickupGateAX = 36, rPickupGateAY = 50, rPickupGateAH = 140;
public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140;
public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 140;
public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -140;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -16,8 +16,8 @@ public class ServoPositions {
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 = 0.25;
public static double spinEndPos = 0.85;
public static double spinStartPos = 0.05;
public static double spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.014;