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

test

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-18 19:27:15 -06:00
parent f8222e292f
commit e5ba0947e3
2 changed files with 41 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,7 +27,6 @@ public class Shooter implements Subsystem {
private final Servo turret2;
private final MultipleTelemetry telemetry;
private boolean telemetryOn = false;
@@ -46,15 +45,10 @@ public class Shooter implements Subsystem {
private PIDController controller;
private double pow = 0.0;
private String shooterMode = "AUTO";
private String turretMode = "AUTO";
public Shooter(Robot robot, MultipleTelemetry TELE) {
this.fly1 = robot.shooter1;
this.fly2 = robot.shooter2;
@@ -68,10 +62,8 @@ public class Shooter implements Subsystem {
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
controller = new PIDController(p, i, d);
controller.setPID(p, i, d);
this.turret1 = robot.turr1;
@@ -80,11 +72,6 @@ public class Shooter implements Subsystem {
this.encoder = robot.shooterEncoder;
}
public void telemetryUpdate() {
@@ -125,11 +112,8 @@ public class Shooter implements Subsystem {
public void setVelocity(double vel) { velocity = vel; }
public void setPosPower(double power) { posPower = power; }
public void setTargetPosition(int pos) {
targetPosition = pos;
}
@@ -152,19 +136,12 @@ public class Shooter implements Subsystem {
}
public void setManualPower(double power) { manualPower = power; }
public String getShooterMode() { return shooterMode; }
public String getTurretMode() { return turretMode; }
public double getECPRPosition() {
return (fly1.getCurrentPosition() * 720 / ecpr);
}
@@ -177,14 +154,11 @@ public class Shooter implements Subsystem {
public void setTurretMode(String mode) { turretMode = mode; }
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) {
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y,
@@ -203,39 +177,28 @@ public class Shooter implements Subsystem {
double hoodAngle = getAngleByDist(distance);
// hoodServo.setPosition(hoodAngle);
moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
return distance;
//0.9974 * 355
}
public double getTurretPosByDeltaPose(Pose2d dPose, double offset) {
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x));
telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) {
deltaAngle -= 360;
}
// deltaAngle += aTanAngle;
deltaAngle /= (335);
@@ -244,18 +207,14 @@ public class Shooter implements Subsystem {
telemetry.addData("AtanAngle", aTanAngle);
return ((0.30 - deltaAngle) + offset);
}
//62, 0.44
//56.5, 0.5
public double getPowerByDist(double dist) {
//TODO: ADD LOGIC
@@ -264,20 +223,13 @@ public class Shooter implements Subsystem {
public double getAngleByDist(double dist) {
double newDist = dist - 56.5;
double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
return pos;
}
public void setTelemetryOn(boolean state) { telemetryOn = state; }
public void moveTurret(double pos) {
@@ -292,40 +244,27 @@ public class Shooter implements Subsystem {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly1.setPower(manualPower);
fly2.setPower(manualPower);
}
else if (Objects.equals(shooterMode, "VEL")){
} else if (Objects.equals(shooterMode, "VEL")) {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
double powPID = controller.calculate(getVelocity(mcpr) * ecpr, velocity);
fly1.setPower(powPID);
fly2.setPower(powPID);
}
if (Objects.equals(turretMode, "MANUAL")) {
// hoodServo.setPosition(hoodPos);
moveTurret(turretPos);
}
if (telemetryOn) { telemetryUpdate(); }
}
}

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

@@ -12,7 +12,6 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@TeleOp
@Config
public class ShooterTest extends LinearOpMode {
@@ -28,7 +27,6 @@ public class ShooterTest extends LinearOpMode {
public static double p = 0.001, i = 0, d = 0;
public static String flyMode = "MANUAL";
public static String turrMode = "MANUAL";
@@ -38,6 +36,7 @@ public class ShooterTest extends LinearOpMode {
public static double servoPosition = 0.501;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException {
@@ -53,15 +52,12 @@ public class ShooterTest extends LinearOpMode {
shooter.setShooterMode(flyMode);
shooter.setControllerCoefficients(p, i, d);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
shooter.setControllerCoefficients(p, i, d);
@@ -82,16 +78,11 @@ public class ShooterTest extends LinearOpMode {
if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
shooter.update();
TELE.update();
}
}
}