diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index 8c796da..5305330 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -3,6 +3,7 @@ import java.text.SimpleDateFormat // // build.gradle in FtcRobotController // + apply plugin: 'com.android.library' android { diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 93b4474..878287a 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -10,34 +10,10 @@ // Custom definitions may go here -repositories { - // Dairy releases repository - maven { - url = "https://repo.dairy.foundation/releases" - } - // Dairy snapshots repository - maven { - url = "https://repo.dairy.foundation/snapshots" - } -} - -buildscript { - repositories { - mavenCentral() - maven { - url "https://repo.dairy.foundation/releases" - } - } - dependencies { - classpath "dev.frozenmilk:Load:0.2.4" - } -} - // Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' -apply plugin: 'dev.frozenmilk.sinister.sloth.load' android { namespace = 'org.firstinspires.ftc.teamcode' @@ -49,6 +25,4 @@ android { dependencies { implementation project(':FtcRobotController') - implementation("dev.frozenmilk.sinister:Sloth:0.2.4") - implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12") } 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 228227d..c443448 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 @@ -172,7 +172,7 @@ public class hwMap { public NormalizedColorSensor[] sensors; public TransferHwMap(HardwareMap hardwareMap) { - Limiter = hardwareMap.servo.get("limiter"); + Limiter = hardwareMap.servo.get(Constants.TransferConstants.LIMITER_SERVO); indexA = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_A); @@ -259,8 +259,8 @@ public class hwMap { turretLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR); turretRightMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR); - turretLeftMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_L_DIRECTION); - turretRightMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_R_DIRECTION); + turretLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD); + turretRightMotor.setDirection(DcMotorSimple.Direction.FORWARD); turretRightMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); turretLeftMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); @@ -271,11 +271,12 @@ public class hwMap { // --- Turret Rotation Motor --- turretrotation = hardwareMap.dcMotor.get(Constants.TurretConstants.TURRET_ROTATION_MOTOR); turretrotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + turretrotation.setDirection(Constants.TurretConstants.TURRET_ROTATION_DIR); turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // --- Hood Servo --- hoodservo = hardwareMap.servo.get(Constants.TurretConstants.HOOD_TURRET_SERVO); - hoodservo.setDirection(Servo.Direction.FORWARD); + hoodservo.setDirection(Servo.Direction.REVERSE); // --- External Encoder (if used separately) --- 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 87e6271..311cd3a 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 @@ -18,10 +18,16 @@ import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants; public class Constants { public static FollowerConstants followerConstants = new FollowerConstants() - .mass(12); + .mass(12) + .translationalPIDFCoefficients(new PIDFCoefficients(0.26, 0, 0.01, 0.03)) + .headingPIDFCoefficients(new PIDFCoefficients(1.8, 0, 0.01, 0.03)) + .forwardZeroPowerAcceleration(-39.64668514000648) + .lateralZeroPowerAcceleration(-76.57271150137258); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(0.8) + .xVelocity(68.10320673181904) + .yVelocity(57.52038399624941) .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR) .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR) .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR) @@ -37,13 +43,13 @@ public class Constants { public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants() .forwardTicksToInches(0.001978956) .strafeTicksToInches(0.001978956) - .turnTicksToInches(0.001978956) + .turnTicksToInches(0.0022824233082645137) .leftPodY(3.75) .rightPodY(-3.25) .strafePodX(-6.25) .leftEncoder_HardwareMapName("intake") .rightEncoder_HardwareMapName("fl") - .strafeEncoder_HardwareMapName("fr") + .strafeEncoder_HardwareMapName("bl") .leftEncoderDirection(Encoder.FORWARD) .rightEncoderDirection(Encoder.FORWARD) .strafeEncoderDirection(Encoder.FORWARD); 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 d154c1a..619b410 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 @@ -292,7 +292,7 @@ public class Turret { } private double getHoodPOS(double dist) { - return ((-(3.62429 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000638975 * Math.pow(dist, 3) - (0.000153252 * Math.pow(dist, 2)) + (-0.0197027 * dist) + 1.40511); + 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); } public void setHoodPos(double pos) { 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 c402f6e..7cd7496 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 @@ -31,11 +31,6 @@ public class Constants { public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; public static final double TICKS_PER_REVOLUTION = 537.6; - // Drive characteristics - public static final double MAX_VELOCITY = 30.0; // inches per second - public static final double MAX_ANGULAR_VELOCITY = Math.toRadians(180.0); // radians per second - public static final double MAX_ACCELERATION = 30.0; // inches per second squared - public static final double STOP_SPEED_MULTIPLIER = 0.0; public static final double PRECISION_SPEED_MULTIPLIER = 0.3; public static final double NORMAL_SPEED_MULTIPLIER = 0.6; @@ -45,7 +40,7 @@ public class Constants { public static class IntakeConstants { public static final String FRONT_INTAKE_MOTOR = "intake"; - public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD; public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE; public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER; @@ -74,7 +69,7 @@ public class Constants { } public static class TransferConstants { - public static final String LIMITER_SERVO = "activeTransfer"; + public static final String LIMITER_SERVO = "limiter"; public static final Servo.Direction LIMITER_SERVO_DIR = Servo.Direction.FORWARD; @@ -83,20 +78,20 @@ public class Constants { public static final String INDEX_SENSOR_B = "colorB"; public static final String INDEX_SENSOR_C = "colorC"; - public static final double LIMIT_POS = 0.99; + public static final double LIMIT_POS = 1; - public static final double UNLOCK_POS = 0.81; + public static final double UNLOCK_POS = 0.82; } public static class TurretConstants { public static final double TICKS_PER_REV_MOTOR = 384.5; - public static final double EXTERNAL_GEAR_RATIO = 1.315994798439532; + public static final double EXTERNAL_GEAR_RATIO = 2.68888889; public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0; - public static final double KP = 0.028; + public static final double KP = 0.02; public static final double KI = 0.0; - public static final double KD = 0.00055; + public static final double KD = 0.001; public static final double MAX_POWER = 0.6; public static final double MAX_INTEGRAL = 0.5; @@ -107,12 +102,14 @@ public class Constants { 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 = -6; - public static final double LL_LOGIC_MULTIPLIER = -1.0; - public static final double OFFSET_SMOOTHING = 0.3; + 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 final double SOFT_LIMIT_NEG = -100.0; - public static final double SOFT_LIMIT_POS = 250.0; + 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 final double HOOD_POS_CLOSE = 0.3; public static final double HOOD_POS_FAR = 0.7; @@ -130,25 +127,25 @@ public class Constants { public static final double INTAKE_POWER = -0.04; public static final String TURRET_ROTATION_MOTOR = "turretRotation"; - public static final String HOOD_TURRET_SERVO = "hoodServo"; + public static final String HOOD_TURRET_SERVO = "hoodservo"; public static final Servo.Direction HOOD_SERVO_DIR = Servo.Direction.FORWARD; public static final String TURRET_RIGHT_MOTOR = "rturret"; public static final String TURRET_LEFT_MOTOR = "lturret"; - public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.FORWARD; public static final DcMotorSimple.Direction TURRET_MOTOR_L_DIRECTION = DcMotorSimple.Direction.REVERSE; public static final int LIMELIGHT_PIPELINE_MOTIF = 0; 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 = 15.5; + 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 = 2.0; + public static final double CAMERA_MOUNT_ANGLE_DEGREES = 7.9; - public static final double SERVO_MAX = 0.7; - public static final double SERVO_MIN = 0.26; + public static final double SERVO_MAX = 0.5; + public static final double SERVO_MIN = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java index 89946f5..25ba556 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java @@ -7,6 +7,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.teamcode.Hware.hwMap; + +@Disabled @TeleOp(name="Color Sensor Tuning", group="Diagnostics") public class ColorSensorTuning extends LinearOpMode { 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 faff252..faa45c8 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 @@ -1,4 +1,70 @@ package org.firstinspires.ftc.teamcode.tuning; -public class PIDFTuning { -} +import com.bylazar.configurables.annotations.Configurable; +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.PIDFCoefficients; + +@Configurable +@TeleOp +public class PIDFTuning extends OpMode { + + public DcMotorEx flywheelMotorR; + public DcMotorEx flywheelMotorL; + + public static double targetVelocity = 0; + + public static double targetVelocity1 = 1500; + public static double targetVelocity2 = 900; + public static double f = 0; + public static double p = 0; + + public static double d = 0; + + public static boolean reverse = false; + + @Override + public void init() { + flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret"); + flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE); + + flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret"); + flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + + telemetry.addLine("Init complete"); + } + + @Override + public void loop() { + PIDFCoefficients pidfCoefficients = new PIDFCoefficients(p, 0, d, f); + flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + + flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + + flywheelMotorR.setVelocity(targetVelocity); + flywheelMotorL.setVelocity(targetVelocity); + + if (gamepad1.aWasPressed()) { + targetVelocity = targetVelocity1; + } else if (gamepad1.bWasPressed()) { + targetVelocity = targetVelocity2; + } + + + double curVelocity = flywheelMotorR.getVelocity(); + double error = targetVelocity - curVelocity; + + telemetry.addData("Target Velocity", targetVelocity); + telemetry.addData("Current Velocity", curVelocity); + telemetry.addData("Error", error); + telemetry.addData("Tuning P", p); + telemetry.addData("Tuning F", f); + + telemetry.update(); + } +} \ 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 c2e59ba..f7d48b7 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 @@ -9,30 +9,36 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; +import org.firstinspires.ftc.teamcode.util.FPSCounter; + @Configurable @TeleOp(name = "Anchor Mode Tuner", group = "Tuning") public class anchorTuning extends LinearOpMode { - public static double driveP = -0.001; + public static double driveP = 0.0; public static double driveI = 0.0; - public static double driveD = -0.00015; + public static double driveD = 0.0; - public static double strafeP = -0.002; + public static double strafeP = 0.26; public static double strafeI = 0.0; - public static double strafeD = -0.00008; + public static double strafeD = 0.01; - public static double turnP = -0.08; + public static double turnP = 1.8; public static double turnI = 0.0; - public static double turnD = -0.0001; // Tuned for Degrees + public static double turnD = 0.01; // Tuned for Degrees - public static double MAX_POWER = 0.2; + public static double MAX_POWER = 0.4; + + public static int targetFPS = 110; private DriveTrain driveTrain; private TelemetryManager telemetryM; private boolean lastPS = false; + FPSCounter fps = new FPSCounter(); + @Override public void runOpMode() throws InterruptedException { // Initialize Telemetry @@ -44,11 +50,14 @@ public class anchorTuning extends LinearOpMode { telemetryM.debug("Status", "Initialized. Press PS/Guide to Toggle Anchor."); telemetryM.update(telemetry); + fps.reset(); waitForStart(); while (opModeIsActive()) { // Update PID values from Dashboard to Subsystem in real-time + + fps.startLoop(); driveTrain.updateAnchorPID( driveP, driveI, driveD, strafeP, strafeI, strafeD, @@ -74,8 +83,12 @@ 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()); + telemetryM.debug("Error FWD (Ticks)", driveTrain.getLastForwardError()); telemetryM.debug("Error STR (Ticks)", driveTrain.getLastStrafeError()); telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError()); 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 new file mode 100644 index 0000000..1127af5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java @@ -0,0 +1,443 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.bylazar.configurables.annotations.Configurable; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +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.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.util.FPSCounter; + +import com.qualcomm.hardware.lynx.LynxModule; +import java.util.List; + +@Configurable +@TeleOp(name = "Auto-Collect Testing", group = "Turret") +public class autoCollection extends LinearOpMode { + + public static double kP = 0.02; + public static double kI = 0.0; + public static double kD = 0.001; + public static double MAX_POWER = 0.6; + public static double MAX_INTEGRAL = 0.5; + + 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 LL_LOGIC_MULTIPLIER = 1.0; + public static double SOFT_LIMIT_NEG = -230; + public static double SOFT_LIMIT_POS = 230; + + 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 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 BLUE_OBSERVATION_ANGLE = 160; + public static double RED_OBSERVATION_ANGLE = -160; + public static double BLUE_RAY_TARGET_X = 4.0; + public static double RED_RAY_TARGET_X = 140.0; + public static double BLUE_DRIVE_TARGET_X = 8.5; + public static double RED_DRIVE_TARGET_X = 135.5; + + public static double CLUSTER_RADIUS_DEG = 5.0; + public static double MIN_SNAPSHOT_WAIT_MS = 300; + public static double TURRET_STABLE_ERROR_DEG = 2.0; + + public static int PIPELINE_GOAL_BLUE = 1; + public static int PIPELINE_GOAL_RED = 2; + public static int PIPELINE_ARTIFACTS = 3; + + public static double OFFSET_SMOOTHING = 0.2; + public static int targetFPS = 110; + + private Limelight3A limelight; + private DcMotorEx turretMotor; + private DcMotor intake; + private Follower follower; + + private double integralSum = 0; + private double lastError = 0; + private double zeroOffsetTicks = 0; + private double ticksPerDegree = 0; + private double targetCorrectionOffsetTicks = 0; + private double fusedTargetTicks = 0; + + private enum AutoCollectState { + IDLE, + INIT_COLLECT, + WAIT_FOR_TURRET_AND_VISION, + CALCULATE_SNAPSHOT, + DRIVING_TO_INTAKE, + PATH_RETURN, + DRIVING_RETURN + } + + private AutoCollectState autoCollectState = AutoCollectState.IDLE; + + private enum Alliance { RED, BLUE } + private Alliance currentAlliance = Alliance.BLUE; + private Alliance lastAlliance = null; + + private Pose returnPose; + private final ElapsedTime sequenceTimer = new ElapsedTime(); + + private final ElapsedTime timer = new ElapsedTime(); + protected FPSCounter fpsCounter = new FPSCounter(); + protected List allHubs; + + public static double start_x = 72; + public static double start_y = 8.5; + public static double start_heading = 90; + + @Override + public void runOpMode() { + turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + intake = hardwareMap.get(DcMotor.class, "intake"); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading))); + + turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + applyMotorDirection(); + recalculateTicksPerDegree(); + + limelight.start(); + + telemetry.addData("Status", "Initialized. Auto-Track is Armed."); + telemetry.update(); + + waitForStart(); + timer.reset(); + + follower.startTeleopDrive(); + fpsCounter.reset(); + + while (opModeIsActive()) { + for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + fpsCounter.startLoop(); + + follower.update(); + + double dt = timer.seconds(); + timer.reset(); + if (dt < 0.001) dt = 0.001; + + double rawTicks = turretMotor.getCurrentPosition(); + double currentTicks = rawTicks - zeroOffsetTicks; + double currentDegrees = currentTicks / ticksPerDegree; + + handleInput(); + + if (autoCollectState == AutoCollectState.IDLE) { + updateAlliancePipeline(); + } + + updateTurretTarget(currentTicks, currentDegrees); + + runPidControl(currentTicks, dt); + + switch (autoCollectState) { + case IDLE: + follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + true + ); + break; + + case INIT_COLLECT: + returnPose = follower.getPose(); + + limelight.pipelineSwitch(PIPELINE_ARTIFACTS); + sequenceTimer.reset(); + + autoCollectState = AutoCollectState.WAIT_FOR_TURRET_AND_VISION; + break; + + case WAIT_FOR_TURRET_AND_VISION: + double errorDeg = Math.abs(fusedTargetTicks - currentTicks) / ticksPerDegree; + if (errorDeg <= TURRET_STABLE_ERROR_DEG && sequenceTimer.milliseconds() > MIN_SNAPSHOT_WAIT_MS) { + autoCollectState = AutoCollectState.CALCULATE_SNAPSHOT; + } + break; + + case CALCULATE_SNAPSHOT: + boolean snapshotSuccess = calculateSnapshotAndPath(currentDegrees); + if (snapshotSuccess) { + intake.setPower(1.0); + autoCollectState = AutoCollectState.DRIVING_TO_INTAKE; + } else { + abortAutoCollectSequence(); + } + break; + + case DRIVING_TO_INTAKE: + if (!follower.isBusy()) { + autoCollectState = AutoCollectState.PATH_RETURN; + } + break; + + case PATH_RETURN: + PathChain returnChain = follower.pathBuilder() + .addPath(new BezierLine(follower.getPose(), returnPose)) + .setLinearHeadingInterpolation(follower.getPose().getHeading(), returnPose.getHeading()) + .build(); + + follower.followPath(returnChain, true); + autoCollectState = AutoCollectState.DRIVING_RETURN; + break; + + case DRIVING_RETURN: + if (!follower.isBusy()) { + abortAutoCollectSequence(); + } + break; + } + + sleep(fpsCounter.getSyncTime(targetFPS)); + updateTelemetry(currentTicks, currentDegrees); + } + limelight.stop(); + } + + private boolean calculateSnapshotAndPath(double turretCurrentDegrees) { + LLResult result = limelight.getLatestResult(); + if (result == null || !result.isValid()) return false; + + List targets = result.getColorResults(); + if (targets == null || targets.isEmpty()) return false; + + if (targets.size() > 8) { + targets = targets.subList(0, 8); + } + + LLResultTypes.ColorResult bestTarget = null; + int bestCount = 0; + + for (int i = 0; i < targets.size(); i++) { + LLResultTypes.ColorResult a = targets.get(i); + int count = 0; + for (int j = 0; j < targets.size(); j++) { + if (i == j) continue; + LLResultTypes.ColorResult b = targets.get(j); + double dx = a.getTargetXDegrees() - b.getTargetXDegrees(); + double dy = a.getTargetYDegrees() - b.getTargetYDegrees(); + double dist = Math.sqrt(dx * dx + dy * dy); + if (dist < CLUSTER_RADIUS_DEG) { + count++; + } + } + if (count >= bestCount) { + bestCount = count; + bestTarget = a; + } + } + + double sumTx = 0; + int clusterSize = 0; + for (LLResultTypes.ColorResult t : targets) { + double dx = bestTarget.getTargetXDegrees() - t.getTargetXDegrees(); + double dy = bestTarget.getTargetYDegrees() - t.getTargetYDegrees(); + double dist = Math.sqrt(dx * dx + dy * dy); + if (dist < CLUSTER_RADIUS_DEG) { + sumTx += t.getTargetXDegrees(); + clusterSize++; + } + } + double avgTx = sumTx / clusterSize; + + Pose robotPose = follower.getPose(); + + double rayHeadingRad = robotPose.getHeading() + Math.toRadians(turretCurrentDegrees) + Math.toRadians(avgTx * LL_LOGIC_MULTIPLIER); + + while (rayHeadingRad > Math.PI) rayHeadingRad -= (2 * Math.PI); + while (rayHeadingRad < -Math.PI) rayHeadingRad += (2 * Math.PI); + + double dx = (currentAlliance == Alliance.BLUE) ? + (BLUE_RAY_TARGET_X - robotPose.getX()) : + (RED_RAY_TARGET_X - robotPose.getX()); + + double dy = dx * Math.tan(rayHeadingRad); + double targetY = robotPose.getY() + dy; + + if (targetY < 0 || targetY > 144) { + return false; + } + + Pose targetPose; + if (currentAlliance == Alliance.BLUE) { + targetPose = new Pose(BLUE_DRIVE_TARGET_X, targetY, Math.toRadians(180)); + } else { + targetPose = new Pose(RED_DRIVE_TARGET_X, targetY, Math.toRadians(0)); + } + + PathChain collectPath = follower.pathBuilder() + .addPath(new BezierLine(robotPose, targetPose)) + .setLinearHeadingInterpolation(robotPose.getHeading(), targetPose.getHeading()) + .build(); + + follower.followPath(collectPath, true); + return true; + } + + private void updateTurretTarget(double currentTicks, double currentDegrees) { + if (autoCollectState == AutoCollectState.IDLE) { + double odoTargetTicks = calculateOdometryTargetTicks(); + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks); + targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING)) + (rawErrorTicks * OFFSET_SMOOTHING); + } + double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks; + fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS); + + } else if (autoCollectState == AutoCollectState.INIT_COLLECT || autoCollectState == AutoCollectState.WAIT_FOR_TURRET_AND_VISION) { + double obsAngle = (currentAlliance == Alliance.BLUE) ? BLUE_OBSERVATION_ANGLE : RED_OBSERVATION_ANGLE; + fusedTargetTicks = obsAngle * ticksPerDegree; + + } else { + fusedTargetTicks = 0; + targetCorrectionOffsetTicks = 0; + } + } + + private double calculateOdometryTargetTicks() { + Pose robotPose = follower.getPose(); + double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X; + double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y; + + double dx = targetX - robotPose.getX(); + double dy = targetY - robotPose.getY(); + + double targetFieldHeading = Math.atan2(dy, dx); + double robotHeading = robotPose.getHeading(); + + double relativeRad = targetFieldHeading - robotHeading; + + while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI); + while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI); + + double relativeDegrees = Math.toDegrees(relativeRad); + double staticOffset = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES; + + return (relativeDegrees + staticOffset) * ticksPerDegree; + } + + private void runPidControl(double currentTicks, double dt) { + double error = fusedTargetTicks - currentTicks; + double derivative = (error - lastError) / dt; + + if (Math.abs(error) < (15 * ticksPerDegree)) { + integralSum += (error * dt); + } else { + integralSum = 0; + } + + double integralTerm = kI * integralSum; + if (Math.abs(integralTerm) > MAX_INTEGRAL) { + integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL; + } + + double output = (kP * error) + integralTerm + (kD * derivative); + output = Range.clip(output, -MAX_POWER, MAX_POWER); + + turretMotor.setPower(output); + lastError = error; + } + + 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 void handleInput() { + if (gamepad1.psWasPressed()) { + currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED; + } + + if (gamepad1.x && autoCollectState == AutoCollectState.IDLE) { + autoCollectState = AutoCollectState.INIT_COLLECT; + } + + if (gamepad1.b && autoCollectState != AutoCollectState.IDLE) { + abortAutoCollectSequence(); + } + } + + private void abortAutoCollectSequence() { + autoCollectState = AutoCollectState.IDLE; + intake.setPower(0); + follower.startTeleopDrive(); + lastAlliance = null; + targetCorrectionOffsetTicks = 0; + } + + private void updateAlliancePipeline() { + if (lastAlliance != currentAlliance) { + if (currentAlliance == Alliance.RED) { + limelight.pipelineSwitch(PIPELINE_GOAL_RED); + } else { + limelight.pipelineSwitch(PIPELINE_GOAL_BLUE); + } + lastAlliance = currentAlliance; + } + } + + private void updateTelemetry(double currentTicks, double currentDegrees) { + telemetry.addData("State", autoCollectState); + telemetry.addData("Alliance", currentAlliance); + telemetry.addLine("------------------"); + telemetry.addData("Target Ticks", fusedTargetTicks); + telemetry.addData("Current Ticks", currentTicks); + telemetry.addData("Error Ticks", fusedTargetTicks - currentTicks); + telemetry.addLine("------------------"); + telemetry.addData("FPS", "%.2f", fpsCounter.getAverageFps()); + telemetry.update(); + } + + private void applyMotorDirection() { + turretMotor.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + } + + private void recalculateTicksPerDegree() { + ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java index 82a60b8..7432e04 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.bylazar.telemetry.PanelsTelemetry; - +@Disabled @TeleOp(name = "Encoders Test", group = "Test") public class encoderTest extends OpMode { 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 8c24189..91c2c9e 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 @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode.tuning; -import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES; - import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -9,7 +7,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; 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; @@ -17,7 +14,7 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.teleOp.Constants; @Configurable @TeleOp -public class PIDFTuning extends OpMode { +public class flywheelTuning extends OpMode { private Limelight3A limelight; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index f14d25b..216984d 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -17,6 +17,6 @@ dependencies { implementation 'com.pedropathing:ftc:2.0.6' implementation 'com.pedropathing:telemetry:1.0.0' - //implementation 'com.bylazar:fullpanels:1.0.9' + implementation 'com.bylazar:fullpanels:1.0.9' }