stash

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java

@@ -1,4 +0,0 @@
-package org.firstinspires.ftc.teamcode.constants;
-
-public class blank {
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.libs.RR;
 
+
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.canvas.Canvas;
@@ -211,6 +212,7 @@ public final class MecanumDrive {
                     headingDelta
             ));
 
+
             return twist.velocity().value();
         }
     }
@@ -222,7 +224,7 @@ public final class MecanumDrive {
             module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
         }
 
-        // TODO: make sure your config has motors with these names (or change them)
+        // TODO: make sure your HardwareConfig has motors with these names (or change them)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
         leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
@@ -237,7 +239,7 @@ public final class MecanumDrive {
         // TODO: reverse motor directions if needed
         //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java

@@ -28,7 +28,7 @@ public class OTOSLocalizer implements Localizer {
     private Pose2d currentPose;
 
     public OTOSLocalizer(HardwareMap hardwareMap, Pose2d initialPose) {
-        // TODO: make sure your config has an OTOS device with this name
+        // TODO: make sure your HardwareConfig has an OTOS device with this name
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
         currentPose = initialPose;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java

@@ -29,7 +29,7 @@ public final class PinpointLocalizer implements Localizer {
     private Pose2d txPinpointRobot = new Pose2d(0, 0, 0);
 
     public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has a Pinpoint device with this name
+        // TODO: make sure your HardwareConfig has a Pinpoint device with this name
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         driver = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java

@@ -232,7 +232,7 @@ public final class TankDrive {
             module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
         }
 
-        // TODO: make sure your config has motors with these names (or change them)
+        // TODO: make sure your HardwareConfig has motors with these names (or change them)
         //   add additional motors on each side if you have them
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
@@ -248,7 +248,7 @@ public final class TankDrive {
         // TODO: reverse motor directions if needed
         //   leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
 
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java

@@ -37,7 +37,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
     private Pose2d pose;
 
     public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has **motors** with these names (or change them)
+        // TODO: make sure your HardwareConfig has **motors** with these names (or change them)
         //   the encoders should be plugged into the slot matching the named motor
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java

@@ -46,7 +46,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
     private Pose2d pose;
 
     public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has **motors** with these names (or change them)
+        // TODO: make sure your HardwareConfig has **motors** with these names (or change them)
         //   the encoders should be plugged into the slot matching the named motor
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java

@@ -6,10 +6,10 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.subsystems.Limelight;
 
-
 @Autonomous
 @Config
 
@@ -21,12 +21,21 @@ public class LimelightTest extends LinearOpMode {
 
     MultipleTelemetry TELE;
 
+
+
+
+
+
     Robot robot;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
 
+
+        HardwareConfig.USING_LL= true;
+
+
         robot = new Robot(hardwareMap);
 
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -51,6 +60,8 @@ public class LimelightTest extends LinearOpMode {
 
             TELE.update();
 
+
+
         }
 
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+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 org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class ShooterTest extends LinearOpMode {
+    public static int mode = 0;
+    public static double parameter = 0.0;
+    // --- CONSTANTS YOU TUNE ---
+    public static double hoodPos = 0.501;
+    double initPos = 0.0;
+    double stamp = 0.0;
+    double stamp1 = 0.0;
+    double ticker = 0.0;
+    double currentPos = 0.0;
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
+    double initPosq = 0.0;
+    double stampq = 0.0;
+    double stamp1q = 0.0;
+    double tickerq = 0.0;
+    double currentPosq = 0.0;
+    double veloq = 0.0;
+    double velo1q = 0.0;
+    double velo2q = 0.0;
+    double velo3q = 0.0;
+    double velo4q = 0.0;
+    double velo5q = 0.0;
+    Robot robot;
+    Flywheel flywheel;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+        DcMotorEx leftShooter = robot.shooter1;
+        DcMotorEx rightShooter = robot.shooter2;
+        flywheel = new Flywheel();
+
+        MultipleTelemetry TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            if (mode == 0) {
+                rightShooter.setPower(parameter);
+                leftShooter.setPower(parameter);
+                leftShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                rightShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                if (ticker % 2 == 0) {
+                    velo5 = velo4;
+                    velo4 = velo3;
+                    velo3 = velo2;
+                    velo2 = velo1;
+
+                    currentPos = (double) leftShooter.getCurrentPosition() / 28;
+                    stamp = getRuntime();
+                    velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
+                    initPos = currentPos;
+                    stamp1 = stamp;
+
+                    velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
+                }
+                if (tickerq % 2 == 0) {
+                    velo5q = velo4q;
+                    velo4q = velo3q;
+                    velo3q = velo2q;
+                    velo2q = velo1q;
+
+                    currentPosq = (double) rightShooter.getCurrentPosition() / 28;
+                    stampq = getRuntime();
+                    velo1q = 60 * ((currentPosq - initPosq) / (stampq - stamp1q));
+                    initPosq = currentPosq;
+                    stamp1q = stampq;
+
+                    veloq = (velo1q + velo2q + velo3q + velo4q + velo5q) / 5;
+                }
+            } else if (mode == 1) {
+                leftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                rightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                rightShooter.setPower(powPID);
+                leftShooter.setPower(powPID);
+                TELE.addData("PIDPower", powPID);
+            }
+
+            if (hoodPos != 0.501) {
+                robot.hood.setPosition(hoodPos);
+            }
+
+            TELE.addData("Velocity1", velo);
+            TELE.addData("Velocity2", veloq);
+            TELE.addData("Encoder Velocity", flywheel.getVelo());
+            TELE.addData("Power", robot.shooter1.getPower());
+            TELE.addData("Steady?", flywheel.getSteady());
+            TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28);
+            TELE.addData("Position2", robot.shooter2.getCurrentPosition()/28);
+
+            TELE.update();
+
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java

@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class Flywheel {
+    public static double kP = 0.001;           // small proportional gain (tune this)
+    public static double maxStep = 0.06;         // prevents sudden jumps
+    double initPos = 0.0;
+    double stamp = 0.0;
+    double stamp1 = 0.0;
+    double ticker = 0.0;
+    double currentPos = 0.0;
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
+    double targetVelocity = 0.0;
+    double powPID = 0.0;
+    boolean steady = false;
+    public Flywheel () {
+        //robot = new Robot(hardwareMap);
+    }
+
+    public double getVelo () {
+        return velo;
+    }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    private double getTimeSeconds ()
+    {
+        return (double) System.currentTimeMillis()/1000.0;
+    }
+
+
+    public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
+        targetVelocity = commandedVelocity;
+
+        ticker++;
+        if (ticker % 2 == 0) {
+            velo5 = velo4;
+            velo4 = velo3;
+            velo3 = velo2;
+            velo2 = velo1;
+
+            currentPos = shooter1CurPos / 2048;
+            stamp = getTimeSeconds(); //getRuntime();
+            velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
+            initPos = currentPos;
+            stamp1 = stamp;
+
+            velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
+        }
+        // Flywheel control code here
+        if (targetVelocity - velo > 500) {
+            powPID = 1.0;
+        } else if (velo - targetVelocity > 500){
+            powPID = 0.0;
+        } else {
+            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = targetVelocity - velo;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+        }
+
+        // really should be a running average of the last 5
+        steady = (Math.abs(targetVelocity - velo) < 100.0);
+
+        return powPID;
+    }
+
+    public void update()
+    {
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -1,7 +1,15 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*;
+
 import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
 
 public class Robot {
 
@@ -9,11 +17,43 @@ public class Robot {
 
     public Limelight3A limelight3A;
 
+    public IMU imu;
+
+    public DcMotorEx shooter1;
+
+    public DcMotorEx shooter2;
+
+    public Servo hood;
+
     public Robot (HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
 
         limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
 
+        shooter1 = hardwareMap.get(DcMotorEx.class, "s1");
+        shooter2 = hardwareMap.get(DcMotorEx.class, "s2");
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        hood = hardwareMap.get(Servo.class, "hood");
+
+        if (USING_LL) {
+            limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
+            limelight3A.start(); // This tells Limelight to start looking!
+        }
+
+        imu = hardwareMap.get(IMU.class, "imu");
+
+        RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+
+        RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoFacingDirection, usbFacingDirection);
+
+        imu.initialize(new IMU.Parameters(orientationOnRobot));
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java

@@ -1,4 +0,0 @@
-package org.firstinspires.ftc.teamcode.utils.functions;
-
-public class DistanceToTarget {
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java

@@ -6,11 +6,13 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.LLStatus;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.hardware.IMU;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.robotcore.external.navigation.Position;
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Subsystem;
 
@@ -42,17 +44,28 @@ public class Limelight implements Subsystem {
     private double ty = 0.0;
     private double tync = 0.0;
 
+    private double ta = 0.0;
+
     private List<LLResultTypes.BarcodeResult> barcodeResults = new ArrayList<>();
     private List<LLResultTypes.ClassifierResult> classifierResults = new ArrayList<>();
     private List<LLResultTypes.DetectorResult> detectorResults = new ArrayList<>();
     private List<LLResultTypes.FiducialResult> fiducialResults = new ArrayList<>();
     private List<LLResultTypes.ColorResult> colorResults = new ArrayList<>();
 
+    private IMU imu;
+
     public Limelight(Robot robot, MultipleTelemetry tele) {
+
+        HardwareConfig.USING_LL= true;
+
         this.limelight = robot.limelight3A;
         this.telemetry = tele;
         limelight.pipelineSwitch(1);
 
+        this.imu = robot.imu;
+
+        this.imu.resetYaw();
+
 
     }
 
@@ -62,12 +75,19 @@ public class Limelight implements Subsystem {
 
     public void setMode(String newMode) { this.mode = newMode; }
 
+
+
     /** ✅ MAIN UPDATE LOOP */
     @Override
     public void update() {
+
+
+
         result = limelight.getLatestResult();
         status = limelight.getStatus();
 
+
+
         if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){
             // Refresh all cached values
             botpose         = result.getBotpose();
@@ -79,6 +99,7 @@ public class Limelight implements Subsystem {
             txnc            = result.getTxNC();
             ty              = result.getTy();
             tync            = result.getTyNC();
+            ta              = result.getTa();
             barcodeResults  = result.getBarcodeResults();
             classifierResults = result.getClassifierResults();
             detectorResults   = result.getDetectorResults();
@@ -105,6 +126,7 @@ public class Limelight implements Subsystem {
 
 
 
+
         if (result != null && result.isValid()) {
             telemetry.addData("LL Latency", getTotalLatency());
             telemetry.addData("Capture Latency", getCaptureLatency());
@@ -115,7 +137,17 @@ public class Limelight implements Subsystem {
             telemetry.addData("txnc", getTxNC());
             telemetry.addData("ty", getTy());
             telemetry.addData("tync", getTyNC());
-            telemetry.addData("Botpose", getBotPose().toString());
+            telemetry.addData("ta", getTa());
+
+
+
+
+            telemetry.addData("BotX", getBotX());
+
+            telemetry.addData("BotY", getBotY());
+
+
+
 
 
             if (Objects.equals(mode, "BR"))
@@ -170,6 +202,15 @@ public class Limelight implements Subsystem {
     public double getTy() { return ty; }
     public double getTyNC() { return tync;}
 
+    public double getTa() {return ta;}
+
+    public double getBotX() {return getBotPose().getPosition().x;}
+
+    public double getBotY() {return getBotPose().getPosition().y;}
+
+
+
+
     public List<LLResultTypes.BarcodeResult> getBarcodeResults() { return barcodeResults; }
     public List<LLResultTypes.ClassifierResult> getClassifierResults() { return classifierResults; }
     public List<LLResultTypes.DetectorResult> getDetectorResults() { return detectorResults; }
@@ -178,4 +219,6 @@ public class Limelight implements Subsystem {
 
     public LLStatus getStatus() { return status; }
     public LLResult getRawResult() { return result; }
+
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/variables/HardwareConfig.java

@@ -1,10 +1,12 @@
-package org.firstinspires.ftc.teamcode.utils;
+package org.firstinspires.ftc.teamcode.variables;
 
 
 import com.acmerobotics.dashboard.config.Config;
 
 @Config
-public class Vars {
+public class HardwareConfig {
 
     public static boolean USING_LL = false;
+
+
 }