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

remodeled close auto and it works except for poses (@KeshavAnandCode that is your job

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-14 15:39:03 -06:00
parent 04ea56e31d
commit d0c34132de
7 changed files with 162 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,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.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -31,12 +31,8 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.005;
public static double spindexerSpeedIncrease = 0.005;
public static double finalSpindexerSpeedIncrease = 0.005;
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double spindexerSpeedIncrease = 0.012;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -51,40 +47,25 @@ public class Auto_LT_Close extends LinearOpMode {
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double normalIntakeTime = 3.3;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
double turretShootPos = 0.0;
public static double finalShootAllTime = 3.0;
public static double shootAllTime = 1.8;
public static double shoot0Time = 1.6;
public static double shootAllTime = 2;
public static double intake1Time = 3.3;
public static double intake2Time = 3.8;
public static double intake3Time = 4.2;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 15;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shootAllVelocity = 2500;
public static double shootAllHood = 0.78 + hoodOffset;
// ---- PICKUP POSITION TOLERANCES ----
public static double pickup1XTolerance = 2.0;
public static double pickup1YTolerance = 2.0;
public static double pickup1Speed = 12;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 2.0;
public static double posYTolerance = 2.0;
// ---- OBELISK DETECTION ----
public static double obelisk1Time = 1.5;
public static double obelisk1XTolerance = 2.0;
public static double obelisk1YTolerance = 2.0;
public static double shoot1ToleranceX = 2.0;
public static double shoot1ToleranceY = 2.0;
public static double shoot1Time = 2;
public static double shoot2Time = 2;
public static double shoot3Time = 2;
public static double colorSenseTime = 1.2;
public static double firstShootTime = 0.3;
public int motif = 0;
Robot robot;
@@ -153,7 +134,7 @@ public class Auto_LT_Close extends LinearOpMode {
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
servos.setSpinPos(autoSpinStartPos);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
@@ -331,17 +312,21 @@ public class Auto_LT_Close extends LinearOpMode {
robot.transfer.setPower(1);
startAuto();
shoot();
if (ballCycles > 0){
cycleStackClose();
shoot();
}
if (ballCycles > 1){
cycleStackMiddle();
shoot();
}
if (ballCycles > 2){
cycleStackFar();
shoot();
}
while (opModeIsActive()) {
@@ -360,50 +345,61 @@ public class Auto_LT_Close 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 startAuto() {
assert shoot0 != null;
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageFlywheel(
shoot0Vel,
shoot0Hood,
autoActions.manageShooterAuto(
flywheel0Time,
x1,
0.501,
shoot0XTolerance,
0.501
y1,
posXTolerance,
posYTolerance,
false
)
)
);
Actions.runBlocking(
autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void cycleStackClose(){
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
autoActions.manageFlywheel(
shootAllVelocity,
shootAllHood,
autoActions.manageShooterAuto(
intake1Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x2b,
y2b,
posXTolerance,
posYTolerance,
true
),
autoActions.intake(intake1Time),
autoActions.detectObelisk(
intake1Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x2b,
y2b,
posXTolerance,
posYTolerance,
obeliskTurrPos1
)
@@ -415,35 +411,29 @@ public class Auto_LT_Close extends LinearOpMode {
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheel(
shootAllVelocity,
shootAllHood,
shoot1Time,
0.501,
0.501,
0.501,
0.501
),
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
)
);
double posX;
double posY;
if (ballCycles > 1){
posX = xShoot;
posY = yShoot;
} else {
posX = xLeave;
posY = yLeave;
}
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
shoot1Time,
posX,
posY,
posXTolerance,
posYTolerance,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
)
);
}
@@ -453,18 +443,19 @@ public class Auto_LT_Close extends LinearOpMode {
pickup2.build(),
autoActions.manageShooterAuto(
intake2Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x3b,
y3b,
posXTolerance,
posYTolerance,
true
),
autoActions.intake(intake2Time),
autoActions.detectObelisk(
intake2Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x3b,
y3b,
posXTolerance,
posYTolerance,
obeliskTurrPos2
)
@@ -476,32 +467,29 @@ public class Auto_LT_Close extends LinearOpMode {
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheelAuto(
shoot2Time,
0.501,
0.501,
0.501,
0.501
),
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
)
);
double posX;
double posY;
if (ballCycles > 2){
posX = xShoot;
posY = yShoot;
} else {
posX = xLeave;
posY = yLeave;
}
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
shoot2Time,
posX,
posY,
posXTolerance,
posYTolerance,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
)
);
}
@@ -511,18 +499,19 @@ public class Auto_LT_Close extends LinearOpMode {
pickup3.build(),
autoActions.manageShooterAuto(
intake3Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x4b,
y4b,
posXTolerance,
posYTolerance,
true
),
autoActions.intake(intake3Time),
autoActions.detectObelisk(
intake3Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x4b,
y4b,
posXTolerance,
posYTolerance,
obeliskTurrPos3
)
@@ -536,30 +525,17 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheelAuto(
autoActions.manageShooterAuto(
shoot3Time,
0.501,
0.501,
0.501,
0.501
xLeave,
yLeave,
posXTolerance,
posYTolerance,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
finalShootAllTime,
0.501,
0.501,
0.501,
0.501
),
autoActions.shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
)
);
}
}

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

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.autonomous;
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.teleStart;
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;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
@@ -45,7 +44,7 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
public static double shoot0Vel = 3300, shoot0Hood = 0.48;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0XTolerance = 1.0;
@@ -270,25 +269,37 @@ 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 startAuto(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheel(
shoot0Vel,
shoot0Hood,
autoActions.manageShooterAuto(
flywheel0Time,
0.501,
0.501,
shoot0XTolerance,
0.501
0.501,
0.501,
false
)
)
);
Actions.runBlocking(
autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void leave3Ball(){

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

@@ -62,6 +62,7 @@ public class AutoActions{
this.targetingSettings = tS;
this.turret = tur;
}
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
@@ -82,29 +83,6 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
@@ -181,8 +159,6 @@ public class AutoActions{
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
@@ -205,9 +181,6 @@ public class AutoActions{
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -218,18 +191,13 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
spindexer.setIntakePower(-0.1);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
@@ -287,9 +255,6 @@ public class AutoActions{
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
@@ -297,51 +262,30 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
spindexer.setIntakePower(-0.1);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
double prevSpinPos = servos.getSpinCmdPos();
teleStart = drive.localizer.getPose();
if (System.currentTimeMillis() - stamp < shootTime*1000 || (prevSpinPos < spinEndPos && shooterTicker < 2)) {
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (System.currentTimeMillis() - stamp < shootTime*1000) {
if (System.currentTimeMillis() - stamp < firstShootTime*1000) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker == 0) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
@@ -379,14 +323,12 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
private boolean detectingObelisk = false;
public Action detectObelisk(
double time,
double posX,
@@ -407,7 +349,7 @@ public class AutoActions{
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
detectingObelisk = true;
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
@@ -425,13 +367,9 @@ public class AutoActions{
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
teleStart = currentPose;
if (shouldFinish){
if (redAlliance){
@@ -439,6 +377,7 @@ public class AutoActions{
} else {
robot.limelight.pipelineSwitch(2);
}
detectingObelisk = false;
return false;
} else {
return true;
@@ -489,12 +428,8 @@ public class AutoActions{
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
teleStart = currentPose;
return !shouldFinish;
@@ -507,7 +442,8 @@ public class AutoActions{
double posX,
double posY,
double posXTolerance,
double posYTolerance
double posYTolerance,
boolean whileIntaking
) {
boolean timeFallback = (time != 0.501);
@@ -531,10 +467,10 @@ public class AutoActions{
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotX = currentPose.position.x;
double robotY = currentPose.position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double robotHeading = currentPose.heading.toDouble();
double goalX = -15;
double goalY = 0;
@@ -548,7 +484,7 @@ public class AutoActions{
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
turret.trackGoal(deltaPose);
if (!detectingObelisk){turret.trackGoal(deltaPose);}
servos.setHoodPos(targetingSettings.hoodAngle);
@@ -557,17 +493,16 @@ public class AutoActions{
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
boolean shouldFinish;
if (whileIntaking){
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
} else {
shouldFinish = timeDone || (xDone && yDone);
}
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
teleStart = currentPose;
return !shouldFinish;
@@ -632,12 +567,8 @@ public class AutoActions{
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
teleStart = currentPose;
return !shouldFinish;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -38,8 +38,8 @@ public class Flywheel {
}
// Set the robot PIDF for the next cycle.
private double prevF = 0.501;
public static int voltagePIDFDifference = 5;
private double prevF = 0;
public static double voltagePIDFDifference = 0.8;
public void setPIDF(double p, double i, double d, double f) {
shooterPIDF1.p = p;
shooterPIDF1.i = i;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -72,7 +74,7 @@ public class Servos {
public double setHoodPos(double pos){
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
robot.hood.setPosition(pos);
robot.hood.setPosition(pos + hoodOffset);
firstHoodPos = false;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -186,7 +186,7 @@ public class Targeting {
if (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
}
return recommendedSettings;
} else {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -24,7 +24,7 @@ public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.39;
public static double turrDefault = 0.37;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static boolean limelightUsed = true;