From d0ee294d265e6f82ff7e412140553e9ec687ae40 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 8 Jun 2026 22:29:36 -0500 Subject: [PATCH] night before --- .../autonomous/Auto12Ball_Back_Sorted.java | 1 - .../autonomous/Auto21Ball_Front_Gate.java | 77 +++++++++++-------- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV4.java | 9 ++- .../ftc/teamcode/tests/Hardware_Tester.java | 2 +- .../ftc/teamcode/tests/NewShooterTest.java | 2 +- .../ftc/teamcode/utilsv2/Flywheel.java | 44 +++++++++-- .../utilsv2/SpindexerTransferIntake.java | 19 ++--- .../ftc/teamcode/utilsv2/Turret.java | 4 +- .../teamcode/utilsv2/VelocityCommander.java | 6 +- 10 files changed, 107 insertions(+), 59 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index 9f1b731..5d6cc60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -393,7 +393,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()){ - robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed); park.unpark(); shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java index 7172845..e54c523 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java @@ -14,6 +14,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; @@ -39,13 +40,15 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { double runtime = 0; // Wait Times - public static double rapidWaitTime = 0.4; - public static double rapidShootTime = 0.45; - public static double openGate1Time = 1.5; - public static double openGate2Time = 1.5; - public static double openGateWaitTimeMax = 3; - public static double openGateWaitTimeMin = 1.75; - public static int maxLoopCycles = 3; + public static double rapidWaitTimeShoot0 = 0.2; + public static double rapidWaitTimeShoot1 = 0.2; + public static double rapidWaitTimeShoot2 = 0.2; + public static double rapidWaitTimeShootGate = 0.4; + public static double rapidShootTime = 0.4; + public static double openGate1Time = 1.8; + public static double openGate2Time = 1; + public static double openGateWaitTimeMax = 3.5; + public static int maxLoopCycles = 4; // Initialize path state machine private enum PathState { @@ -70,11 +73,12 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { public static double openGate2ControlX = 45.9782359679267, openGate2ControlY = -15.106643757159245; public static double openGate2X = 57, openGate2Y = -8, openGate2H = 0; public static double shoot2ControlX = 57, shoot2ControlY = -8; - public static double shoot2X = 25, shoot2Y = 11, shoot2H = -30; - public static double intakeGateControlX = 61, intakeGateControlY = -11; - public static double intakeGateX = 61, intakeGateY = -11, intakeGateH = 20; - public static double shootGateControlX = 56, shootGateControlY = -10; - public static double shootGateX = 25, shootGateY = 11, shootGateH = -30; + public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30; + public static double intakeGateControlX = 60, intakeGateControlY = -12; + public static double toGateX = 60, toGateY = -10; + public static double intakeGateX = 62, intakeGateY = -12.5, intakeGateH = 25; + public static double shootGateControlX = 40, shootGateControlY = -5; + public static double shootGateX = 16, shootGateY = 4, shootGateH = -30; public static double shootLeaveControlX = 56, shootLeaveControlY = -10; public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50; public static double leaveX = 45, leaveY = 10, leaveH = 0; @@ -83,22 +87,22 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X, pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X, intakeGateControlX, intakeGateX, shootGateControlX, shootGateX, - shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX}; + shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX, toGateX}; double[] yPoses = {startPoseY, shoot0Y, pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y, pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y, intakeGateControlY, intakeGateY, shootGateControlY, shootGateY, - shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY}; + shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY, toGateY}; double[] headings = {startPoseH, shoot0H, 0, pickup1H, 0, openGate1H, 0, shoot1H, 0, pickup2H, 0, openGate2H, 0, shoot2H, 0, intakeGateH, 0, shootGateH, - 0, shootLeaveH, leaveH, awayFromGateH}; + 0, shootLeaveH, leaveH, awayFromGateH, 0}; Pose startPose, shoot0, pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1, pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2, intakeGateControl, intakeGate, shootGateControl, shootGate, - shootLeaveControl, shootLeave, leave, awayFromGate; + shootLeaveControl, shootLeave, leave, awayFromGate, toGate; private void initializePoses(){ startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0])); shoot0 = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1])); @@ -122,12 +126,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { shootLeave = new Pose(xPoses[19], yPoses[19], Math.toRadians(headings[19])); leave = new Pose(xPoses[20], yPoses[20], Math.toRadians(headings[20])); awayFromGate = new Pose(xPoses[21], yPoses[21], Math.toRadians(headings[21])); + toGate = new Pose(xPoses[22], yPoses[22]); } //Building Paths PathChain startPose_shoot0, shoot0_pickup1, pickup1_openGate1, openGate1_shoot1, - shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, - shoot2_intakeGate, intakeGate_shootGate, shootGate_intakeGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate; + shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, shootGate_intakeGate, + shoot2_intakeGate, intakeGate_shootGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate; private void buildPaths(){ startPose_shoot0 = follower.pathBuilder() .addPath(new BezierLine(startPose, shoot0)) @@ -160,27 +165,29 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { .build(); openGate2_shoot2 = follower.pathBuilder() - .addPath(new BezierCurve(openGate2, shoot2Control, shoot2)) - .setLinearHeadingInterpolation(openGate2.getHeading(), shoot2.getHeading()) + .addPath(new BezierLine(openGate2, shoot2)) + .setTangentHeadingInterpolation() + .setReversed() .build(); shoot2_intakeGate = follower.pathBuilder() - .addPath(new BezierCurve(shoot2, intakeGateControl, intakeGate)) + .addPath(new BezierCurve(shoot2, intakeGateControl, toGate)) .setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading()) .build(); intakeGate_shootGate = follower.pathBuilder() .addPath(new BezierCurve(intakeGate, shootGateControl, shootGate)) - .setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading()) + .setTangentHeadingInterpolation() + .setReversed() .build(); shootGate_intakeGate = follower.pathBuilder() - .addPath(new BezierCurve(shootGate, intakeGateControl, intakeGate)) + .addPath(new BezierLine(shootGate, intakeGate)) .setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading()) .build(); intakeGate_shootLeave = follower.pathBuilder() - .addPath(new BezierCurve(intakeGate, shootLeaveControl, shootLeave)) + .addPath(new BezierCurve(intakeGate, pickup2Control, shootLeave)) .setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading()) .build(); @@ -199,6 +206,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { private boolean startAuto = true; private double timeStamp = 0; private int cycle = 0; + boolean toGateBool = false; private void pathStateMachine(){ double currentTime = (double) System.currentTimeMillis() / 1000; switch(pathState){ @@ -210,11 +218,11 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { follower.followPath(startPose_shoot0, false); startAuto = false; } - if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ + if ((follower.atParametricEnd() || !follower.isBusy()) && currentTime - timeStamp > rapidWaitTimeShoot0){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT0; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); - } else if (follower.isBusy()){ + } else if (follower.isBusy() && !follower.atParametricEnd()){ timeStamp = currentTime; } break; @@ -241,7 +249,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { } break; case DRIVE_SHOOT1: - if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ + if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot1){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT1; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); @@ -259,6 +267,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { break; case PICKUP2: if (!follower.isBusy()){ + shooter.setFlywheelVelocity(2500); + robot.setHoodPos(0.6); follower.followPath(pickup2_openGate2, false); pathState = PathState.OPENGATE2; timeStamp = currentTime; @@ -272,7 +282,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { } break; case DRIVE_SHOOT2: - if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ + if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot2){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT2; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); @@ -287,17 +297,19 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { follower.followPath(shoot2_intakeGate, false); pathState = PathState.INTAKE_GATE; timeStamp = currentTime; + toGateBool = true; } break; case INTAKE_GATE: - if ((currentTime - timeStamp > openGateWaitTimeMax || (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed())) - && (currentTime - timeStamp > openGateWaitTimeMin)){ + if ((currentTime - timeStamp > openGateWaitTimeMax)){ if (getRuntime() - runtime > 27){ follower.followPath(intakeGate_awayFromGate, true); pathState = PathState.WAIT_SHOOT_LEAVE; } else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){ follower.followPath(intakeGate_shootLeave, true); pathState = PathState.DRIVE_SHOOT_LEAVE; + shooter.setFlywheelVelocity(2300); + robot.setHoodPos(0.68); } else { follower.followPath(intakeGate_shootGate, false); pathState = PathState.DRIVE_SHOOT_GATE; @@ -307,7 +319,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { // TODO: add logic to shoot gate break; case DRIVE_SHOOT_GATE: - if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ + if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT_GATE; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); @@ -326,12 +338,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode { } else { follower.followPath(shootGate_intakeGate, false); pathState = PathState.INTAKE_GATE; + toGateBool = true; } timeStamp = currentTime; } break; case DRIVE_SHOOT_LEAVE: - if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ + if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){ timeStamp = currentTime; pathState = PathState.WAIT_SHOOT_LEAVE; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 10b2e4d..ad22547 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -37,7 +37,7 @@ public class ServoPositions { public static double transferServo_out = 0.28; - public static double transferServo_in = 0.54; + public static double transferServo_in = 0.52; public static double hoodAuto = 0.27; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 07a9cd8..65b6d7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -133,6 +133,7 @@ public class TeleopV4 extends LinearOpMode { follower.update(); Pose currentPose = follower.getPose(); + teleStart = currentPose; if (gamepad1.dpadLeftWasPressed()){ shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); @@ -259,9 +260,9 @@ public class TeleopV4 extends LinearOpMode { } if (gamepad2.dpadRightWasPressed()){ - flywheelOffset+=100; + flywheelOffset+=50; } else if (gamepad2.dpadLeftWasPressed()){ - flywheelOffset-=100; + flywheelOffset-=50; } if (gamepad2.crossWasPressed()){ @@ -279,6 +280,10 @@ public class TeleopV4 extends LinearOpMode { // TELE.addData("Transfer Power", robot.transfer.getPower()); TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + TELE.addData("Velocity 1", flywheel.getVelo1()); + TELE.addData("Velocity 2", flywheel.getVelo2()); + TELE.addData("Flywheel Offset", flywheelOffset); + TELE.addData("Hood Offset", hoodOffset); // // TELE.addData("Current Position", currentPose); // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index 0dc9c5e..c2f01d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -131,7 +131,7 @@ public class Hardware_Tester extends LinearOpMode { NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM)); - TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red)); + TELE.addData("REV Green", revColor.green / (revColor.green + revColor.blue + revColor.red)); TELE.addData("Voltage Sensor", robot.voltage.getVoltage()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 1cfe2c5..7954ca5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode { private boolean shooting = false; public static int flywheelVelo = 0; public static double hoodPos = 0.5; - public static double transferPower = -0.8; + public static double transferPower = -1; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index 4d64b52..d22f807 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -83,11 +83,15 @@ public class Flywheel { // pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0); - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + if (velo1 != 0){ + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } else { + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } + } private double prevF = 0; - public static double voltagePIDFDifference = 1; double averageVoltage = 0; public void setF(double voltage){ @@ -95,7 +99,11 @@ public class Flywheel { double f = shooterPIDF_F / voltage; if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) { shooterPIDF.f = f; - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + if (velo1 != 0){ + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } else { + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } } prevF = f; } @@ -134,21 +142,43 @@ public class Flywheel { double power; double prevTargetTime = 0; double prevTargetVelocity = 0; + int veloMode = 0; public void manageFlywheel(double commandedVelocity) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { targetVelocity = commandedVelocity; } - robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - power = robot.shooter1.getPower(); - robot.shooter2.setPower(power); + if (velo1 < 100){ + } velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = velo1; + if (velo1 < 100){ + velo1 = 0; + velo = velo2; + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + if (veloMode != 2){ + robot.shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + veloMode = 2; + } + power = robot.shooter2.getPower(); + robot.shooter1.setPower(power); + } else { + velo2 = 0; + velo = velo1; + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + if (veloMode != 1){ + robot.shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + veloMode = 1; + } + power = robot.shooter1.getPower(); + robot.shooter2.setPower(power); + } updateVelocityAverage(velo); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index cc2de76..324f965 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -53,8 +53,8 @@ public class SpindexerTransferIntake { } int[] shootOrder = {0, 1, 2}; - final double sensorDistanceThreshold = 5.3; - final long pulseTime = 100; // ms + public static final double sensorDistanceThreshold = 5.35; + final long pulseTime = 70; // ms private DesiredPattern desiredPattern = DesiredPattern.GPP; @@ -325,7 +325,7 @@ public class SpindexerTransferIntake { robot.setSpinPos( ServoPositions.spindexer_A2 ); - robot.setTransferPower(-0.7); + robot.setTransferPower(-1); robot.setTransferServoPos( ServoPositions.transferServo_out ); @@ -335,7 +335,7 @@ public class SpindexerTransferIntake { holdBallsTicker++; } - if (holdBallsTicker > 10){ + if (holdBallsTicker > 20){ setRapidMode(RapidMode.TRANSFER_OFF); holdBallsTicker = 0; } @@ -355,7 +355,7 @@ public class SpindexerTransferIntake { robot.setIntakePower(1.0); - if (stateTime() >= 300) { + if (stateTime() >= 700) { setRapidMode(RapidMode.PULSE_OUT); } @@ -375,25 +375,23 @@ public class SpindexerTransferIntake { robot.setIntakePower(1.0); - if (stateTime() >= 200) { + if (stateTime() >= 400) { setRapidMode(RapidMode.HOLD_BALLS); } - break; case HOLD_BALLS: if (robot.insideBeam.isPressed() - && robot.outsideBeam.isPressed()) { + && robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { robot.setTransferPower(0); robot.setIntakePower(0.1); robot.setTransferServoPos(ServoPositions.transferServo_in); } else { - holdBallsTicker++; - robot.setIntakePower(1); + setRapidMode(RapidMode.INTAKE); } break; @@ -679,7 +677,6 @@ public class SpindexerTransferIntake { robot.setTransferPower(-0.8); } - switch (shootState) { case IDLE: shootOrder = buildShootOrder( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index e09ed8d..241e079 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -17,8 +17,8 @@ public class Turret { private final double servoTicksPer180 = 0.58; public static double neutralPosition = 0.51; - private final double turretMin = 0.05; - private final double turretMax = 0.95; + private final double turretMin = 0.08; + private final double turretMax = 0.91; public static boolean limelightUsed = true; public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005; LLResult result; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java index eb15989..4d7e748 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -67,6 +67,10 @@ public class VelocityCommander { hoodC*Math.pow(dist, 1) + hoodD; } + if (lockBack){ + hoodPos-=0.04; + } + hoodPos = Math.max(0.48, Math.min(0.88, hoodPos)); } public double getHoodPredicted(){ @@ -75,7 +79,7 @@ public class VelocityCommander { private void distToTransferPow(double dist, double voltage){ if (dist < 140){ - transferPow = -0.75; + transferPow = -1; } else { transferPow = -0.6; }