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

front gate auto in progress

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-17 18:07:09 -06:00
parent dd1db74059
commit 44caad767b
4 changed files with 212 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,7 +40,11 @@ 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 spindexerSpeedIncrease = 0.014;
public static double velGate0Start = 2700, hoodGate0Start = 0.61;
public static double velGate0End = 2700, hoodGate0End = 0.4;
public static double spindexerSpeedIncrease = 0.025;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
@@ -63,12 +67,10 @@ 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 xShoot0 = 40, yShoot0 = 0.1, hShoot0 = 0.1;
public static double waitToShoot0 = 1.1;
public static double waitToPickupGate2 = 0.5;
public static double pickupStackGateSpeed = 23;
public static double waitToShoot0 = 0.6;
public static double waitToPickupGate2 = 0.1;
public static double pickupStackGateSpeed = 50;
public static double intake2TimeGate = 6;
public static double xShootGate = 40, yShootGate = 0.2, hShootGate = 0.2;
Robot robot;
MultipleTelemetry TELE;
@@ -94,9 +96,11 @@ public class Auto_LT_Close extends LinearOpMode {
double x4b, y4b, h4b;
double xShoot, yShoot, hShoot;
double xShoot0, yShoot0, hShoot0;
double xGate, yGate, hGate;
double xPrep, yPrep, hPrep;
double xShootGate, yShootGate, hShootGate;
double xLeave, yLeave, hLeave;
double xLeaveGate, yLeaveGate, hLeaveGate;
int ballCycles = 3;
int prevMotif = 0;
@@ -206,9 +210,7 @@ public class Auto_LT_Close extends LinearOpMode {
x4b = rx4b;
y4b = ry4b;
h4b = rh4b;
xPrep = rxPrep;
yPrep = ryPrep;
hPrep = rhPrep;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
@@ -216,6 +218,16 @@ public class Auto_LT_Close extends LinearOpMode {
yLeave = rLeaveY;
hLeave = rLeaveH;
xShoot0 = rShoot0X;
yShoot0 = rShoot0Y;
hShoot0 = rShoot0H;
xShootGate = rShootGateX;
yShootGate = rShootGateY;
hShootGate = rShootGateH;
xLeaveGate = rLeaveGateX;
yLeaveGate = rLeaveGateY;
hLeaveGate = rLeaveGateH;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -247,9 +259,6 @@ public class Auto_LT_Close extends LinearOpMode {
y4b = by4b;
h4b = bh4b;
xPrep = bxPrep;
yPrep = byPrep;
hPrep = bhPrep;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
@@ -257,6 +266,16 @@ public class Auto_LT_Close extends LinearOpMode {
yLeave = bLeaveY;
hLeave = bLeaveH;
xShoot0 = bShoot0X;
yShoot0 = bShoot0Y;
hShoot0 = bShoot0H;
xShootGate = bShootGateX;
yShootGate = bShootGateY;
hShootGate = bShootGateH;
xLeaveGate = bLeaveGateX;
yLeaveGate = bLeaveGateY;
hLeaveGate = bLeaveGateH;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -265,12 +284,22 @@ public class Auto_LT_Close extends LinearOpMode {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
if (gateCycle){
pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickupStackGateSpeed));
} else {
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
}
if (ballCycles < 2){
if (gateCycle){
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
} else if (ballCycles < 2){
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
@@ -283,12 +312,15 @@ public class Auto_LT_Close extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickup1Speed));
if (ballCycles < 3 || gateCycle){
if (gateCycle){
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
} else if (ballCycles < 3){
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
@@ -543,14 +575,16 @@ public class Auto_LT_Close extends LinearOpMode {
new SequentialAction(
new ParallelAction(
autoActions.Wait(waitToShoot0),
autoActions.manageShooterAuto(
autoActions.manageShooterManual(
waitToShoot0,
xShoot0,
yShoot0,
hShoot0
velGate0Start,
hoodGate0Start,
velGate0Start,
hoodGate0Start,
0.501
)
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease),
autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501),
autoActions.intake(
intake2TimeGate,
xShootGate,
@@ -561,4 +595,5 @@ public class Auto_LT_Close extends LinearOpMode {
)
);
}
}

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

@@ -276,6 +276,75 @@ public class AutoActions{
};
}
public Action shootAllManual(double shootTime, double spindexSpeed, double velStart, double hoodStart, double velEnd, double hoodEnd, double turr) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
int shooterTicker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.1);
light.setState(StateEnums.LightState.BALL_COLOR);
light.update();
if (ticker == 1) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterManual(shootTime, velStart, hoodStart, velEnd, hoodEnd, turr);
}
ticker++;
manageShooter.run(telemetryPacket);
double prevSpinPos = servos.getSpinCmdPos();
boolean end;
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);
}
}
return true;
} else {
servos.setTransferPos(transferServo_out);
Spindexer.whileShooting = false;
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action intake(
double time,
double posX,
@@ -305,7 +374,12 @@ public class AutoActions{
manageShooter.run(telemetryPacket);
return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull();
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()){
spindexer.setIntakePower(-0.1);
return false;
} else {
return true;
}
}
};
}
@@ -409,7 +483,7 @@ public class AutoActions{
if (posX != 0.501) {
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
} else {
deltaPose = new Pose2d(robotX, robotY, robotHeading);
deltaPose = new Pose2d(dx, dy, robotHeading);
}
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
@@ -456,6 +530,72 @@ public class AutoActions{
}
};
}
public Action manageShooterManual(
double time,
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);
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = currentPose.position.x;
double robotY = currentPose.position.y;
double robotHeading = currentPose.heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose;
if (turr == 0.501) {
deltaPose = new Pose2d(dx, dy, robotHeading);
if (!detectingObelisk) {
turret.trackGoal(deltaPose);
}
} else {
turret.setTurret(turr);
}
servos.setHoodPos(hoodStart + (hoodEnd-hoodStart) * ((System.currentTimeMillis() - stamp)/(time*1000)));
double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000));
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;
teleStart = currentPose;
TELE.addData("Steady?", flywheel.getSteady());
TELE.update();
return !timeDone;
}
};
}
}

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

@@ -42,5 +42,14 @@ public class Front_Poses {
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1;
public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1;
public static double rShootGateX = 40, rShootGateY = 0.2, rShootGateH = 90;
public static double bShootGateX = 40, bShootGateY = -0.2, bShootGateH = -90;
public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -16,7 +16,7 @@ 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.3;
public static double spinStartPos = 0.25;
public static double spinEndPos = 0.85;
public static double shootAllSpindexerSpeedIncrease = 0.014;