added continous servos and smoother velocity PID

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java

@@ -5,7 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
 
 import androidx.annotation.NonNull;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
 
 import androidx.annotation.NonNull;
 
@@ -42,7 +42,6 @@ public class Red_V2 extends LinearOpMode {
     Flywheel flywheel;
 
     double velo = 0.0;
-    double targetVelocity = 0.0;
     public static double intake1Time = 2.9;
 
     public static double intake2Time = 2.9;
@@ -64,7 +63,6 @@ public class Red_V2 extends LinearOpMode {
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
 
     boolean spindexPosEqual(double spindexer) {
-        TELE.addData("Velocity", velo);
         TELE.addLine("spindex equal");
         TELE.update();
         return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
@@ -73,26 +71,23 @@ public class Red_V2 extends LinearOpMode {
 
     public Action initShooter(int vel) {
         return new Action() {
-            double initPos = 0.0;
             double stamp = 0.0;
             double stamp1 = 0.0;
             double ticker = 0.0;
             double stamp2 = 0.0;
-            double currentPos = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp2 = getRuntime();
                 }
 
-                targetVelocity = (double) vel;
                 ticker++;
                 if (ticker % 16 == 0) {
                     stamp = getRuntime();
                     stamp1 = stamp;
                 }
 
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
@@ -121,7 +116,7 @@ public class Red_V2 extends LinearOpMode {
             double stamp = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 steady = flywheel.getSteady();
                 robot.shooter1.setPower(powPID);
@@ -186,46 +181,12 @@ public class Red_V2 extends LinearOpMode {
         };
     }
 
-    public Action spindex (double spindexer, double vel){
+    public Action spindex (double spindexer, int vel){
         return new Action() {
-            double currentPos = 0.0;
-            double stamp = 0.0;
-            double initPos = 0.0;
-            double stamp1 = 0.0;
-            int ticker = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                ticker++;
-                if (ticker % 8 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
 
-                if (vel - velo > 500 && ticker > 16) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
-                    powPID = 1.0;
-                } else if (velo - vel > 500 && ticker > 16){
-                    powPID = 0.0;
-                } else if (Math.abs(vel - velo) < 100 && ticker > 16){
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - 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));
-                }
-
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
@@ -244,51 +205,18 @@ public class Red_V2 extends LinearOpMode {
         };
     }
 
-    public Action Shoot(double vel) {
+    public Action Shoot(int vel) {
         return new Action() {
             double transferStamp = 0.0;
             int ticker = 1;
             boolean transferIn = false;
-            double currentPos = 0.0;
-            double stamp = 0.0;
-            double initPos = 0.0;
-            double stamp1 = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 TELE.addData("Velocity", velo);
                 TELE.addLine("shooting");
                 TELE.update();
 
-                if (ticker % 8 == 0) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
-
-                if (vel - velo > 500 && ticker > 16) {
-                    powPID = 1.0;
-                } else if (velo - vel > 500 && ticker > 16){
-                    powPID = 0.0;
-                } else if (Math.abs(vel - velo) < 100 && ticker > 16){
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - 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));
-                }
-
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
@@ -297,7 +225,6 @@ public class Red_V2 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
-
                 if (ticker == 1) {
                     transferStamp = getRuntime();
                     ticker++;
@@ -560,7 +487,7 @@ public class Red_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -596,7 +523,7 @@ public class Red_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -626,7 +553,7 @@ public class Red_V2 extends LinearOpMode {
                     )
             );
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.teleop;
 import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;

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

@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+
+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.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@TeleOp
+@Config
+public class PIDServoTest extends LinearOpMode {
+
+    public static double p = 0.0003, i = 0, d = 0.00001;
+
+    public static double target = 0.5;
+
+    public static int mode = 0; //0 is for turret, 1 is for spindexer
+
+    public static double scalar = 1.112;
+    public static double restPos = 0.15;
+
+    Robot robot;
+
+    double pos = 0.0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        PIDController controller = new PIDController(p, i, d);
+
+        controller.setTolerance(0);
+        robot = new Robot(hardwareMap);
+
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+            controller.setPID(p, i, d);
+
+            if (mode == 0) {
+                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+
+                double pid = controller.calculate(pos, target);
+
+                robot.turr1.setPosition(pid);
+                robot.turr2.setPosition(-pid);
+            } else if (mode == 1) {
+                pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
+
+                double pid = controller.calculate(pos, target);
+
+                robot.spin1.setPosition(pid);
+                robot.spin2.setPosition(-pid);
+            }
+
+            telemetry.addData("pos", pos);
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
+            telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
+            telemetry.addData("target", target);
+            telemetry.addData("Mode", mode);
+            telemetry.update();
+
+        }
+
+    }
+}

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

@@ -1,7 +1,5 @@
 package org.firstinspires.ftc.teamcode.tests;
 
-import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -9,6 +7,7 @@ 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
@@ -18,7 +17,6 @@ public class ShooterTest extends LinearOpMode {
     public static int mode = 0;
     public static double parameter = 0.0;
     // --- CONSTANTS YOU TUNE ---
-    public static double MAX_RPM = 4500;      // your measured max RPM
 
     //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
     public static double transferPower = 0.0;
@@ -26,8 +24,8 @@ public class ShooterTest extends LinearOpMode {
 
     public static double turretPos = 0.501;
     Robot robot;
-    private double lastEncoderRevolutions = 0.0;
+    Flywheel flywheel;
-    private double lastTimeStamp = 0.0;
+
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -35,7 +33,6 @@ public class ShooterTest extends LinearOpMode {
         robot = new Robot(hardwareMap);
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
-        DcMotorEx encoder = robot.shooter1;
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -47,50 +44,19 @@ public class ShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
-            double kF = 1.0 / MAX_RPM;     // baseline feedforward
-
-            double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
-            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
-
-            TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
-            TELE.addLine("Parameter = pow, vel, or pos");
-            TELE.addData("leftShooterPower", leftShooter.getPower());
-            TELE.addData("rightShooterPower", rightShooter.getPower());
-            TELE.addData("shaftEncoderPos", encoderRevolutions);
-            TELE.addData("shaftEncoderVel", velocity);
-
-            double velPID;
-
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
 
-                // --- FEEDFORWARD BASE POWER ---
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
-                double feed = kF * parameter;        // Example: vel=2500 → feed=0.5
-
-                // --- PROPORTIONAL CORRECTION ---
-                double error = parameter - velocity;
-                double correction = kP * error;
-
-                // limit how fast power changes (prevents oscillation)
-                correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                // --- FINAL MOTOR POWER ---
-                velPID = feed + correction;
-
-                // clamp to allowed range
-                velPID = Math.max(0, Math.min(1, velPID));
-
-                rightShooter.setPower(velPID);
-                leftShooter.setPower(velPID);
 
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("PIDPower", powPID);
+                TELE.addData("Power", robot.shooter1.getPower());
+                TELE.addData("Steady?", flywheel.getSteady());
             }
 
-            lastTimeStamp = getRuntime();
-            lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
             if (hoodPos != 0.501) {
                 robot.hood.setPosition(hoodPos);
             }

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -16,8 +18,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
     public static double transferPos = 0.501;
     public static double hoodPos = 0.501;
 
-    public static double scalar = 1.112;
-    public static double restPos = 0.15;
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
@@ -43,10 +43,10 @@ public class PositionalServoProgrammer extends LinearOpMode {
             TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
             TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
             TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
-            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
+            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
-            TELE.addData("hoodA", robot.hoodPos.getVoltage());
+            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
-            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
+            TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
-            TELE.addData("turretA", robot.turr1Pos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
             TELE.update();
         }
     }

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

@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.controller.PIDController;
+
+public class Servos {
+    PIDController spinPID;
+
+    PIDController turretPID;
+
+    //PID constants
+    public static double spinP = 0.0, spinI = 0.0, spinD = 0.0;
+    public static double turrP = 0.0, turrI = 0.0, turrD = 0.0;
+
+    public static double spin_scalar = 0.15;
+    public static double spin_restPos = 1.112;
+    public static double turret_scalar = 0.15;
+    public static double turret_restPos = 1.112;
+
+    public void initServos() {
+        spinPID = new PIDController(spinP, spinI, spinD);
+        turretPID = new PIDController(turrP, turrI, turrD);
+    }
+
+    // In the code below, encoder = robot.servo.getVoltage()
+
+    public double getSpinPos(double encoder){
+        return spin_scalar * ((encoder - spin_restPos) / 3.3);
+    }
+    public double setSpinPos(double pos, double encoder){
+        spinPID.setPID(spinP, spinI, spinD);
+
+        return spinPID.calculate(this.getSpinPos(encoder), pos);
+    }
+    public boolean spinEqual(double pos, double encoder){
+        return Math.abs(pos - this.getSpinPos(encoder)) < 0.01;
+    }
+
+    public double getTurrPos(double encoder){
+        return turret_scalar * ((encoder - turret_restPos) / 3.3);
+    }
+    public double setTurrPos(double pos, double encoder){
+        turretPID.setPID(turrP, turrI, turrD);
+
+        return spinPID.calculate(this.getTurrPos(encoder), pos);
+    }
+    public boolean turretEqual(double pos, double encoder){
+        return Math.abs(pos - this.getTurrPos(encoder)) < 0.01;
+    }
+}