Shooter Test

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -34,7 +35,7 @@ public class AprilTagWebcam {
                 .build();
 
         VisionPortal.Builder builder = new VisionPortal.Builder();
-        builder.setCamera(robot.webcamName);
+        builder.setCamera(robot.webcam);
         builder.setCameraResolution(new Size(640, 480));
         builder.addProcessor(aprilTagProcessor);
 

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

@@ -1,25 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-
-public class Robot {
-
-    public DcMotorEx shooter1;
-    public DcMotorEx shooter2;
-    public WebcamName webcamName;
-
-    public Robot(HardwareMap hardwareMap) {
-
-        webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
-        shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
-        shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
-
-        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
-
-    }
-
-}

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

@@ -7,7 +7,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
-import org.firstinspires.ftc.teamcode.subsystems.Robot;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
 @TeleOp

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

@@ -1,16 +1,13 @@
 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.arcrobotics.ftclib.controller.PIDController;
-import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
-import org.firstinspires.ftc.teamcode.subsystems.Robot;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
 @TeleOp
@@ -18,35 +15,32 @@ public class ShooterTest extends LinearOpMode {
 
     public static int mode = 0;
     public static double parameter = 0.0;
-    Robot robot;
-    private DcMotorEx leftShooter;
-    private DcMotorEx rightShooter;
-    private DcMotorEx encoder;
-    private double encoderRevolutions = 0.0;
-    private double lastEncoderRevolutions = 0.0;
-    private double timeStamp = 0.0;
-    private double lastTimeStamp = 0.0;
-
-
     // --- CONSTANTS YOU TUNE ---
     public static double MAX_RPM = 2500;      // your measured max RPM
     public static double kP = 0.01;           // small proportional gain (tune this)
     public static double maxStep = 0.2;         // prevents sudden jumps
 
+    //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
+    public static double transferPower = 0.0;
+    public static double hoodPos = 0.501;
+
+    public static double turretPos = 0.501;
+    Robot robot;
+    private double lastEncoderRevolutions = 0.0;
+    private double lastTimeStamp = 0.0;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
         robot = new Robot(hardwareMap);
-        leftShooter = robot.shooter1;
-        rightShooter = robot.shooter2;
-        encoder = robot.shooter1;
+        DcMotorEx leftShooter = robot.shooter1;
+        DcMotorEx rightShooter = robot.shooter2;
+        DcMotorEx encoder = robot.shooter1;
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
 
-
-
         waitForStart();
 
         if (isStopRequested()) return;
@@ -55,12 +49,9 @@ public class ShooterTest extends LinearOpMode {
 
             double kF = 1.0 / MAX_RPM;     // baseline feedforward
 
+            double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
 
-
-
-            encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
-            double velocity = -60*(encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
+            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
 
             TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
             TELE.addLine("Parameter = pow, vel, or pos");
@@ -69,15 +60,13 @@ public class ShooterTest extends LinearOpMode {
             TELE.addData("shaftEncoderPos", encoderRevolutions);
             TELE.addData("shaftEncoderVel", velocity);
 
-            double velPID = 0.0;
+            double velPID;
 
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
 
-
-
                 // --- FEEDFORWARD BASE POWER ---
                 double feed = kF * parameter;        // Example: vel=2500 → feed=0.5
 
@@ -102,12 +91,19 @@ public class ShooterTest extends LinearOpMode {
             lastTimeStamp = getRuntime();
             lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
 
+            if (hoodPos != 0.501) {
+                robot.hood.setPosition(hoodPos);
+            }
 
+            if (turretPos!=0.501){
+                robot.turr1.setPosition(turretPos);
+                robot.turr2.setPosition(turretPos);
+            }
+
+            robot.transfer.setPower(transferPower);
 
             TELE.update();
 
-
-
         }
 
     }

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

@@ -1,4 +1,46 @@
 package org.firstinspires.ftc.teamcode.tests;
 
-public class TrackingTest {
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+public class TrackingTest extends LinearOpMode {
+
+    Robot robot;
+
+    Drivetrain drivetrain;
+
+    MultipleTelemetry TELE;
+
+    GamepadEx g1;
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        g1 = new GamepadEx(gamepad1);
+
+        drivetrain = new Drivetrain(robot, TELE, g1);
+
+
+        waitForStart();
+        if(isStopRequested()) return;
+        while (opModeIsActive()){
+            drivetrain.update();
+        }
+
+    }
 }

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

@@ -40,6 +40,7 @@ public class Robot {
 
     public Servo rejecter;
 
+
     public Servo turr1;
 
     public Servo turr2;