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

far auto in development part 2

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-15 15:47:36 -06:00
parent 78c65c9d93
commit 1c3100966c
5 changed files with 35 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
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.005;
public static double spindexerSpeedIncrease = 0.008;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -167,8 +167,8 @@ public class Auto_LT_Close extends LinearOpMode {
}
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(1);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
@@ -336,6 +336,7 @@ public class Auto_LT_Close extends LinearOpMode {
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
robot.transfer.setPower(0);
TELE.addLine("finished");
TELE.update();

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

@@ -72,7 +72,7 @@ public class Auto_LT_Far extends LinearOpMode {
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 12;
int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.005;
public static double spindexerSpeedIncrease = 0.008;
public static double shootAllTime = 2;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
@@ -131,12 +131,6 @@ public class Auto_LT_Far extends LinearOpMode {
while (opModeInInit()) {
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
@@ -187,6 +181,12 @@ public class Auto_LT_Far extends LinearOpMode {
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(4);
robot.limelight.start();
gamepad2.rumble(500);
}
} else {
robot.light.setPosition(0.6);
@@ -214,6 +214,12 @@ public class Auto_LT_Far extends LinearOpMode {
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(2);
robot.limelight.start();
gamepad2.rumble(500);
}
}
leave3Ball = drive.actionBuilder(autoStart)
@@ -274,6 +280,7 @@ public class Auto_LT_Far extends LinearOpMode {
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
robot.transfer.setPower(0);
TELE.addLine("finished");
TELE.update();

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

@@ -193,77 +193,6 @@ public class AutoActions{
};
}
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = vel;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.1);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
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 && servos.getSpinPos() < 0.85) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
@@ -385,7 +314,7 @@ public class AutoActions{
if (ticker == 0) {
stamp = System.currentTimeMillis();
robot.limelight.pipelineSwitch(1);
turret.pipelineSwitch(1);
}
ticker++;
@@ -403,9 +332,9 @@ public class AutoActions{
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
turret.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
turret.pipelineSwitch(2);
}
detectingObelisk = false;
return false;
@@ -417,56 +346,6 @@ public class AutoActions{
};
}
public Action manageFlywheel(
double vel,
double hoodPos,
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(hoodPos);
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 shouldFinish = timeDone || xDone || yDone;
teleStart = currentPose;
return !shouldFinish;
}
};
}
public Action manageShooterAuto(
double time,
double posX,
@@ -511,7 +390,7 @@ 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;
if (posX != 0.501){
if (posX != 0.501) {
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
} else {
deltaPose = new Pose2d(robotX, robotY, robotHeading);
@@ -522,7 +401,7 @@ public class AutoActions{
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
if (!detectingObelisk){
if (!detectingObelisk) {
turret.trackGoal(deltaPose);
}
@@ -536,7 +415,7 @@ public class AutoActions{
boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
boolean shouldFinish;
if (whileIntaking){
if (whileIntaking) {
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
} else {
shouldFinish = timeDone || (xDone && yDone) || doneShooting;
@@ -547,7 +426,7 @@ public class AutoActions{
TELE.addData("Steady?", flywheel.getSteady());
TELE.update();
if (shouldFinish){
if (shouldFinish) {
doneShooting = false;
return false;
} else {
@@ -557,72 +436,6 @@ public class AutoActions{
}
};
}
public Action manageFlywheelAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().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 = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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 shouldFinish = timeDone || xDone || yDone;
teleStart = currentPose;
return !shouldFinish;
}
};
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -116,10 +116,10 @@ public class TeleopV3 extends LinearOpMode {
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
robot.limelight.pipelineSwitch(4);
turret.pipelineSwitch(4);
light.setManualLightColor(Color.LightRed);
} else {
robot.limelight.pipelineSwitch(2);
turret.pipelineSwitch(2);
light.setManualLightColor(Color.LightBlue);
}

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

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import java.util.List;
@@ -48,6 +49,7 @@ public class Turret {
double limelightPosX = 0.0;
double limelightPosY = 0.0;
LLResult result;
boolean bearingAligned = false;
private boolean lockOffset = false;
private int obeliskID = 0;
@@ -56,6 +58,7 @@ public class Turret {
private double lightColor = Color.LightRed;
private int currentTrackCount = 0;
private double permanentOffset = 0.0;
private int prevPipeline = -1;
private PIDController bearingPID;
private double prevTurretPos = 0.0;
@@ -91,6 +94,12 @@ public class Turret {
}
prevTurrPos = pos;
}
public void pipelineSwitch(int pipeline){
if (prevPipeline != pipeline){
robot.limelight.pipelineSwitch(pipeline);
}
prevPipeline = pipeline;
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;