From d0c34132dec9c1304af3bd98cfb7d72270dfecc9 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 14 Feb 2026 15:39:03 -0600 Subject: [PATCH] remodeled close auto and it works except for poses (@KeshavAnandCode that is your job --- .../teamcode/autonomous/Auto_LT_Close.java | 232 ++++++++---------- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 33 ++- .../autonomous/actions/AutoActions.java | 127 +++------- .../ftc/teamcode/utils/Flywheel.java | 4 +- .../ftc/teamcode/utils/Servos.java | 4 +- .../ftc/teamcode/utils/Targeting.java | 2 +- .../ftc/teamcode/utils/Turret.java | 2 +- 7 files changed, 162 insertions(+), 242 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index d6520b8..0130863 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -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) - ) - - ); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index d521854..039fa13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -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(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 57611a8..3f856d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index b5e017f..3dd8100 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index c66ff79..2aed644 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 0ca44b2..9d46f99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -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 { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 61ac3d1..d9ed048 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -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;