From 0bf392f81f83a844bff5aea629409f9daac2b979 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 2 Jan 2026 00:17:28 -0600 Subject: [PATCH] added continous servos and smoother velocity PID --- .../ftc/teamcode/autonomous/Blue_V2.java | 2 +- .../ftc/teamcode/autonomous/Red_V2.java | 93 ++----------------- .../ftc/teamcode/teleop/TeleopV2.java | 2 +- .../ftc/teamcode/tests/PIDServoTest.java | 76 +++++++++++++++ .../ftc/teamcode/tests/ShooterTest.java | 50 ++-------- .../utils/PositionalServoProgrammer.java | 12 +-- .../ftc/teamcode/utils/Servos.java | 50 ++++++++++ 7 files changed, 152 insertions(+), 133 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java index 52e3c23..7cb04a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index 1edabf1..71a742a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/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 = 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()); + powPID = flywheel.manageFlywheel(vel, 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) { - 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()); + powPID = flywheel.manageFlywheel(vel, 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 2d22a20..83f9805 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java new file mode 100644 index 0000000..028290f --- /dev/null +++ b/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(); + + } + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 2db3dd3..0c9607d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/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; - private double lastTimeStamp = 0.0; + Flywheel flywheel; + @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 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); + double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition()); + 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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 64c001e..125b5e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/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("hoodA", robot.hoodPos.getVoltage()); - TELE.addData("transferServoA", robot.transferServoPos.getVoltage()); - TELE.addData("turretA", robot.turr1Pos.getVoltage()); + TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); + TELE.addData("hood voltage", robot.hoodPos.getVoltage()); + TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); + TELE.addData("turret voltage", robot.turr1Pos.getVoltage()); TELE.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java new file mode 100644 index 0000000..98319c8 --- /dev/null +++ b/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; + } +}