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

auto actions class tested: works but needs remodeling

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-12 21:26:23 -06:00
parent b616a41a08
commit 04ea56e31d
4 changed files with 20 additions and 614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,25 +3,13 @@ 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.spinEndPos;
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_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
@@ -40,17 +28,15 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@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.02;
public static double shoot0SpinSpeedIncrease = 0.005;
public static double spindexerSpeedIncrease = 0.03;
public static double finalSpindexerSpeedIncrease = 0.03;
public static double spindexerSpeedIncrease = 0.005;
public static double finalSpindexerSpeedIncrease = 0.005;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -111,8 +97,6 @@ public class Auto_LT_Close extends LinearOpMode {
Targeting targeting;
Targeting.Settings targetingSettings;
AutoActions autoActions;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
double x1, y1, h1;
double x2a, y2a, h2a, t2a;
@@ -132,11 +116,6 @@ public class Auto_LT_Close extends LinearOpMode {
private double shoot1Tangent;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
int ballCycles = 3;
int prevMotif = 0;
@@ -170,8 +149,6 @@ public class Auto_LT_Close extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);

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

@@ -31,6 +31,7 @@ import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -64,6 +65,7 @@ public class Auto_LT_Far extends LinearOpMode {
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
AutoActions autoActions;
double firstSpindexShootPos = autoSpinStartPos;
boolean shootForward = true;
double xShoot, yShoot, hShoot;
@@ -83,588 +85,7 @@ public class Auto_LT_Far extends LinearOpMode {
public static int pickupStackSpeed = 15;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spindexerWiggle = 0.01;
boolean decideGreenSlot = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
servos.setTransferPos(transferServo_out);
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();
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;
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
decideGreenSlot = true;
return true;
} else if (decideGreenSlot) {
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
mostGreenSlot = 3;
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
mostGreenSlot = 2;
} else {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
}
}
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);
return true;
} else {
return false;
}
}
};
}
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) {
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);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
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();
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 ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
servos.setSpinPos(autoSpinStartPos);
} 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;
}
}
};
}
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = 0.0;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
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();
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 (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) {
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;
} else {
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.processIntake();
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
public Action detectObelisk(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance,
double turrPos
) {
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();
robot.limelight.pipelineSwitch(1);
}
ticker++;
motif = turret.detectObelisk();
turret.setTurret(turrPos);
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};
}
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
public Action manageShooterAuto(
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);
turret.trackGoal(deltaPose);
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
@@ -700,6 +121,8 @@ public class Auto_LT_Far extends LinearOpMode {
drive = new MecanumDrive(hardwareMap, autoStart);
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
servos.setSpinPos(autoSpinStartPos);
servos.setTransferPos(transferServo_out);
@@ -850,7 +273,7 @@ public class Auto_LT_Far extends LinearOpMode {
void startAuto(){
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
autoActions.manageFlywheel(
shoot0Vel,
shoot0Hood,
flywheel0Time,
@@ -864,7 +287,7 @@ public class Auto_LT_Far extends LinearOpMode {
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}

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

@@ -15,6 +15,7 @@ import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
@@ -30,6 +31,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Config
public class AutoActions{
Robot robot;
MultipleTelemetry TELE;
@@ -246,7 +248,7 @@ public class AutoActions{
turret.trackGoal(deltaPose);
if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos);
@@ -325,9 +327,9 @@ public class AutoActions{
turret.trackGoal(deltaPose);
if (System.currentTimeMillis() - stamp < shootTime) {
if (System.currentTimeMillis() - stamp < shootTime*1000) {
if (System.currentTimeMillis() - stamp < firstShootTime) {
if (System.currentTimeMillis() - stamp < firstShootTime*1000) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {

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

@@ -81,12 +81,16 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
private double prevTurrPos = 0.501;
private double prevTurrPos = 0;
private boolean isFirstTurretPos = true;
public void setTurret(double pos) {
if (prevTurrPos != 0.501 && prevTurrPos != pos){
if (isFirstTurretPos || prevTurrPos != pos){
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
isFirstTurretPos = false;
}
TELE.addLine("Moved Turret");
TELE.update();
prevTurrPos = pos;
}