stash

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

@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+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.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;
+    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);
+            } else if (mode == 1) {
+                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("Velocity", flywheel.getVelo());
+            TELE.addData("Power", robot.shooter1.getPower());
+            TELE.addData("Steady?", flywheel.getSteady());
+            TELE.addData("Position", robot.shooter1.getCurrentPosition());
+
+            TELE.update();
+
+        }
+
+    }
+}

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

@@ -7,6 +7,7 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
 
 public class Robot {
 
@@ -20,6 +21,8 @@ public class Robot {
 
     public DcMotorEx shooter2;
 
+    public Servo hood;
+
     public Robot (HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -30,6 +33,8 @@ public class Robot {
 
         shooter2 = hardwareMap.get(DcMotorEx.class, "s2");
 
+        hood = hardwareMap.get(Servo.class, "hood");
+
         if (USING_LL) {
             limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
             limelight3A.start(); // This tells Limelight to start looking!