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

close auto is pretty close to good

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-14 17:16:25 -06:00
parent d0c34132de
commit 28816a6e34
3 changed files with 117 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -32,7 +32,7 @@ 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.012;
public static double spindexerSpeedIncrease = 0.005;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -59,12 +59,12 @@ public class Auto_LT_Close extends LinearOpMode {
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 12;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 2.0;
public static double posYTolerance = 2.0;
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0;
// ---- OBELISK DETECTION ----
public static double shoot1Time = 2;
public static double shoot2Time = 2;
public static double shoot3Time = 2;
public static double shoot1Time = 2.5;
public static double shoot2Time = 2.5;
public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2;
public int motif = 0;
@@ -354,6 +354,7 @@ public class Auto_LT_Close extends LinearOpMode {
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
@@ -374,6 +375,7 @@ public class Auto_LT_Close extends LinearOpMode {
y1,
posXTolerance,
posYTolerance,
h1,
false
)
@@ -391,6 +393,7 @@ public class Auto_LT_Close extends LinearOpMode {
y2b,
posXTolerance,
posYTolerance,
h2b,
true
),
autoActions.intake(intake1Time),
@@ -413,12 +416,15 @@ public class Auto_LT_Close extends LinearOpMode {
double posX;
double posY;
double posH;
if (ballCycles > 1){
posX = xShoot;
posY = yShoot;
posH = hShoot;
} else {
posX = xLeave;
posY = yLeave;
posH = hLeave;
}
Actions.runBlocking(
@@ -429,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode {
posY,
posXTolerance,
posYTolerance,
posH,
false
),
shoot1.build(),
@@ -447,6 +454,7 @@ public class Auto_LT_Close extends LinearOpMode {
y3b,
posXTolerance,
posYTolerance,
h3b,
true
),
autoActions.intake(intake2Time),
@@ -469,12 +477,15 @@ public class Auto_LT_Close extends LinearOpMode {
double posX;
double posY;
double posH;
if (ballCycles > 2){
posX = xShoot;
posY = yShoot;
posH = hShoot;
} else {
posX = xLeave;
posY = yLeave;
posH = hLeave;
}
Actions.runBlocking(
@@ -485,6 +496,7 @@ public class Auto_LT_Close extends LinearOpMode {
posY,
posXTolerance,
posYTolerance,
posH,
false
),
shoot2.build(),
@@ -503,6 +515,7 @@ public class Auto_LT_Close extends LinearOpMode {
y4b,
posXTolerance,
posYTolerance,
h4b,
true
),
autoActions.intake(intake3Time),
@@ -531,6 +544,7 @@ public class Auto_LT_Close extends LinearOpMode {
yLeave,
posXTolerance,
posYTolerance,
hLeave,
false
),
shoot3.build(),

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

@@ -81,9 +81,9 @@ public class Auto_LT_Far extends LinearOpMode {
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15;
public static int pickupStackSpeed = 12;
int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.005;
// initialize path variables here
@@ -236,8 +236,6 @@ 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();
@@ -246,6 +244,8 @@ public class Auto_LT_Far extends LinearOpMode {
//cycleStackFar();
}
//TODO: insert code here for gate auto
if (gatePickup || stack3){
leave();
} else {
@@ -269,22 +269,22 @@ public class Auto_LT_Far extends LinearOpMode {
}
// void shoot(){
// Actions.runBlocking(
// new ParallelAction(
// autoActions.manageShooterAuto(
// shootAllTime,
// 0.501,
// 0.501,
// 0.501,
// 0.501,
// false
// ),
// autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
// )
//
// );
// }
void shoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
}
void startAuto(){
Actions.runBlocking(
@@ -295,6 +295,7 @@ public class Auto_LT_Far extends LinearOpMode {
0.501,
0.501,
0.501,
0.501,
false
)

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

@@ -2,7 +2,6 @@ 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.spinEndPos;
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;
@@ -21,6 +20,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -50,6 +50,7 @@ public class AutoActions{
private boolean shootForward = true;
public static double firstShootTime = 0.3;
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){
this.robot = rob;
@@ -72,6 +73,36 @@ public class AutoActions{
boolean decideGreenSlot = false;
void spin1PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = spindexer_outtakeBall3 + 0.1;
}
void spin2PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = spindexer_outtakeBall3b - 0.1;
}
void reverseSpin2PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
spinEndPos = 0.95;
}
void spin3PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = spindexer_outtakeBall1 - 0.1;
}
void oddSpin3PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = spindexer_outtakeBall2 + 0.1;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
@@ -122,39 +153,29 @@ public class AutoActions{
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spin1PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spin2PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spin2PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
reverseSpin2PosFirst();
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
oddSpin3PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spin1PosFirst();
}
}
return true;
@@ -216,7 +237,7 @@ public class AutoActions{
turret.trackGoal(deltaPose);
if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < 0.85) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos);
@@ -243,6 +264,7 @@ public class AutoActions{
};
}
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
@@ -271,9 +293,16 @@ public class AutoActions{
double prevSpinPos = servos.getSpinCmdPos();
if (System.currentTimeMillis() - stamp < shootTime*1000 || (prevSpinPos < spinEndPos && shooterTicker < 2)) {
boolean end;
if (shootForward){
end = prevSpinPos > spinEndPos;
} else {
end = prevSpinPos < spinEndPos;
}
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker == 0) {
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
@@ -295,6 +324,7 @@ public class AutoActions{
spindexer.resetSpindexer();
spindexer.processIntake();
doneShooting = true;
return false;
@@ -443,6 +473,7 @@ public class AutoActions{
double posY,
double posXTolerance,
double posYTolerance,
double posH,
boolean whileIntaking
) {
@@ -454,6 +485,8 @@ public class AutoActions{
double stamp = 0.0;
int ticker = 0;
int shootingTicker = 0;
double shootingStamp = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -477,14 +510,21 @@ public class AutoActions{
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
Pose2d deltaPose;
if (posX != 0.501){
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
} else {
deltaPose = new Pose2d(robotX, robotY, robotHeading);
}
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
if (!detectingObelisk){turret.trackGoal(deltaPose);}
if (!detectingObelisk){
turret.trackGoal(deltaPose);
}
servos.setHoodPos(targetingSettings.hoodAngle);
@@ -504,7 +544,17 @@ public class AutoActions{
teleStart = currentPose;
return !shouldFinish;
if (doneShooting && shootingTicker == 0){
shootingTicker++;
shootingStamp = System.currentTimeMillis();
}
if (System.currentTimeMillis() - shootingStamp > 100 || shouldFinish){
doneShooting = false;
return false;
} else {
return true;
}
}
};