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

56
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,30 +78,37 @@ public class ShooterTest extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
if (AutoTrack){
hoodPos = hoodAnglePrediction(distance);
vel = velPrediction(distance);
}
shooter.setShooterMode(flyMode); shooter.setShooterMode(flyMode);
shooter.setManualPower(pow); shooter.setManualPower(pow);
robot.hood.setPosition(hoodPos); robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos); robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos); robot.turr2.setPosition(1 - turretPos);
if (intake){ if (intake) {
robot.transfer.setPower(0); robot.transfer.setPower(0);
robot.intake.setPower(0.75); robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos); robot.spin1.setPosition(spindexer_intakePos);
robot.spin2.setPosition(1-spindexer_intakePos); robot.spin2.setPosition(1 - spindexer_intakePos);
} else { } else {
robot.transfer.setPower(1); robot.transfer.setPower(1);
robot.intake.setPower(0); robot.intake.setPower(0);
if (spindexPos == 1){ if (spindexPos == 1) {
robot.spin1.setPosition(spindexer_outtakeBall1); robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1-spindexer_outtakeBall1); robot.spin2.setPosition(1 - spindexer_outtakeBall1);
} else if (spindexPos == 2){ } else if (spindexPos == 2) {
robot.spin1.setPosition(spindexer_outtakeBall2); robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1-spindexer_outtakeBall2); robot.spin2.setPosition(1 - spindexer_outtakeBall2);
} else if (spindexPos == 3){ } else if (spindexPos == 3) {
robot.spin1.setPosition(spindexer_outtakeBall3); robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1-spindexer_outtakeBall3); robot.spin2.setPosition(1 - spindexer_outtakeBall3);
} }
} }
@@ -122,8 +132,7 @@ public class ShooterTest extends LinearOpMode {
powPID = Math.max(0, Math.min(1, powPID)); powPID = Math.max(0, Math.min(1, powPID));
shooter.setVelocity(powPID); shooter.setVelocity(powPID);
if (shoot) {
if (shoot){
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
} else { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
@@ -140,7 +149,32 @@ public class ShooterTest extends LinearOpMode {
TELE.addData("Velocity", velo1); TELE.addData("Velocity", velo1);
TELE.update(); 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;
}
} }