diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java index c443448..0ed240a 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java @@ -35,12 +35,10 @@ public class hwMap { } public static class LedHwMap { - public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1; - public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2; + public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led; public LedHwMap(HardwareMap hardwareMap) { - /*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1"); - led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/ + //led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED); } } @@ -89,7 +87,7 @@ public class hwMap { imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP ) ); @@ -250,7 +248,7 @@ public class hwMap { private double lastKnownDistance = 0.0; private ElapsedTime noTargetTimer = new ElapsedTime(); - PIDFCoefficients pidfCoefficients = new PIDFCoefficients(22.5, 0, 0.001, 15.6); + PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15); public TurretHwMap(HardwareMap hardwareMap) { noTargetTimer.reset(); @@ -283,12 +281,31 @@ public class hwMap { initLimelight(hardwareMap); } - public void setPIDF(double p, double i, double d, double f) { - PIDFCoefficients pidf = new PIDFCoefficients(p, i, d, f); - turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); - turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + public void resetTurret() { + turretrotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } + public void setPIDF(double p, double i, double d, double f) { + PIDFCoefficients PIDF = new PIDFCoefficients(p, i, d, f); + turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF); + turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF); + } + + + public void enableTurretHold() { + turretrotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + public void disableTurretHold() { + turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void runToPos(int target) { + turretrotation.setTargetPosition(target); + } + + public double getTurretRotationPosition() { return turretrotation.getCurrentPosition(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java index 535128a..408af94 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java @@ -517,6 +517,8 @@ public class Auton { hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap); hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap); + turretHw.setPIDF(120, 0, 10, 15); + systems = new pedroStateMachine(intakeHw, transferHw, turretHw); systems.setAlliance(allianceTarget); @@ -531,13 +533,16 @@ public class Auton { telemetry.update(); } + follower.update(); + + turretHw.resetTurret(); + waitForStart(); systems.update(allianceTarget, follower.getPose()); opmodeTimer.resetTimer(); setPathState(PathState.DRIVE_TO_SCORE_PRELOAD); while (opModeIsActive()) { - for (LynxModule hub : allHubs) { hub.clearBulkCache(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 311cd3a..5cdf47a 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -25,7 +25,7 @@ public class Constants { .lateralZeroPowerAcceleration(-76.57271150137258); public static MecanumConstants driveConstants = new MecanumConstants() - .maxPower(0.8) + .maxPower(0.7) .xVelocity(68.10320673181904) .yVelocity(57.52038399624941) .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR) @@ -41,15 +41,15 @@ public class Constants { public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants() - .forwardTicksToInches(0.001978956) - .strafeTicksToInches(0.001978956) + .forwardTicksToInches(0.0020041908950982315) + .strafeTicksToInches(0.002004094712407555) .turnTicksToInches(0.0022824233082645137) .leftPodY(3.75) .rightPodY(-3.25) .strafePodX(-6.25) .leftEncoder_HardwareMapName("intake") .rightEncoder_HardwareMapName("fl") - .strafeEncoder_HardwareMapName("bl") + .strafeEncoder_HardwareMapName("fr") .leftEncoderDirection(Encoder.FORWARD) .rightEncoderDirection(Encoder.FORWARD) .strafeEncoderDirection(Encoder.FORWARD); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java index 59e6a46..3cf091a 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java @@ -1,76 +1,56 @@ package org.firstinspires.ftc.teamcode.subsystems; +import org.firstinspires.ftc.robotcore.external.Const; import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.Prism.Color; import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight; import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; +import org.firstinspires.ftc.teamcode.teleOp.Constants; public class LED { private final hwMap.LedHwMap hardware; // Track states to prevent spamming the I2C bus - private int currentLed1State = -1; - private int currentLed2State = -1; + private int currentLedState = -1; // Reusable animations for efficiency private final PrismAnimations.Solid solidRed = new PrismAnimations.Solid(Color.RED); private final PrismAnimations.Solid solidOrange = new PrismAnimations.Solid(Color.ORANGE); + private final PrismAnimations.Solid solidYellow = new PrismAnimations.Solid(Color.YELLOW); private final PrismAnimations.Solid solidGreen = new PrismAnimations.Solid(Color.GREEN); - private final PrismAnimations.Solid solidBlue = new PrismAnimations.Solid(Color.BLUE); - private final PrismAnimations.Solid solidPurple = new PrismAnimations.Solid(Color.PURPLE); // or MAGENTA if PURPLE is missing public LED(hwMap.LedHwMap hardware) { this.hardware = hardware; - solidRed.setBrightness(50); - solidOrange.setBrightness(50); - solidGreen.setBrightness(50); - solidBlue.setBrightness(50); - solidPurple.setBrightness(50); + int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS; + solidRed.setBrightness(LED_BRIGHTNESS); + solidOrange.setBrightness(LED_BRIGHTNESS); + solidYellow.setBrightness(LED_BRIGHTNESS); + solidGreen.setBrightness(LED_BRIGHTNESS); } - public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) { - /* - int newLed1State = 0; - if (isScoring) { - if (isTurretReady) { - newLed1State = 2; - } else { - newLed1State = 1; - } + public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) { + /*int newLedState = 0; + + if (isTurretLocked && isFlywheelUpToSpeed) { + newLedState = 1; + } else if (isTurretLocked) { + newLedState = 2; + } else if (isFlywheelUpToSpeed) { + newLedState = 3; } - if (newLed1State != currentLed1State) { - if (newLed1State == 0) { - hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed); - } else if (newLed1State == 1) { - hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange); - } else if (newLed1State == 2) { - hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen); + if (newLedState != currentLedState) { + if (newLedState == 0) { + hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed); + } else if (newLedState == 1) { + hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen); + } else if (newLedState == 2) { + hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange); + } else if (newLedState == 3) { + hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow); } - currentLed1State = newLed1State; - } - - int newLed2State = 0; - if (hasTarget && isAnchorActive) { - newLed2State = 3; - } else if (hasTarget) { - newLed2State = 1; - } else if (isAnchorActive) { - newLed2State = 2; - } - - if (newLed2State != currentLed2State) { - if (newLed2State == 0) { - hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed); - } else if (newLed2State == 1) { - hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen); - } else if (newLed2State == 2) { - hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue); - } else if (newLed2State == 3) { - hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple); - } - currentLed2State = newLed2State; + currentLedState = newLedState; }*/ } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java index b84b382..22b9d86 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystems; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.Hware.hwMap; +import org.firstinspires.ftc.teamcode.teleOp.Constants; public class TransferSys { private final hwMap.TransferHwMap hardware; @@ -9,7 +10,7 @@ public class TransferSys { private boolean isTurretReady = false; private boolean limiterClosed = true; - public double launchDuration = 2; // CHANGEABLE TODO + public double launchDuration = Constants.TransferConstants.launch_duration; // CHANGEABLE TODO public enum TransferState { LAUNCHING, // Gate OPEN @@ -94,6 +95,13 @@ public class TransferSys { return limiterClosed; } + public double getLaunchElapsedTime() { + if (currentState == TransferState.LAUNCHING && isLaunchSequenceActive) { + return timer.seconds(); + } + return 100.0; + } + public TransferState getTransferState() { return currentState; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java index 619b410..b355c41 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java @@ -1,8 +1,5 @@ package org.firstinspires.ftc.teamcode.subsystems; -import static org.firstinspires.ftc.teamcode.tuning.allInOne.OFFSET_GAIN; -import static org.firstinspires.ftc.teamcode.tuning.allInOne.SERVO_MIN; - import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.robotcore.util.ElapsedTime; @@ -33,7 +30,14 @@ public class Turret { private Pose robotPose; + public double targetTick; + private final ElapsedTime loopTimer = new ElapsedTime(); + private final ElapsedTime stateTimer = new ElapsedTime(); + + public boolean turretModeHold = false; + + public boolean lastTurretModeHold = false; public enum TurretState { LAUNCH, @@ -45,17 +49,23 @@ public class Turret { public Turret(hwMap.TurretHwMap hardware) { this.hardware = hardware; + offsetTicks = 0; // Reset manual offset on every init + targetCorrectionOffsetTicks = 0; // Reset vision correction loopTimer.reset(); + stateTimer.reset(); } public void setAlliance(int alliance) { - this.alliance = alliance; - if (alliance == 2) { - hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET); - } else if(alliance == 1) { - hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET); - } else { - hardware.setPipeline(0); + if (this.alliance != alliance) { + this.alliance = alliance; + targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes + if (alliance == 2) { + hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET); + } else if (alliance == 1) { + hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET); + } else { + hardware.setPipeline(0); + } } } @@ -68,21 +78,24 @@ public class Turret { } public void setTurretState(TurretState state) { - this.currentState = state; + if (this.currentState != state) { + this.currentState = state; + stateTimer.reset(); - switch (state) { - case IDLE: - hardware.turretOff(); - hardware.stopTurretRotation(); - integralSum = 0; - lastError = 0; - break; - case EXTAKE: - hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER); - break; - case INTAKING: - hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER); - break; + switch (state) { + case IDLE: + hardware.turretOff(); + hardware.stopTurretRotation(); + integralSum = 0; + lastError = 0; + break; + case EXTAKE: + hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER); + break; + case INTAKING: + hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER); + break; + } } } @@ -95,7 +108,7 @@ public class Turret { return hardware.hasTarget(); } - public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) { + public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) { double dt = loopTimer.seconds(); loopTimer.reset(); if (dt < 0.001) dt = 0.001; @@ -115,8 +128,18 @@ public class Turret { } if (currentState == TurretState.LAUNCH) { - handleLaunchLogic(); + handleLaunchLogic(elapsedTime); } + else { + // idleLaunch(); + } + } + + public void idleLaunch() { + hardware.setTurretVelocity(1000); + + double calculatedHoodPos = getHoodPOS(65); + setHoodPos(calculatedHoodPos); } public void updateAuton(int alliance, Pose currentPose, double offset) { @@ -127,6 +150,11 @@ public class Turret { setAlliance(alliance); + if (currentState == TurretState.IDLE) { + hardware.stopTurretRotation(); + return; + } + double rawTicks = hardware.getTurretRotationPosition(); double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE; @@ -134,38 +162,63 @@ public class Turret { calculateHybridTarget(currentPose, currentDegrees, rawTicks); runPIDControl(rawTicks, dt); - handleLaunchLogic(-offset); + handleLaunchLogicOffset(-offset); } - private void handleLaunchLogic() { + private void handleLaunchLogic(double elapsedTime) { if (distance > 0) { - targetVelocity = getFlywheelVelocity(distance); + double baseVelocity = getFlywheelVelocity(distance); + double t = stateTimer.seconds(); + + /* TUNING GUIDE: + * 1. Adjust ARTIFACT_1_WINDOW and ARTIFACT_2_WINDOW in Constants.java to match your launch sequence timing. + * 2. Adjust ARTIFACT_OFFSET_LOW to lower the velocity for the 1st and 3rd artifacts. + * 3. ARTIFACT_OFFSET_NORMAL (usually 0) is for the 2nd artifact. + */ + double compensation; + if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) { + compensation = Constants.TurretConstants.ARTIFACT_1; + } else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) { + compensation = Constants.TurretConstants.ARTIFACT_2; + } else { + compensation = Constants.TurretConstants.ARTIFACT_3; + } + + targetVelocity = baseVelocity + compensation; hardware.setTurretVelocity(targetVelocity); double calculatedHoodPos = getHoodPOS(distance); - double currentVel = hardware.getFlywheelVelocities()[0]; - double dropThreshold = targetVelocity * 0.95; - double recoilOffset = 0.0; + double currentVel = hardware.getFlywheelVelocities()[1]; - recoilOffset = (currentVel - targetVelocity) * 0.000004; + double recoilOffset = (currentVel - targetVelocity) * -0.000185; - setHoodPos(calculatedHoodPos + recoilOffset); + setHoodPos(calculatedHoodPos + recoilOffset - 0.03); } else { hardware.setTurretVelocity(0); } } - private void handleLaunchLogic(double offset) { + private void handleLaunchLogicOffset(double offset) { if (distance > 0) { - targetVelocity = getFlywheelVelocity(distance); + double baseVelocity = getFlywheelVelocity(distance); + double t = stateTimer.seconds(); + + double compensation; + if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) { + compensation = Constants.TurretConstants.ARTIFACT_1; + } else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) { + compensation = Constants.TurretConstants.ARTIFACT_2; + } else { + compensation = Constants.TurretConstants.ARTIFACT_3; + } + + targetVelocity = baseVelocity + compensation; hardware.setTurretVelocity(targetVelocity); double calculatedHoodPos = getHoodPOS(distance); - double currentVel = hardware.getFlywheelVelocities()[0]; - double dropThreshold = targetVelocity * 0.95; - double recoilOffset = 0.0; + double currentVel = hardware.getFlywheelVelocities()[1]; - recoilOffset = (currentVel - targetVelocity) * 0.000004; + double recoilOffset = (currentVel - targetVelocity) * -0.000185; setHoodPos(calculatedHoodPos + recoilOffset); } else { @@ -178,13 +231,36 @@ public class Turret { LLResult result = hardware.getLimelightResult(); if (result != null && result.isValid()) { - double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks); - targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING)) - + (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING); + // 1. Calculate Vision Truth: Where the goal sits based on the camera right now + double tx = result.getTx(); + double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER); + double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES; + double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE; + + // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset) + double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks; + + double correctionError = visionTargetTicks - currentFusedTarget; + + // 4. Normalize the error for wrapping (360 degrees) + double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE; + while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev; + while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev; + + // 5. Apply the error to the persistent offset (Bias-Shift) + targetCorrectionOffsetTicks += (correctionError * Constants.TurretConstants.OFFSET_SMOOTHING); } double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks; + + double fullWrapTicks = Constants.TurretConstants.TURRET_FULL_WRAP_TICKS; + double halfWrapTicks = fullWrapTicks / 2.0; + + if (rawTarget > halfWrapTicks) {rawTarget -= fullWrapTicks;} + else if (rawTarget < -halfWrapTicks) {rawTarget += fullWrapTicks;} + fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS); + targetTick = fusedTargetTicks; } private double calculateOdometryTargetTicks(Pose robotPose) { @@ -208,42 +284,53 @@ public class Turret { return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE; } - private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) { - double tx = result.getTx(); - double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER); - double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES; - double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE; - - double rawErrorTicks = visionTargetTicks - odoTargetTicks; - double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE; - - while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev; - while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev; - return rawErrorTicks; - } - private void runPIDControl(double currentTicks, double dt) { fusedTargetTicks = fusedTargetTicks + offsetTicks; double error = fusedTargetTicks - currentTicks; double derivative = (error - lastError) / dt; - if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) { - integralSum += (error * dt); - } else { - integralSum = 0; + // Hysteresis thresholds for mode switching + double entryThreshold = 15; // Enter hold mode if error is within this range + double exitThreshold = 30; // Exit hold mode if error exceeds this range + + if (!turretModeHold && Math.abs(error) <= entryThreshold) { + turretModeHold = true; + } else if (turretModeHold && Math.abs(error) > exitThreshold) { + turretModeHold = false; } - double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL); - double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative); - output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER); + if (turretModeHold) { + if (lastTurretModeHold != turretModeHold) { + hardware.runToPos((int) fusedTargetTicks); // Set target first + hardware.enableTurretHold(); // Then set mode + hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power + } + hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode + } + else { + if (lastTurretModeHold != turretModeHold) { + hardware.disableTurretHold(); + } - hardware.setTurretRotationPower(output); - turretPowerRotation = output; + if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) { + integralSum += (error * dt); + } else { + integralSum = 0; + } + + double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL); + double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative); + output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER); + + hardware.setTurretRotationPower(output); + turretPowerRotation = output; + } + lastTurretModeHold = turretModeHold; lastError = error; } public boolean hasReachedVelocity() { - double currentVel = hardware.getFlywheelVelocities()[0]; + double currentVel = hardware.getFlywheelVelocities()[1]; if (Math.abs(targetVelocity) > 1000) { double absCurrent = Math.abs(currentVel); @@ -258,7 +345,7 @@ public class Turret { } public boolean isTurretReady() { - return hasReachedVelocity(); + return hasReachedVelocity() && isTurretRotationLocked(); } @@ -288,11 +375,11 @@ public class Turret { } private double getFlywheelVelocity(double dist) { - return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908); + return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126); } private double getHoodPOS(double dist) { - return (((1.2113 * Math.pow(10, -8)) * Math.pow(dist, 4)) - (0.00000498646 * Math.pow(dist, 3)) + (0.00068726 * Math.pow(dist, 2)) + (-0.0405783 * dist) + 1.07118); + return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859); } public void setHoodPos(double pos) { @@ -311,7 +398,20 @@ public class Turret { return turretPower; } + public double returnFlywheelSpeed() { + return hardware.getFlywheelVelocities()[1]; + } + public double getTargetVelocity() { return targetVelocity; } -} \ No newline at end of file + + + public void resetTurret() { + hardware.resetTurret(); + } + + public boolean isTurretRotationLocked() { + return turretModeHold && Math.abs(lastError) <= 10; + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java index 7cd7496..88bfe1d 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java @@ -5,7 +5,8 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; -@Configurable + + public class Constants { public static class FieldConstants { @@ -37,6 +38,13 @@ public class Constants { public static final double TURBO_SPEED_MULTIPLIER = 1.0; } + public static class LEDConstants { + public static final String LED = "prism"; + public static final int LAYER_BRIGHTNESS = 50; + + } + + public static class IntakeConstants { public static final String FRONT_INTAKE_MOTOR = "intake"; @@ -68,6 +76,9 @@ public class Constants { public static final double POWER_HOLDING = 0.1; } + + @Configurable + public static class TransferConstants { public static final String LIMITER_SERVO = "limiter"; @@ -82,34 +93,37 @@ public class Constants { public static final double UNLOCK_POS = 0.82; + public static double launch_duration = 0.77; + } + @Configurable public static class TurretConstants { - public static final double TICKS_PER_REV_MOTOR = 384.5; - public static final double EXTERNAL_GEAR_RATIO = 2.68888889; + public static double TICKS_PER_REV_MOTOR = 145.1; + public static double EXTERNAL_GEAR_RATIO = 9.179875; public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0; - public static final double KP = 0.02; - public static final double KI = 0.0; - public static final double KD = 0.001; - public static final double MAX_POWER = 0.6; + public static double KP = 0.0048; + public static double KI = 0.00801; + public static double KD = 0.00083; + public static final double MAX_POWER = 0.8; public static final double MAX_INTEGRAL = 0.5; - public static final double GOAL_RED_X = 138; - public static final double GOAL_RED_Y = 138; - public static final double GOAL_BLUE_X = 6; - public static final double GOAL_BLUE_Y = 138; + public static double GOAL_RED_X = 140; + public static double GOAL_RED_Y = 140; + public static double GOAL_BLUE_X = 4; + public static double GOAL_BLUE_Y = 140; - public static final double RED_TARGET_OFFSET_DEGREES = 14; - public static final double BLUE_TARGET_OFFSET_DEGREES = 17; - public static final double LL_TARGET_OFFSET_DEGREES = -2; - public static final double LL_LOGIC_MULTIPLIER = 1.0; - public static final double OFFSET_SMOOTHING = 0.2; + public static double RED_TARGET_OFFSET_DEGREES = 0; + public static double BLUE_TARGET_OFFSET_DEGREES = 0; + public static double LL_TARGET_OFFSET_DEGREES = 0; + public static double LL_LOGIC_MULTIPLIER = -1.0; + public static double OFFSET_SMOOTHING = 0.2; public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD; - public static final double SOFT_LIMIT_NEG = -230; - public static final double SOFT_LIMIT_POS = 230; + public static double SOFT_LIMIT_NEG = -324; + public static double SOFT_LIMIT_POS = 195; public static final double HOOD_POS_CLOSE = 0.3; public static final double HOOD_POS_FAR = 0.7; @@ -122,6 +136,7 @@ public class Constants { public static final double TURRET_POWER_MID = -0.84; public static final double TURRET_POWER_MAX = -1; public static final double TURRET_POWER_LOW = -0.7; + public static final double TURRET_HOLD_POWER = 0.75; public static final double EXTAKE_POWER = 0.3; public static final double INTAKE_POWER = -0.04; @@ -140,12 +155,25 @@ public class Constants { public static final int LIMELIGHT_PIPELINE_BLUE_TARGET = 1; public static final int LIMELIGHT_PIPELINE_RED_TARGET = 2; - public static final double CAMERA_HEIGHT_INCHES = 12.4; - public static final double GOAL_HEIGHT_INCHES = 29.5; - public static final double CAMERA_MOUNT_ANGLE_DEGREES = 7.9; + public static double CAMERA_HEIGHT_INCHES = 12.75; + public static double GOAL_HEIGHT_INCHES = 29.5; + public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8; public static final double SERVO_MAX = 0.5; public static final double SERVO_MIN = 0; + public static double LL_POSE_HEALING_SMOOTHING = 0.05; + public static double TURRET_FULL_WRAP_TICKS = 1340.0; + + // Artifact Compensation Tuning + // Lower (negative offset) for 1st and 3rd, Normal (0 offset) for 2 + public static double ARTIFACT_1 = -95.0; + public static double ARTIFACT_2 = 20.0; + + public static double ARTIFACT_3 = -20.0; + + // Timing Windows (seconds) + public static double ARTIFACT_1_WINDOW = 0.35; + public static double ARTIFACT_2_WINDOW = 0.55; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java index 1cc35e3..a4ab669 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java @@ -14,6 +14,8 @@ import org.firstinspires.ftc.teamcode.util.AutoTransfer; import org.firstinspires.ftc.teamcode.util.FPSCounter; import com.qualcomm.hardware.lynx.LynxModule; import java.util.List; + +import com.qualcomm.robotcore.hardware.VoltageSensor; public class SOLOTeleOP { @TeleOp(name = "Red SOLO", group = "FINAL") @@ -35,14 +37,17 @@ public class SOLOTeleOP { private Follower follower; private int currentAlliance; private final int initialAlliance; - private boolean isScoringActive = false; protected List allHubs; + private final Pose DEFAULT_RED_START = new Pose(129.25, 112, Math.toRadians(90)); private final Pose DEFAULT_BLUE_START = new Pose(14.75, 112, Math.toRadians(90)); protected FPSCounter fpsCounter = new FPSCounter(); + private boolean isScoringActive = false; + + public BaseSoloTeleOp(int alliance) { this.initialAlliance = alliance; this.currentAlliance = 0; @@ -63,6 +68,7 @@ public class SOLOTeleOP { currentAlliance = initialAlliance; if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START); else follower.setStartingPose(DEFAULT_BLUE_START); + stateMachine.getTurret().resetTurret(); telemetry.addLine("MANUAL START - NO AUTON DATA"); } @@ -91,16 +97,12 @@ public class SOLOTeleOP { follower.update(); Pose currentPose = follower.getPose(); - //Pose healingPose = stateMachine.getTurret().getSelfHealingPose(); - //if (healingPose != null) { - // follower.setPose(healingPose); - //} + stateMachine.update(false, false, currentPose); + handleGlobalLogic(); handleDriverControls(); - stateMachine.update(false, false, currentPose); - updateTelemetry(currentPose); } @@ -159,9 +161,8 @@ public class SOLOTeleOP { } if (stateMachine.getCurrentGameState() != GameState.SCORING) { - if (!stateMachine.getTurret().hasReachedVelocity()) { - int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2; - gamepad1.rumble(rumbleDuration); + if (!stateMachine.getTurret().hasReachedVelocity() && (gamepad1.timestamp % 100 < 10)) { + gamepad1.rumble(50); } } @@ -195,14 +196,16 @@ public class SOLOTeleOP { else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE); } - if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.06); - if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06); + if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2); + if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2); + if (gamepad1.aWasPressed()) { + stateMachine.forceLaunchSequence(); + } + } - } - - private void updateTelemetry(Pose p) { - telemetry.addData("OpMod ev", "SOLO TeleOp"); + private void updateTelemetry(Pose p) { + telemetry.addData("OpMode", "SOLO TeleOp"); telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps()); telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰"); @@ -212,13 +215,15 @@ public class SOLOTeleOP { telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE"); telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE"); telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState()); + telemetry.addData("Flywheel RPM", stateMachine.getTurret().returnFlywheelSpeed()); telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰"); telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading())); telemetry.addData("Distance", "%.1f in", stateMachine.getTurret().getDistance()); telemetry.addData("Turret Ready", stateMachine.getTurret().isTurretReady()); + telemetry.addData("Target Tick", stateMachine.getTurret().targetTick); telemetry.update(); - } - } -} \ No newline at end of file + } + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java index 8cacfc4..2c3fcfd 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java @@ -81,7 +81,7 @@ public class StateMachine { public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) { m_driveTrain.update(); - m_turret.update(manualTracking, manualSAM, currentPose); + m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime()); if (currentGameState == GameState.SCORING) { m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); @@ -94,10 +94,8 @@ public class StateMachine { m_transfer.update(); m_led.update( - currentGameState == GameState.SCORING, - m_turret.hasReachedVelocity(), - m_turret.hasTarget(), - m_driveTrain.isAnchorActive() + m_turret.isTurretRotationLocked(), + m_turret.hasReachedVelocity() ); if (currentGameState == GameState.SCORING) { @@ -108,7 +106,7 @@ public class StateMachine { } public void initUpdate() { - m_led.update(false, false, false, false); + m_led.update(false, false); } private void handleGameStateEntry(GameState newState) { @@ -182,6 +180,13 @@ public class StateMachine { setGameState(GameState.IDLE); } + public void forceLaunchSequence() { + setGameState(GameState.SCORING); + m_transfer.updateTurretReady(true); + m_intake.setIntakeState(Intake.IntakeState.LAUNCH); + isIntakeLaunching = true; + } + public DriveTrain getDriveTrain() { return m_driveTrain; } public Intake getIntake() { return m_intake; } public Turret getTurret() { return m_turret; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java index 8c83bfa..98f0845 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java @@ -107,11 +107,6 @@ public class finalTeleOp { handleOperatorInput(); stateMachine.update(manualSAM, manualTracking, currentPose); - //Pose healingPose = stateMachine.getTurret().getSelfHealingPose(); - - // if (healingPose != null) { - // follower.setPose(healingPose); - // } if (gamepad2.a) { stateMachine.getTransfer().openLimiter(); @@ -124,6 +119,7 @@ public class finalTeleOp { updateTelemetry(currentPose); + sleep(fpsCounter.getSyncTime(100)); } stateMachine.setRobotState(RobotState.ESTOP); @@ -168,9 +164,8 @@ public class finalTeleOp { stateMachine.emergencyStop(); } - if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) { - int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2; - gamepad2.rumble(rumbleDuration); + if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity() && (gamepad2.timestamp % 100 < 10)) { + gamepad2.rumble(50); } if (gamepad1.psWasPressed()) { @@ -218,8 +213,8 @@ public class finalTeleOp { - if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6); - if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6); + if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2); + if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.2); } private void handleOperatorInput() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java index 05b48ad..de659aa 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java @@ -20,15 +20,16 @@ public class GetDistanceTuning extends OpMode { private Limelight3A limelight; private double distance; - public static double CAMERA_HEIGHT_INCHES = 12.5; + + public static double CAMERA_HEIGHT_INCHES = 12.75; public static double GOAL_HEIGHT_INCHES = 29.5; public static int pipeline = 1; - public static double CAMERA_MOUNT_ANGLE_DEGREES = 14.3; + public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8; + + public static double TARGET_DISTANCE = 0.0; private TelemetryManager telemetryM; - - @Override public void init() { limelight = hardwareMap.get(Limelight3A.class, "limelight"); @@ -49,14 +50,22 @@ public class GetDistanceTuning extends OpMode { double targetY = result.getTy(); double targetArea = result.getTa(); + if (TARGET_DISTANCE != 0) { + double requiredAngleRadians = Math.atan((GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / TARGET_DISTANCE); + CAMERA_MOUNT_ANGLE_DEGREES = Math.toDegrees(requiredAngleRadians) - targetY; + + telemetryM.debug("Mode", "Angle Auto-Tuning (Diff Mode)"); + } else { + telemetryM.debug("Mode", "Standard"); + } distance = getTrigBasedDistance(targetY); - double areaDistance = getDistanceFromTag(targetArea); telemetryM.debug("Method", "Trigonometry"); telemetryM.debug("Target Y (ty)", targetY); telemetryM.debug("Calculated Distance", distance); + telemetryM.debug("Camera Mount Angle", CAMERA_MOUNT_ANGLE_DEGREES); telemetryM.debug("Area Algo Distance", areaDistance); @@ -71,13 +80,7 @@ public class GetDistanceTuning extends OpMode { } private double getTrigBasedDistance(double targetOffsetAngleVertical) { - return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + targetOffsetAngleVertical)); - - // Calculate distance - //double distanceFromLimelightToGoalInches = (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians); - - //return distanceFromLimelightToGoalInches; } private double getDistanceFromTag(double x) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java index faa45c8..d9480ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java @@ -30,11 +30,11 @@ public class PIDFTuning extends OpMode { public void init() { flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret"); flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE); + flywheelMotorR.setDirection(DcMotorSimple.Direction.FORWARD); - flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret"); - flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + flywheelMotorL = hardwareMap.get(DcMotorEx.class, "lturret"); + flywheelMotorL.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); telemetry.addLine("Init complete"); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java index 80e03f2..60165bb 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java @@ -27,8 +27,8 @@ public class TurretRotationTuning extends LinearOpMode { public static double MAX_INTEGRAL = 0.5; // --- MECHANICAL CONSTANTS --- - public static double TICKS_PER_REV_MOTOR = 384.5; - public static double EXTERNAL_GEAR_RATIO = 2.68888889; + public static double TICKS_PER_REV_MOTOR = 145.1; + public static double EXTERNAL_GEAR_RATIO = 9.179875; public static boolean MOTOR_REVERSED = true; // --- CALIBRATION SETTINGS --- @@ -117,8 +117,6 @@ public class TurretRotationTuning extends LinearOpMode { break; } - sleep(fps.getSyncTime(targetFPS)); - // --- TELEMETRY --- telemetryM.debug("▰▰▰▰▰ MODE ▰▰▰▰▰", currentMode.toString()); telemetryM.debug("Gear Ratio", EXTERNAL_GEAR_RATIO); @@ -128,6 +126,8 @@ public class TurretRotationTuning extends LinearOpMode { telemetryM.debug("Target Deg", targetTicks / ticksPerDegree); telemetry.addData("FPS", fps.getAverageFps()); telemetryM.update(telemetry); + + sleep(fps.getSyncTime(targetFPS)); } limelight.stop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java index 93a2362..ad50a7b 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java @@ -336,7 +336,7 @@ public class allInOne extends LinearOpMode { double ta = result.getTa(); calculatedDistance = getDistanceFromTag(ta); calculatedVelocity = getFlywheelVelocity(calculatedDistance); - calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity); + calculatedHoodPos = getHoodPOS(calculatedDistance); if (launch) { leftTurret.setVelocity(calculatedVelocity); @@ -459,7 +459,7 @@ public class allInOne extends LinearOpMode { return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908); } - private double getHoodPOS(double dist, double targetVelocity) { - return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX); + private double getHoodPOS(double dist) { + return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java index f7d48b7..a4c3840 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java @@ -83,8 +83,6 @@ public class anchorTuning extends LinearOpMode { driveTrain.teleopDrive(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x); } - sleep(fps.getSyncTime(targetFPS)); - telemetryM.debug("Anchor Active", driveTrain.isAnchorActive()); telemetry.addData("FPS", fps.getAverageFps()); @@ -94,6 +92,8 @@ public class anchorTuning extends LinearOpMode { telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError()); telemetryM.update(telemetry); + + sleep(fps.getSyncTime(targetFPS)); } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java index 1127af5..6a91223 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java @@ -223,8 +223,8 @@ public class autoCollection extends LinearOpMode { break; } - sleep(fpsCounter.getSyncTime(targetFPS)); updateTelemetry(currentTicks, currentDegrees); + sleep(fpsCounter.getSyncTime(targetFPS)); } limelight.stop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java index 91c2c9e..9268ee6 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java @@ -5,6 +5,7 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; @@ -12,26 +13,46 @@ import com.qualcomm.robotcore.util.Range; // Make sure your import points to your actual Constants file import org.firstinspires.ftc.teamcode.teleOp.Constants; + @Configurable @TeleOp public class flywheelTuning extends OpMode { + public enum CalibrationState { + IDLE, + SPOOLING, + TRACKING, + TUNING + } private Limelight3A limelight; public DcMotorEx flywheelMotorR; public DcMotorEx flywheelMotorL; - public Servo hoodServo; + public DcMotor intake; + public Servo hoodServo; + public Servo limiter; + + // Configurable / Dashboard variables + public static boolean autoMode = false; public static double curTargetVelocity = 0; public static double targetPOS = 0; public static int pipeline = 1; - public static boolean reverse = false; + // Standard runtime variables double distance = 0; + // State machine and tracking variables + CalibrationState calibState = CalibrationState.IDLE; + boolean lastX = false; + + double maxError = 0.0; + double snapshotAutoHood = 0.0; + double calculatedGain = 0.0; + @Override public void init() { flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR); @@ -43,45 +64,168 @@ public class flywheelTuning extends OpMode { flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); hoodServo = hardwareMap.servo.get("hoodservo"); - hoodServo.setDirection(Servo.Direction.REVERSE); limelight = hardwareMap.get(Limelight3A.class, "limelight"); limelight.pipelineSwitch(pipeline); limelight.start(); + intake = hardwareMap.get(DcMotor.class, "intake"); + intake.setDirection(DcMotorSimple.Direction.FORWARD); + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + limiter = hardwareMap.servo.get("limiter"); + limiter.setDirection(Servo.Direction.FORWARD); + + limiter.setPosition(1); + + + telemetry.addLine("Init complete"); } @Override public void loop() { limelight.pipelineSwitch(pipeline); - LLResult result = limelight.getLatestResult(); - if (result.isValid()) { + if (result != null && result.isValid()) { double targetY = result.getTy(); telemetry.addData("Ty", targetY); distance = getTrigBasedDistance(targetY); } - if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity); - flywheelMotorL.setVelocity(curTargetVelocity);} + boolean currentX = gamepad1.x; + if (currentX && !lastX) { + switch (calibState) { + case IDLE: + maxError = 0.0; + calculatedGain = 0.0; + calibState = CalibrationState.SPOOLING; + break; + case SPOOLING: + calibState = CalibrationState.TRACKING; + break; + case TRACKING: + snapshotAutoHood = getHoodPOS(distance); + calibState = CalibrationState.TUNING; + break; + case TUNING: + calibState = CalibrationState.IDLE; + break; + } + } + lastX = currentX; - hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX)); + double applyVel = 0; + double applyHood = 0; + double currentVel = flywheelMotorR.getVelocity(); + + double autoTargetVel = getFlywheelVelocity(distance); + double autoHoodPos = getHoodPOS(distance); - telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity()); - telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity); - telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS); - telemetry.addData("Distance", distance); + if (autoMode) { + switch (calibState) { + case IDLE: + applyVel = autoTargetVel; + applyHood = autoHoodPos; + break; + + case SPOOLING: + applyVel = autoTargetVel; + applyHood = autoHoodPos; + if (Math.abs(currentVel - autoTargetVel) <= 30) { + calibState = CalibrationState.TRACKING; + } + break; + + case TRACKING: + applyVel = autoTargetVel; + applyHood = autoHoodPos; + + double currentError = currentVel - autoTargetVel; + if (currentError < maxError) { + maxError = currentError; + } + break; + + case TUNING: + applyVel = autoTargetVel + maxError; + + applyHood = targetPOS; + + if (maxError != 0) { + calculatedGain = (targetPOS - snapshotAutoHood) / maxError; + } else { + calculatedGain = 0; + } + break; + } + } else { + applyVel = curTargetVelocity; + applyHood = targetPOS; + } + + if (!gamepad1.b) { + flywheelMotorR.setVelocity(applyVel); + flywheelMotorL.setVelocity(applyVel); + } else { + flywheelMotorR.setVelocity(0); + flywheelMotorL.setVelocity(0); + } + + if (gamepad1.a) { + intake.setPower(1); + limiter.setPosition(0.82); + } else { + limiter.setPosition(1); + intake.setPower(0); + } + + hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX)); + + + // --- TELEMETRY --- + telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL"); + telemetry.addData("Calibration State", calibState.toString()); + telemetry.addLine(); + + telemetry.addData("Current Velocity", "%.2f", currentVel); + telemetry.addData("Applied Target Vel", "%.2f", applyVel); + telemetry.addData("Applied Target Hood", "%.4f", applyHood); + telemetry.addData("Distance", "%.2f inches", distance); + telemetry.addLine(); + + if (autoMode) { + telemetry.addData("Raw Auto Target Vel", "%.2f", autoTargetVel); + telemetry.addData("Raw Auto Target Hood", "%.4f", autoHoodPos); + telemetry.addLine("--- CALIBRATION DATA ---"); + telemetry.addData("Max RPM Drop (Error)", "%.2f", maxError); + telemetry.addData("Snapshot Auto Hood", "%.4f", snapshotAutoHood); + telemetry.addData("User Tuning Hood (targetPOS)", "%.4f", targetPOS); + telemetry.addData("CALCULATED GAIN", "%.8f", calculatedGain); + telemetry.addLine("NOTE: When done tuning, copy CALCULATED GAIN into your snippet!"); + } telemetry.update(); } private double getTrigBasedDistance(double targetOffsetAngleVertical) { - return (17.1) / Math.tan(Math.toRadians(7.9 + targetOffsetAngleVertical)); + return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical)); // ^ GOAL - CAMERA HEIGHT ^ Offset Degrees } + + private double getFlywheelVelocity(double dist) { + return (0.0000119972 * Math.pow(dist, 4) + - 0.00249603 * Math.pow(dist, 3) + + 0.179513 * Math.pow(dist, 2) + + (0.0519688 * dist) + + 1489.8908); + } + + private double getHoodPOS(double dist) { + return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java index 8244839..04a8b32 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java @@ -23,38 +23,40 @@ import java.util.List; @TeleOp(name = "Odo Turret Tracking", group = "Turret") public class odoTracking extends LinearOpMode { - public static double kP = 0.02; - public static double kI = 0.0; - public static double kD = 0.001; + public static double kP = 0.0040; + public static double kI = 0.002; + public static double kD = 0.0004; public static double MAX_POWER = 0.6; public static double MAX_INTEGRAL = 0.5; // ================= Hardware Constants ================= - public static double TICKS_PER_REV_MOTOR = 384.5; - public static double EXTERNAL_GEAR_RATIO = 2.68888889; - public static boolean MOTOR_REVERSED = true; + public static double TICKS_PER_REV_MOTOR = 145.1; + public static double EXTERNAL_GEAR_RATIO = 9.179875; + public static boolean MOTOR_REVERSED = false; // ================= Limits & Logic ================= - public static double LL_LOGIC_MULTIPLIER = 1.0; - public static double SOFT_LIMIT_NEG = -230; - public static double SOFT_LIMIT_POS = 230; + public static double LL_LOGIC_MULTIPLIER = -1.0; + public static double SOFT_LIMIT_NEG = -330; + public static double SOFT_LIMIT_POS = 350; // ================= Targeting Offsets ================= - public static double RED_TARGET_OFFSET_DEGREES = 14; - public static double BLUE_TARGET_OFFSET_DEGREES = 17; - public static double LL_TARGET_OFFSET_DEGREES = -2; + public static double RED_TARGET_OFFSET_DEGREES = 0; + public static double BLUE_TARGET_OFFSET_DEGREES = 0; + public static double LL_TARGET_OFFSET_DEGREES = 0; // ================= Field Coordinates ================= - public static double GOAL_RED_X = 138; - public static double GOAL_RED_Y = 138; - public static double GOAL_BLUE_X = 6; - public static double GOAL_BLUE_Y = 138; + public static double GOAL_RED_X = 140; + public static double GOAL_RED_Y = 140; + public static double GOAL_BLUE_X = 4; + public static double GOAL_BLUE_Y = 140; // ================= Sensor Fusion Settings ================= public static double OFFSET_SMOOTHING = 0.2; public static int targetFPS = 100; + public static double TURRET_FULL_WRAP_TICKS = 1340.0; + // ================= Hardware Objects ================= private Limelight3A limelight; private DcMotorEx turretMotor; @@ -68,7 +70,6 @@ public class odoTracking extends LinearOpMode { private double targetCorrectionOffsetTicks = 0; private double fusedTargetTicks = 0; - private enum TuningMode { CALIBRATION, TRACKING } private TuningMode currentMode = TuningMode.CALIBRATION; @@ -80,7 +81,7 @@ public class odoTracking extends LinearOpMode { protected FPSCounter fpsCounter = new FPSCounter(); public static double start_x = 72; - public static double start_y = 8.5; + public static double start_y = 72; public static double start_heading = 90; @@ -157,48 +158,65 @@ public class odoTracking extends LinearOpMode { break; } - sleep(fpsCounter.getSyncTime(targetFPS)); updateTelemetry(currentTicks, currentDegrees); + sleep(fpsCounter.getSyncTime(targetFPS)); } limelight.stop(); } private void calculateHybridTarget(double currentTicks, double currentDegrees) { - double odoTargetTicks = calculateOdometryTargetTicks(); + Pose robotPose = follower.getPose(); + double odoTargetTicks = calculateOdometryTargetTicks(robotPose); LLResult result = limelight.getLatestResult(); if (result != null && result.isValid()) { - double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks); + // 1. Calculate Vision Truth: Where the goal sits based on the camera right now + double tx = result.getTx(); + double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER); + double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES; + double visionTargetTicks = finalVisionDegrees * ticksPerDegree; - targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING)) - + (rawErrorTicks * OFFSET_SMOOTHING); + // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset) + double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks; + + // 3. Calculate Correction Error: The delta between our current belief and camera reality + double correctionError = visionTargetTicks - currentFusedTarget; + + // 4. Normalize the error for wrapping (360 degrees) + double ticksPerRev = 360.0 * ticksPerDegree; + while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev; + while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev; + + // 5. Apply the error to the persistent offset (Bias-Shift) + // We use OFFSET_SMOOTHING as a gain to prevent single-frame noise from causing jitter + targetCorrectionOffsetTicks += (correctionError * OFFSET_SMOOTHING); } double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks; - fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS); + // 1. Apply initial soft limits + double preWrapTarget = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS); + + // 2. Implement wrapping logic: if target exceeds 180 degrees (670 ticks), normalize to the other side. + // This implies a full wrap cycle is 2 * 670 = 1340 ticks. + double fullWrapTicks = TURRET_FULL_WRAP_TICKS; + double halfWrapTicks = fullWrapTicks / 2.0; + + // If target is beyond positive half-wrap threshold, wrap to negative equivalent + if (preWrapTarget > halfWrapTicks) { + preWrapTarget -= fullWrapTicks; + } + // If target is beyond negative half-wrap threshold, wrap to positive equivalent + else if (preWrapTarget < -halfWrapTicks) { + preWrapTarget += fullWrapTicks; + } + + // 3. Apply final soft limits after wrapping + fusedTargetTicks = Range.clip(preWrapTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS); } - private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) { - double tx = result.getTx(); - - double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER); - - double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES; - double visionTargetTicks = finalVisionDegrees * ticksPerDegree; - - double rawErrorTicks = visionTargetTicks - odoTargetTicks; - - double ticksPerRev = 360.0 * ticksPerDegree; - - while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev; - while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev; - return rawErrorTicks; - } - - private double calculateOdometryTargetTicks() { - Pose robotPose = follower.getPose(); + private double calculateOdometryTargetTicks(Pose robotPose) { double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X; double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y; @@ -237,6 +255,7 @@ public class odoTracking extends LinearOpMode { double output = (kP * error) + integralTerm + (kD * derivative); output = Range.clip(output, -MAX_POWER, MAX_POWER); + telemetry.addData("Power", output); turretMotor.setPower(output); lastError = error; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java index d932853..818b487 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java @@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.util; import com.qualcomm.robotcore.util.ElapsedTime; public class FPSCounter { - private ElapsedTime loopTimer = new ElapsedTime(); + private final ElapsedTime loopTimer = new ElapsedTime(); private double loopTimeSum = 0; private int loopCount = 0; private double currentLoopTime = 0; @@ -13,6 +13,9 @@ public class FPSCounter { reset(); } + /** + * Call this at the very beginning of your while(opModeIsActive) loop. + */ public void startLoop() { double now = loopTimer.milliseconds(); if (loopCount > 0) { @@ -23,10 +26,17 @@ public class FPSCounter { loopCount++; } + /** + * Calculates the time remaining to hit the target FPS. + * To be accurate, call this at the very end of your loop, + * AFTER telemetry.update(). + */ public long getSyncTime(int targetFps) { + if (targetFps <= 0) return 0; + double targetMs = 1000.0 / targetFps; - - double elapsedWorkMs = loopTimer.milliseconds() - lastTime; + double now = loopTimer.milliseconds(); + double elapsedWorkMs = now - lastTime; double remainingMs = targetMs - elapsedWorkMs; return (remainingMs > 0) ? (long) remainingMs : 0; @@ -57,4 +67,4 @@ public class FPSCounter { loopCount = 0; currentLoopTime = 0; } -} \ No newline at end of file +} diff --git a/gradle.properties b/gradle.properties index 9dce7ee..f840830 100644 --- a/gradle.properties +++ b/gradle.properties @@ -7,7 +7,12 @@ android.useAndroidX=true android.enableJetifier=false # Allow Gradle to use up to 1 GB of RAM -org.gradle.jvmargs=-Xmx1024M +org.gradle.jvmargs=-Xmx2048M android.nonTransitiveRClass=false -org.gradle.configuration-cache=true \ No newline at end of file +org.gradle.configuration-cache=true + +org.gradle.parallel=true +org.gradle.caching=true + +org.gradle.daemon=true \ No newline at end of file