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

Auto

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-29 14:11:37 -06:00
parent 4bbe5f218c
commit 88e1c428a5
1 changed files with 83 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,6 +26,8 @@ public class ShooterTest extends LinearOpMode {
public static String flyMode = "VEL"; public static String flyMode = "VEL";
public static boolean AutoTrack = false;
double initPos = 0.0; double initPos = 0.0;
double velo1 = 0.0; double velo1 = 0.0;
@@ -48,6 +50,7 @@ public class ShooterTest extends LinearOpMode {
public static double kP = 0.0005; // small proportional gain (tune this) public static double kP = 0.0005; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps public static double maxStep = 0.06; // prevents sudden jumps
public static double distance = 50;
MultipleTelemetry TELE; MultipleTelemetry TELE;
@@ -75,72 +78,103 @@ public class ShooterTest extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
shooter.setShooterMode(flyMode); if (AutoTrack){
hoodPos = hoodAnglePrediction(distance);
shooter.setManualPower(pow); vel = velPrediction(distance);
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos);
if (intake){
robot.transfer.setPower(0);
robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos);
robot.spin2.setPosition(1-spindexer_intakePos);
} else {
robot.transfer.setPower(1);
robot.intake.setPower(0);
if (spindexPos == 1){
robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1-spindexer_outtakeBall1);
} else if (spindexPos == 2){
robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1-spindexer_outtakeBall2);
} else if (spindexPos == 3){
robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1-spindexer_outtakeBall3);
}
} }
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition();
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
shooter.setShooterMode(flyMode);
shooter.setManualPower(pow);
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1 - turretPos);
if (intake) {
robot.transfer.setPower(0);
robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos);
robot.spin2.setPosition(1 - spindexer_intakePos);
} else {
robot.transfer.setPower(1);
robot.intake.setPower(0);
if (spindexPos == 1) {
robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
} else if (spindexPos == 2) {
robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
} else if (spindexPos == 3) {
robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
}
}
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition();
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION --- // --- PROPORTIONAL CORRECTION ---
double error = vel - velo1; double error = vel - velo1;
double correction = kP * error; double correction = kP * error;
// limit how fast power changes (prevents oscillation) // limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction)); correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER --- // --- FINAL MOTOR POWER ---
powPID = feed + correction; powPID = feed + correction;
// clamp to allowed range // clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID)); powPID = Math.max(0, Math.min(1, powPID));
shooter.setVelocity(powPID); shooter.setVelocity(powPID);
if (shoot) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
if (shoot){ shooter.update();
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
shooter.update(); TELE.addData("Revolutions", shooter.getECPRPosition());
TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower());
TELE.addData("Power Fly 2", robot.shooter2.getPower());
TELE.addData("powPID", shooter.getpowPID());
TELE.addData("Velocity", velo1);
TELE.update();
TELE.addData("Revolutions", shooter.getECPRPosition());
TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower());
TELE.addData("Power Fly 2", robot.shooter2.getPower());
TELE.addData("powPID", shooter.getpowPID());
TELE.addData("Velocity", velo1);
TELE.update();
} }
} }
public double hoodAnglePrediction(double distance) {
double L = 0.298317;
double A = 1.02124;
double k = 0.0157892;
double n = 3.39375;
double dist = Math.sqrt(distance*distance+24*24);
return L + A * Math.exp(-Math.pow(k * dist, n));
}
public static double velPrediction(double distance) {
double x = Math.sqrt(distance*distance+24*24);
double A = -211149.992;
double B = -1.19943;
double C = 3720.15909;
return A * Math.pow(x, B) + C;
}
} }