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") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode { public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93; 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 // These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12; 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 flywheel0Time = 3.5;
public static double pickup1Speed = 12; public static double pickup1Speed = 12;
// ---- POSITION TOLERANCES ---- // ---- POSITION TOLERANCES ----
public static double posXTolerance = 2.0; public static double posXTolerance = 5.0;
public static double posYTolerance = 2.0; public static double posYTolerance = 5.0;
// ---- OBELISK DETECTION ---- // ---- OBELISK DETECTION ----
public static double shoot1Time = 2; public static double shoot1Time = 2.5;
public static double shoot2Time = 2; public static double shoot2Time = 2.5;
public static double shoot3Time = 2; public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2; public static double colorSenseTime = 1.2;
public int motif = 0; public int motif = 0;
@@ -354,6 +354,7 @@ public class Auto_LT_Close extends LinearOpMode {
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501,
false false
), ),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
@@ -374,6 +375,7 @@ public class Auto_LT_Close extends LinearOpMode {
y1, y1,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
h1,
false false
) )
@@ -391,6 +393,7 @@ public class Auto_LT_Close extends LinearOpMode {
y2b, y2b,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
h2b,
true true
), ),
autoActions.intake(intake1Time), autoActions.intake(intake1Time),
@@ -413,12 +416,15 @@ public class Auto_LT_Close extends LinearOpMode {
double posX; double posX;
double posY; double posY;
double posH;
if (ballCycles > 1){ if (ballCycles > 1){
posX = xShoot; posX = xShoot;
posY = yShoot; posY = yShoot;
posH = hShoot;
} else { } else {
posX = xLeave; posX = xLeave;
posY = yLeave; posY = yLeave;
posH = hLeave;
} }
Actions.runBlocking( Actions.runBlocking(
@@ -429,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode {
posY, posY,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
posH,
false false
), ),
shoot1.build(), shoot1.build(),
@@ -447,6 +454,7 @@ public class Auto_LT_Close extends LinearOpMode {
y3b, y3b,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
h3b,
true true
), ),
autoActions.intake(intake2Time), autoActions.intake(intake2Time),
@@ -469,12 +477,15 @@ public class Auto_LT_Close extends LinearOpMode {
double posX; double posX;
double posY; double posY;
double posH;
if (ballCycles > 2){ if (ballCycles > 2){
posX = xShoot; posX = xShoot;
posY = yShoot; posY = yShoot;
posH = hShoot;
} else { } else {
posX = xLeave; posX = xLeave;
posY = yLeave; posY = yLeave;
posH = hLeave;
} }
Actions.runBlocking( Actions.runBlocking(
@@ -485,6 +496,7 @@ public class Auto_LT_Close extends LinearOpMode {
posY, posY,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
posH,
false false
), ),
shoot2.build(), shoot2.build(),
@@ -503,6 +515,7 @@ public class Auto_LT_Close extends LinearOpMode {
y4b, y4b,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
h4b,
true true
), ),
autoActions.intake(intake3Time), autoActions.intake(intake3Time),
@@ -531,6 +544,7 @@ public class Auto_LT_Close extends LinearOpMode {
yLeave, yLeave,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
hLeave,
false false
), ),
shoot3.build(), 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; boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA; double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB; double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15; public static int pickupStackSpeed = 12;
int prevMotif = 0; int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.005;
// initialize path variables here // 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 // Currently only shoots; keep this start and modify times and then add extra paths
if (opModeIsActive()) { if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1); robot.transfer.setPower(1);
startAuto(); startAuto();
@@ -246,6 +244,8 @@ public class Auto_LT_Far extends LinearOpMode {
//cycleStackFar(); //cycleStackFar();
} }
//TODO: insert code here for gate auto
if (gatePickup || stack3){ if (gatePickup || stack3){
leave(); leave();
} else { } else {
@@ -269,22 +269,22 @@ public class Auto_LT_Far extends LinearOpMode {
} }
// void shoot(){ void shoot(){
// Actions.runBlocking( Actions.runBlocking(
// new ParallelAction( new ParallelAction(
// autoActions.manageShooterAuto( autoActions.manageShooterAuto(
// shootAllTime, shootAllTime,
// 0.501, 0.501,
// 0.501, 0.501,
// 0.501, 0.501,
// 0.501, 0.501,
// false false
// ), ),
// autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
// ) )
//
// ); );
// } }
void startAuto(){ void startAuto(){
Actions.runBlocking( Actions.runBlocking(
@@ -295,6 +295,7 @@ public class Auto_LT_Far extends LinearOpMode {
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501,
false 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.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; 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.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; 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_outtakeBall1;
@@ -21,6 +20,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d; 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.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -50,6 +50,7 @@ public class AutoActions{
private boolean shootForward = true; private boolean shootForward = true;
public static double firstShootTime = 0.3; public static double firstShootTime = 0.3;
public int motif = 0; 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){ public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){
this.robot = rob; this.robot = rob;
@@ -72,6 +73,36 @@ public class AutoActions{
boolean decideGreenSlot = false; 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 @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) { if (ticker == 0) {
@@ -122,39 +153,29 @@ public class AutoActions{
if (motif_id == 21) { if (motif_id == 21) {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1; spin1PosFirst();
shootForward = true;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2; spin2PosFirst();
shootForward = false;
} else { } else {
firstSpindexShootPos = spindexer_outtakeBall3; spin3PosFirst();
shootForward = false;
} }
} else if (motif_id == 22) { } else if (motif_id == 22) {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2; spin2PosFirst();
shootForward = false;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3; spin3PosFirst();
shootForward = false;
} else { } else {
firstSpindexShootPos = spindexer_outtakeBall2; reverseSpin2PosFirst();
shootForward = true;
} }
} else { } else {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3; spin3PosFirst();
shootForward = false;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b; oddSpin3PosFirst();
shootForward = true;
} else { } else {
firstSpindexShootPos = spindexer_outtakeBall1; spin1PosFirst();
shootForward = true;
} }
} }
return true; return true;
@@ -216,7 +237,7 @@ public class AutoActions{
turret.trackGoal(deltaPose); 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)) { if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos); servos.setSpinPos(spinStartPos);
@@ -243,6 +264,7 @@ public class AutoActions{
}; };
} }
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) { public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() { return new Action() {
int ticker = 1; int ticker = 1;
@@ -271,9 +293,16 @@ public class AutoActions{
double prevSpinPos = servos.getSpinCmdPos(); 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.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
} else { } else {
@@ -295,6 +324,7 @@ public class AutoActions{
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
doneShooting = true;
return false; return false;
@@ -443,6 +473,7 @@ public class AutoActions{
double posY, double posY,
double posXTolerance, double posXTolerance,
double posYTolerance, double posYTolerance,
double posH,
boolean whileIntaking boolean whileIntaking
) { ) {
@@ -454,6 +485,8 @@ public class AutoActions{
double stamp = 0.0; double stamp = 0.0;
int ticker = 0; int ticker = 0;
int shootingTicker = 0;
double shootingStamp = 0;
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -477,14 +510,21 @@ public class AutoActions{
double dx = robotX - goalX; // delta x from robot to goal double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y 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); double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false); (robotX, robotY, robotHeading, 0.0, false);
if (!detectingObelisk){turret.trackGoal(deltaPose);} if (!detectingObelisk){
turret.trackGoal(deltaPose);
}
servos.setHoodPos(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
@@ -504,7 +544,17 @@ public class AutoActions{
teleStart = currentPose; 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;
}
} }
}; };