KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

changes to PID

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-28 18:58:15 -06:00
parent 46f1bd5191
commit b0bc7b7b5b
2 changed files with 8 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -45,7 +45,7 @@ public class Shooter implements Subsystem {
public double powPID = 1.0; public double powPID = 1.0;
private double p = 0.0003, i = 0, d = 0.00001; private double p = 0.0003, i = 0, d = 0.00001, f=0;
private PIDFController controller; private PIDFController controller;
private double pow = 0.0; private double pow = 0.0;
@@ -133,7 +133,7 @@ public class Shooter implements Subsystem {
} }
public double getMCPRPosition() { public double getMCPRPosition() {
return fly1.getCurrentPosition() / (2 * mcpr); return (double) fly1.getCurrentPosition() / 4;
} }
public void setShooterMode(String mode) { shooterMode = mode; } public void setShooterMode(String mode) { shooterMode = mode; }

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

@@ -20,18 +20,11 @@ public class ShooterTest extends LinearOpMode {
public static double pow = 0.0; public static double pow = 0.0;
public static double vel = 0.0; public static double vel = 0.0;
public static double mcpr = 28.0; // CPR of the motor
public static double ecpr = 1024.0; // CPR of the encoder public static double ecpr = 1024.0; // CPR of the encoder
public static double hoodPos = 0.5; public static double hoodPos = 0.5;
public static double posPower = 0.0;
public static double turretPos = 0.9; public static double turretPos = 0.9;
public static double p = 0.0003, i = 0, d = 0, f = 0; public static String flyMode = "VEL";
public static String flyMode = "MANUAL";
public static String turrMode = "MANUAL";
double initPos = 0.0; double initPos = 0.0;
@@ -45,22 +38,14 @@ public class ShooterTest extends LinearOpMode {
public static int maxVel = 4000; public static int maxVel = 4000;
public static int tolerance = 300;
public static boolean shoot = false; public static boolean shoot = false;
public static int spindexPos = 1; public static int spindexPos = 1;
public static int initTolerance = 800;
public static boolean intake = true; public static boolean intake = true;
double initVel = 0; public static double kP = 0.0005; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
double stamp = 0.0;
public static double kP = 0.01; // small proportional gain (tune this)
public static double maxStep = 0.2; // prevents sudden jumps
MultipleTelemetry TELE; MultipleTelemetry TELE;
@@ -78,24 +63,16 @@ public class ShooterTest extends LinearOpMode {
shooter.setTelemetryOn(true); shooter.setTelemetryOn(true);
shooter.setTurretMode(turrMode);
shooter.setShooterMode(flyMode); shooter.setShooterMode(flyMode);
shooter.setControllerCoefficients(p, i, d, f);
initPos = shooter.getECPRPosition(); initPos = shooter.getECPRPosition();
initVel = vel;
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) { while (opModeIsActive()) {
shooter.setControllerCoefficients(p, i, d, f);
shooter.setShooterMode(flyMode); shooter.setShooterMode(flyMode);
shooter.setManualPower(pow); shooter.setManualPower(pow);
@@ -123,25 +100,11 @@ public class ShooterTest extends LinearOpMode {
} }
} }
if (vel != initVel){
stamp = getRuntime();
initVel = vel;
}
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime(); stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition(); initPos1 = shooter.getECPRPosition();
if (Math.abs(vel - velo1) > initTolerance && getRuntime() - stamp < 2) {
powPID = vel / maxVel; double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
} else if (vel - tolerance > velo1) {
powPID = powPID + 0.001;
} else if (vel + tolerance < velo1) {
powPID = powPID - 0.001;
}
if (powPID > 1.0){
powPID = 1.0;
}
double feed = kF * vel; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION --- // --- PROPORTIONAL CORRECTION ---
double error = vel - velo1; double error = vel - velo1;
@@ -165,8 +128,7 @@ public class ShooterTest extends LinearOpMode {
shooter.update(); shooter.update();
TELE.addData("ECPR Revolutions", shooter.getECPRPosition()); TELE.addData("Revolutions", shooter.getECPRPosition());
TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
TELE.addData("hoodPos", shooter.gethoodPosition()); TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition()); TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower()); TELE.addData("Power Fly 1", robot.shooter1.getPower());