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 0ed240a..0c0acf2 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 @@ -15,6 +15,10 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.SwitchableLight; + +import com.qualcomm.robotcore.hardware.ServoImplEx; +import com.qualcomm.robotcore.hardware.PwmControl; + import org.firstinspires.ftc.teamcode.teleOp.Constants; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @@ -35,10 +39,24 @@ public class hwMap { } public static class LedHwMap { - public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led; + private ServoImplEx led; public LedHwMap(HardwareMap hardwareMap) { - //led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED); + + led = hardwareMap.get(ServoImplEx.class, Constants.LEDConstants.LED); + led.setPwmRange(new PwmControl.PwmRange(500, 2500)); + } + + public void setLedPos(double pos) { + led.setPosition(pos); + } + + public void setLedOn(boolean yes) { + if (yes) { + led.setPwmEnable(); + } else { + led.setPwmDisable(); + } } } @@ -248,7 +266,7 @@ public class hwMap { private double lastKnownDistance = 0.0; private ElapsedTime noTargetTimer = new ElapsedTime(); - PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15); + PIDFCoefficients pidfCoefficients = new PIDFCoefficients(10, 0, 11, 14.3); public TurretHwMap(HardwareMap hardwareMap) { noTargetTimer.reset(); 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 408af94..f105e1d 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 @@ -5,6 +5,7 @@ import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierCurve; import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; import com.pedropathing.paths.PathChain; import com.pedropathing.util.Timer; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -116,7 +117,7 @@ public class Auton { @Override public void buildPaths() { - startPose = new Pose(31.5, 137, Math.toRadians(180)); + startPose = new Pose(21, 124, Math.toRadians(143.5)); follower.setStartingPose(startPose); // Waypoints @@ -124,9 +125,9 @@ public class Auton { Pose set2DrivePose = new Pose(39, 60, Math.toRadians(180)); Pose set2PickPose = new Pose(14, 60, Math.toRadians(180)); // tangential Pose set2ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose gate1PickPose = new Pose(10.652, 61.079, Math.toRadians(140)); + Pose gate1PickPose = new Pose(9, 60, Math.toRadians(143.5)); Pose gate1ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose gate2PickPose = new Pose(10.652, 61.079, Math.toRadians(140)); + Pose gate2PickPose = new Pose(9, 60, Math.toRadians(143.5)); Pose gate2ScorePose = new Pose(54, 90, Math.toRadians(144)); Pose set3DrivePose = new Pose(38.515, 47.156, Math.toRadians(220)); Pose set3PickPose = new Pose(16.419, 35.945, Math.toRadians(185)); @@ -137,7 +138,6 @@ public class Auton { Pose parkTargetPose = new Pose(36.219, 93.170, Math.toRadians(148)); // Controls - Pose preloadControl = new Pose(27.400, 115.541); Pose set2ScoreControl = new Pose(50.386, 78.923); Pose gate1PickControl = new Pose(40.119, 55.595); Pose gate1ScoreControl = new Pose(41.918, 62.187); @@ -148,7 +148,7 @@ public class Auton { Pose set1PickControl = new Pose(14.863, 77.692); scorePreloadPath = follower.pathBuilder() - .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose)) + .addPath(new BezierLine(startPose, preloadScorePose)) .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading()) .build(); @@ -319,7 +319,7 @@ public class Auton { @Override public void buildPaths() { - startPose = new Pose(112.5, 137, Math.toRadians(0)); + startPose = new Pose(122, 125.5, Math.toRadians(36)); follower.setStartingPose(startPose); // Waypoints @@ -327,9 +327,9 @@ public class Auton { Pose set2DrivePose = new Pose(105, 60, Math.toRadians(0)); Pose set2PickPose = new Pose(130, 60, Math.toRadians(0)); // tangential Pose set2ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose gate1PickPose = new Pose(133.348, 61.079, Math.toRadians(40)); + Pose gate1PickPose = new Pose(132.7, 56, Math.toRadians(36)); Pose gate1ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose gate2PickPose = new Pose(133.348, 61.079, Math.toRadians(40)); + Pose gate2PickPose = new Pose(132.7, 56, Math.toRadians(36)); Pose gate2ScorePose = new Pose(90, 90, Math.toRadians(36)); Pose set3DrivePose = new Pose(105.485, 47.156, Math.toRadians(-40)); Pose set3PickPose = new Pose(127.581, 35.945, Math.toRadians(-5)); @@ -442,8 +442,8 @@ public class Auton { protected final boolean isNet; protected final boolean useTimer; - public static double timerTimeout = 4; - public static double gateWaitTime = 3.0; + public static double timerTimeout = 4.5; + public static double gateWaitTime = 4.0; protected Pose startPose; @@ -517,8 +517,6 @@ 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); @@ -538,7 +536,7 @@ public class Auton { turretHw.resetTurret(); waitForStart(); - systems.update(allianceTarget, follower.getPose()); + systems.update(allianceTarget, follower.getPose(), new Vector(0, 0)); opmodeTimer.resetTimer(); setPathState(PathState.DRIVE_TO_SCORE_PRELOAD); @@ -548,7 +546,7 @@ public class Auton { } follower.update(); statePathUpdate(); - systems.update(allianceTarget, follower.getPose()); + systems.update(allianceTarget, follower.getPose(), follower.getVelocity()); telemetry.addData("Path State", pathState); telemetry.addData("System State", systems.getCurrentState()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java index d5fdcd3..4164294 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java @@ -137,7 +137,7 @@ public abstract class TimeAuton extends LinearOpMode { timer.reset(); while(opModeIsActive() && timer.seconds() < 2.0) { updateSystems(); - if(turret.isTurretReady()) break; + if(turret.isTurretReady(true)) break; } @@ -167,13 +167,13 @@ public abstract class TimeAuton extends LinearOpMode { AutoTransfer.updatePose(currentPose); // Update Turret with current Pose - turret.updateAuton(alliance, currentPose, 0.2); + turret.updateAuton(alliance, currentPose, follower.getVelocity(), 0.2); - transfer.updateTurretReady(turret.isTurretReady()); + transfer.updateTurretReady(turret.isTurretReady(true)); transfer.update(); // Telemetry for debugging - telemetry.addData("Turret Ready", turret.isTurretReady()); + telemetry.addData("Turret Ready", turret.isTurretReady(false)); telemetry.addData("Transfer State", transfer.getTransferState()); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java index b03732a..ebe2917 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.auton; import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; // Unused but kept for reference @@ -88,8 +89,8 @@ public class pedroStateMachine { } } - public void update(int allianceTarget, Pose currentPose) { - m_turret.updateAuton(allianceTarget, currentPose, 0.2); + public void update(int allianceTarget, Pose currentPose, Vector currentVelocity) { + m_turret.updateAuton(allianceTarget, currentPose, currentVelocity, 0.2); if (currentState == AutonState.SCORING) { m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java index fe5f557..2f4cf03 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java @@ -40,7 +40,7 @@ public class DriveTrain { public enum DriveState { NORMAL, TURBO, PRECISION, STOP } - private DriveState currentState = DriveState.NORMAL; + private DriveState currentState = DriveState.TURBO; private double speedMultiplier = 1.0; public DriveTrain(hwMap.DriveHwMap hardware) { @@ -59,8 +59,8 @@ public class DriveTrain { if(dt == 0) dt = 0.001; lastLoopTime = currentTime; - double currentFwd = h_driveTrain.backRightMotor.getCurrentPosition(); - double currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition(); + double currentFwd = h_driveTrain.frontLeftMotor.getCurrentPosition(); + double currentStrafe = h_driveTrain.frontRightMotor.getCurrentPosition(); double currentHeading = getHeadingDegrees(); // 2. Calculate Errors @@ -72,8 +72,6 @@ public class DriveTrain { while (errorTurn > 180) errorTurn -= 360; while (errorTurn < -180) errorTurn += 360; - // 3. PID Calculations - // Forward sumFwd += errorForward * dt; double dFwd = (errorForward - lastFwdErr) / dt; 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 3cf091a..5602ed7 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,36 +1,20 @@ 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 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); - public LED(hwMap.LedHwMap hardware) { this.hardware = hardware; - - int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS; - solidRed.setBrightness(LED_BRIGHTNESS); - solidOrange.setBrightness(LED_BRIGHTNESS); - solidYellow.setBrightness(LED_BRIGHTNESS); - solidGreen.setBrightness(LED_BRIGHTNESS); + turnOn(); } public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) { - /*int newLedState = 0; + int newLedState = 0; if (isTurretLocked && isFlywheelUpToSpeed) { newLedState = 1; @@ -42,15 +26,25 @@ public class LED { if (newLedState != currentLedState) { if (newLedState == 0) { - hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed); + hardware.setLedPos(Constants.LEDConstants.COLOR_RED); } else if (newLedState == 1) { - hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen); + hardware.setLedPos(Constants.LEDConstants.COLOR_GREEN); } else if (newLedState == 2) { - hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange); + hardware.setLedPos(Constants.LEDConstants.COLOR_ORANGE); } else if (newLedState == 3) { - hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow); + hardware.setLedPos(Constants.LEDConstants.COLOR_YELLOW); } + currentLedState = newLedState; - }*/ + } + } + + public void turnOff() { + hardware.setLedOn(false); + currentLedState = -1; + } + + public void turnOn() { + hardware.setLedOn(true); } } \ No newline at end of file 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 b355c41..b6b57fb 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 @@ -7,6 +7,8 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.teleOp.Constants; +import com.pedropathing.math.Vector; + public class Turret { private final hwMap.TurretHwMap hardware; @@ -24,11 +26,14 @@ public class Turret { private double targetCorrectionOffsetTicks = 0; private double fusedTargetTicks = 0; public double turretPowerRotation; - public static double offsetTicks = 0.0; + private double offsetTicks = 0.0; public double offsetHood = 0; private Pose robotPose; + private Vector robotVelocity = new Vector(0,0); // NEW + private double kinematicDistanceOffset = 0.0; // NEW + private double kinematicAngularOffsetTicks = 0.0; // NEW public double targetTick; @@ -59,6 +64,7 @@ public class Turret { if (this.alliance != alliance) { this.alliance = alliance; targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes + offsetTicks = 0; // Reset manual offset when alliance changes if (alliance == 2) { hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET); } else if (alliance == 1) { @@ -108,17 +114,17 @@ public class Turret { return hardware.hasTarget(); } - public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) { + public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, Vector currentVelocity, double elapsedTime) { double dt = loopTimer.seconds(); loopTimer.reset(); if (dt < 0.001) dt = 0.001; - updatePose(currentPose); + updatePose(currentPose, currentVelocity); double rawTicks = hardware.getTurretRotationPosition(); double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE; - distance = getDistance(); + distance = getDistance() + kinematicDistanceOffset; if (!manualTracking) { calculateHybridTarget(currentPose, currentDegrees, rawTicks); @@ -131,19 +137,21 @@ public class Turret { handleLaunchLogic(elapsedTime); } else { - // idleLaunch(); + idleLaunch(); } } + + public void idleLaunch() { - hardware.setTurretVelocity(1000); + hardware.setTurretVelocity(1300); double calculatedHoodPos = getHoodPOS(65); setHoodPos(calculatedHoodPos); } - public void updateAuton(int alliance, Pose currentPose, double offset) { - updatePose(currentPose); + public void updateAuton(int alliance, Pose currentPose, Vector currentVelocity, double offset) { + updatePose(currentPose, currentVelocity); // Updated double dt = loopTimer.seconds(); loopTimer.reset(); if (dt < 0.001) dt = 0.001; @@ -158,11 +166,11 @@ public class Turret { double rawTicks = hardware.getTurretRotationPosition(); double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE; - distance = getDistance(); + distance = getDistance() + kinematicDistanceOffset; calculateHybridTarget(currentPose, currentDegrees, rawTicks); runPIDControl(rawTicks, dt); - handleLaunchLogicOffset(-offset); + handleLaunchLogicOffset(0); } private void handleLaunchLogic(double elapsedTime) { @@ -170,11 +178,6 @@ public class Turret { 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; @@ -190,7 +193,7 @@ public class Turret { double calculatedHoodPos = getHoodPOS(distance); double currentVel = hardware.getFlywheelVelocities()[1]; - double recoilOffset = (currentVel - targetVelocity) * -0.000185; + double recoilOffset = (currentVel - targetVelocity) * -0.000385; setHoodPos(calculatedHoodPos + recoilOffset - 0.03); } else { @@ -218,7 +221,7 @@ public class Turret { double calculatedHoodPos = getHoodPOS(distance); double currentVel = hardware.getFlywheelVelocities()[1]; - double recoilOffset = (currentVel - targetVelocity) * -0.000185; + double recoilOffset = (currentVel - targetVelocity) * -0.000385; setHoodPos(calculatedHoodPos + recoilOffset); } else { @@ -226,21 +229,27 @@ public class Turret { } } + + + private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) { double odoTargetTicks = calculateOdometryTargetTicks(robotPose); LLResult result = hardware.getLimelightResult(); if (result != null && result.isValid()) { - // 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) + visionTargetTicks += kinematicAngularOffsetTicks; + + // Shift the vision target by our manual offset so LL doesn't fight it + double visionTargetWithManualOffset = visionTargetTicks + offsetTicks; + double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks; - double correctionError = visionTargetTicks - currentFusedTarget; + double correctionError = visionTargetWithManualOffset - currentFusedTarget; // 4. Normalize the error for wrapping (360 degrees) double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE; @@ -277,21 +286,19 @@ public class Turret { while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI); while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI); - double relativeDegrees = Math.toDegrees(relativeRad); double staticOffset = (alliance == 2) ? Constants.TurretConstants.RED_TARGET_OFFSET_DEGREES : Constants.TurretConstants.BLUE_TARGET_OFFSET_DEGREES; - return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE; + // APPLY THE KINEMATIC SHIFT + return ((relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE) + kinematicAngularOffsetTicks; } private void runPIDControl(double currentTicks, double dt) { - fusedTargetTicks = fusedTargetTicks + offsetTicks; double error = fusedTargetTicks - currentTicks; double derivative = (error - lastError) / dt; - // 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 + double entryThreshold = 10; + double exitThreshold = 30; if (!turretModeHold && Math.abs(error) <= entryThreshold) { turretModeHold = true; @@ -302,7 +309,7 @@ public class Turret { if (turretModeHold) { if (lastTurretModeHold != turretModeHold) { hardware.runToPos((int) fusedTargetTicks); // Set target first - hardware.enableTurretHold(); // Then set mode + hardware.enableTurretHold(); hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power } hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode @@ -337,20 +344,60 @@ public class Turret { double absTarget = Math.abs(targetVelocity); double error = Math.abs(absCurrent - absTarget); - double tolerance = absTarget * 0.03; + double tolerance = absTarget * 0.05; return error <= tolerance; } return false; } - public boolean isTurretReady() { - return hasReachedVelocity() && isTurretRotationLocked(); + public boolean isTurretReady(boolean isAuton) { + + if (!isAuton) return hasReachedVelocity() && isTurretRotationLocked(); + else return hasReachedVelocity(); } - public void updatePose(Pose robotPose) { + public void updatePose(Pose robotPose) { // Fallback if called somewhere else without velocity + updatePose(robotPose, new Vector(0,0)); + } + + public void updatePose(Pose robotPose, Vector robotVelocity) { this.robotPose = robotPose; + this.robotVelocity = robotVelocity != null ? robotVelocity : new Vector(0,0); + calculateKinematicOffsets(); + } + + private void calculateKinematicOffsets() { + if (robotPose == null || robotVelocity == null) { + kinematicDistanceOffset = 0; + kinematicAngularOffsetTicks = 0; + return; + } + + double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X; + double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y; + + double virtualX = targetX - (robotVelocity.getXComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT); + double virtualY = targetY - (robotVelocity.getYComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT); + + double realDx = targetX - robotPose.getX(); + double realDy = targetY - robotPose.getY(); + double virtualDx = virtualX - robotPose.getX(); + double virtualDy = virtualY - robotPose.getY(); + + double realDistance = Math.hypot(realDx, realDy); + double virtualDistance = Math.hypot(virtualDx, virtualDy); + kinematicDistanceOffset = virtualDistance - realDistance; + + double realAngle = Math.atan2(realDy, realDx); + double virtualAngle = Math.atan2(virtualDy, virtualDx); + + double angleDiff = virtualAngle - realAngle; + while (angleDiff > Math.PI) angleDiff -= (2 * Math.PI); + while (angleDiff < -Math.PI) angleDiff += (2 * Math.PI); + + kinematicAngularOffsetTicks = Math.toDegrees(angleDiff) * Constants.TurretConstants.TICKS_PER_DEGREE; } public double getDistance() { @@ -375,7 +422,11 @@ public class Turret { } private double getFlywheelVelocity(double dist) { - return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126); + return (-0.0000144121 * Math.pow(dist, 4) + + 0.00398607 * Math.pow(dist, 3) + - 0.342198 * Math.pow(dist, 2) + + (16.43014 * dist) + + 1198.24434); } private double getHoodPOS(double dist) { @@ -392,6 +443,7 @@ public class Turret { public void updateOffsetTicks(double change) { offsetTicks += change; + targetCorrectionOffsetTicks += change; } public double getTurretPower() { @@ -407,11 +459,11 @@ public class Turret { } - public void resetTurret() { - hardware.resetTurret(); - } + public void resetTurret() { + hardware.resetTurret(); + } - public boolean isTurretRotationLocked() { - return turretModeHold && Math.abs(lastError) <= 10; - } - } \ No newline at end of file + 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 88bfe1d..9fcc721 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 @@ -40,8 +40,12 @@ public class Constants { public static class LEDConstants { public static final String LED = "prism"; - public static final int LAYER_BRIGHTNESS = 50; + // TODO: Tune these! + public static final double COLOR_RED = 0.28; + public static final double COLOR_GREEN = 0.45; + public static final double COLOR_ORANGE = 0.32; + public static final double COLOR_YELLOW = 0.38; } @@ -93,7 +97,7 @@ public class Constants { public static final double UNLOCK_POS = 0.82; - public static double launch_duration = 0.77; + public static double launch_duration = 0.9; } @@ -122,8 +126,8 @@ public class Constants { public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD; - public static double SOFT_LIMIT_NEG = -324; - public static double SOFT_LIMIT_POS = 195; + public static double SOFT_LIMIT_NEG = -370; + public static double SOFT_LIMIT_POS = 370; public static final double HOOD_POS_CLOSE = 0.3; public static final double HOOD_POS_FAR = 0.7; @@ -167,13 +171,15 @@ public class Constants { // 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_2 = 45.0; - public static double ARTIFACT_3 = -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; + + public static double SHOT_TIME_OF_FLIGHT = 0.83; } 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 a4ab669..25f0faa 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 @@ -12,10 +12,15 @@ import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.TransferSys; 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.pedropathing.math.Vector; + import com.qualcomm.robotcore.hardware.VoltageSensor; + public class SOLOTeleOP { @TeleOp(name = "Red SOLO", group = "FINAL") @@ -24,6 +29,7 @@ public class SOLOTeleOP { super(2); } } + @TeleOp(name = "Blue SOLO", group = "FINAL") public static class BlueSOLO extends BaseSoloTeleOp { public BlueSOLO() { @@ -96,8 +102,9 @@ public class SOLOTeleOP { follower.update(); Pose currentPose = follower.getPose(); + Vector currentVelocity = follower.getVelocity(); - stateMachine.update(false, false, currentPose); + stateMachine.update(false, false, currentPose, currentVelocity); handleGlobalLogic(); @@ -173,9 +180,12 @@ public class SOLOTeleOP { private void handleDriverControls() { - if (gamepad1.dpad_up) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO); - else if (gamepad1.dpad_left) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL); - else if (gamepad1.dpad_down) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION); + if (gamepad1.dpad_up) + stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO); + else if (gamepad1.dpad_left) + stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL); + else if (gamepad1.dpad_down) + stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION); if (stateMachine.getCurrentGameState() != GameState.SCORING) { if (gamepad1.right_trigger > 0.1) { @@ -193,18 +203,19 @@ public class SOLOTeleOP { if (gamepad1.yWasPressed()) { isScoringActive = !isScoringActive; if (isScoringActive) stateMachine.setGameState(GameState.SCORING); - else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE); + else if (stateMachine.getCurrentGameState() == GameState.SCORING) + stateMachine.setGameState(GameState.IDLE); } - if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2); - if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2); + if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(3); + if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-3); if (gamepad1.aWasPressed()) { stateMachine.forceLaunchSequence(); } - } + } - private void updateTelemetry(Pose p) { + private void updateTelemetry(Pose p) { telemetry.addData("OpMode", "SOLO TeleOp"); telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps()); @@ -220,10 +231,10 @@ public class SOLOTeleOP { 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("Turret Ready", stateMachine.getTurret().isTurretReady(false)); 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 2c3fcfd..c498763 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 @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.teleOp; import com.pedropathing.geometry.Pose; // ADDED +import com.pedropathing.math.Vector; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; @@ -29,11 +31,13 @@ public class StateMachine { private final Lift m_lift; private final LED m_led; + private final ElapsedTime ledTimer = new ElapsedTime(); boolean isMotifEdited = false; private long lastIndexAllArtifactsTime = 0; public boolean isIntakeLaunching = false; + private boolean isForceLaunch = false; public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) { this.m_driveTrain = new DriveTrain(h_driveTrain); @@ -69,6 +73,7 @@ public class StateMachine { m_turret.setTurretState(Turret.TurretState.IDLE); m_intake.setIntakeState(Intake.IntakeState.IDLE); isIntakeLaunching = false; + isForceLaunch = false; //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL); m_driveTrain.stopAnchor(); break; @@ -78,14 +83,17 @@ public class StateMachine { } } - public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) { + public void update(boolean manualSAM, boolean manualTracking, Pose currentPose, Vector currentVelocity) { m_driveTrain.update(); - m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime()); + m_turret.update(manualTracking, manualSAM, currentPose, currentVelocity, m_transfer.getLaunchElapsedTime()); if (currentGameState == GameState.SCORING) { - m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); - if (m_turret.hasReachedVelocity()) { + if (!isForceLaunch) { + m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); + } + + if (m_turret.hasReachedVelocity() || isForceLaunch) { isIntakeLaunching = true; m_intake.setIntakeState(Intake.IntakeState.LAUNCH); } @@ -182,6 +190,7 @@ public class StateMachine { public void forceLaunchSequence() { setGameState(GameState.SCORING); + isForceLaunch = true; m_transfer.updateTurretReady(true); m_intake.setIntakeState(Intake.IntakeState.LAUNCH); isIntakeLaunching = true; 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 98f0845..bec8a2b 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 @@ -15,6 +15,8 @@ import org.firstinspires.ftc.teamcode.util.FPSCounter; import com.qualcomm.hardware.lynx.LynxModule; import java.util.List; +import com.pedropathing.math.Vector; + public class finalTeleOp { @TeleOp(name = "Red Final", group = "FINAL") @@ -91,7 +93,7 @@ public class finalTeleOp { if (stateMachine.getCurrentGameState() != GameState.SCORING) { stateMachine.getTransfer().closeLimiter(); - } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady()) { + } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady(false)) { stateMachine.getTransfer().openLimiter(); } @@ -101,12 +103,14 @@ public class finalTeleOp { follower.update(); Pose currentPose = follower.getPose(); + Vector currentVelocity = follower.getVelocity(); + handleGlobalLogic(); handleDriverInput(); handleOperatorInput(); - stateMachine.update(manualSAM, manualTracking, currentPose); + stateMachine.update(manualSAM, manualTracking, currentPose, currentVelocity); if (gamepad2.a) { stateMachine.getTransfer().openLimiter(); @@ -273,7 +277,7 @@ public class finalTeleOp { 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("Turret Ready", stateMachine.getTurret().isTurretReady(false)); telemetry.addData("Hood Offset", stateMachine.getTurret().offsetHood); 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 9268ee6..3b43674 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 @@ -8,6 +8,7 @@ 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.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; @@ -35,17 +36,14 @@ public class flywheelTuning extends OpMode { 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; @@ -63,6 +61,8 @@ public class flywheelTuning extends OpMode { flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3)); + flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3)); hoodServo = hardwareMap.servo.get("hoodservo"); hoodServo.setDirection(Servo.Direction.REVERSE); @@ -184,7 +184,9 @@ public class flywheelTuning extends OpMode { intake.setPower(0); } - hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX)); + double recoilOffset = (currentVel - applyVel) * -0.000185; + + hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX)); // --- TELEMETRY --- @@ -213,19 +215,19 @@ public class flywheelTuning extends OpMode { } private double getTrigBasedDistance(double targetOffsetAngleVertical) { - return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical)); - // ^ GOAL - CAMERA HEIGHT ^ Offset Degrees + return (29.5-12.75) / Math.tan(Math.toRadians(19.8 + 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); + return (-0.0000144121 * Math.pow(dist, 4) + + 0.00398607 * Math.pow(dist, 3) + - 0.342198 * Math.pow(dist, 2) + + (16.43014 * dist) + + 1198.24434); } 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); + return (((3.55794 * Math.pow(10, -8)) * Math.pow(dist, 4)) + (-0.00000995887 * Math.pow(dist, 3)) - (0.000924672 * Math.pow(dist, 2)) + (-0.037262 * dist) + 0.943067); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ledTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ledTest.java new file mode 100644 index 0000000..cdcce16 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ledTest.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ServoImplEx; +import com.qualcomm.robotcore.hardware.PwmControl; +import com.bylazar.configurables.annotations.Configurable; + +@Configurable +@TeleOp(name = "GoBilda RGB Indicator Test", group = "Tests") +public class ledTest extends OpMode { + + // Declare the light as a ServoImplEx + private ServoImplEx rgbLight; + + public static double LED1 = 0.33; + public static double LED2 = 0.66; + public static double LED3 = 0.99; + + private boolean lightOn = false; + + private double lightPosition = 0; + + + + + @Override + public void init() { + rgbLight = hardwareMap.get(ServoImplEx.class, "prism"); + + rgbLight.setPwmRange(new PwmControl.PwmRange(500, 2500)); + + telemetry.addData("Status", "Initialized. Ready to test colors."); + } + + @Override + public void loop() { + + if (gamepad1.psWasPressed()) { + lightOn = !lightOn; + } + + if (gamepad1.a) { + lightPosition = LED1; + } + else if (gamepad1.b) { + lightPosition = 0; + } + else if (gamepad1.x) { + lightPosition = LED2; + } + else if (gamepad1.y) { + lightPosition = LED3; + } + + if (gamepad1.right_trigger > 0.1) { + lightPosition = gamepad1.right_trigger; + } + + if (lightOn) { + rgbLight.setPwmEnable(); + } else { + rgbLight.setPwmDisable(); + } + + rgbLight.setPosition(lightPosition); + + telemetry.addData("Current Light Position", rgbLight.getPosition()); + telemetry.update(); + } +} \ No newline at end of file