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

135
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 Servo turret2;
private final MultipleTelemetry telemetry; private final MultipleTelemetry telemetry;
private boolean telemetryOn = false; private boolean telemetryOn = false;
@@ -46,15 +45,10 @@ public class Shooter implements Subsystem {
private PIDController controller; private PIDController controller;
private double pow = 0.0; private double pow = 0.0;
private String shooterMode = "AUTO"; private String shooterMode = "AUTO";
private String turretMode = "AUTO"; private String turretMode = "AUTO";
public Shooter(Robot robot, MultipleTelemetry TELE) { public Shooter(Robot robot, MultipleTelemetry TELE) {
this.fly1 = robot.shooter1; this.fly1 = robot.shooter1;
this.fly2 = robot.shooter2; 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);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
controller = new PIDController(p, i, d); controller = new PIDController(p, i, d);
controller.setPID(p, i, d); controller.setPID(p, i, d);
this.turret1 = robot.turr1; this.turret1 = robot.turr1;
@@ -80,11 +72,6 @@ public class Shooter implements Subsystem {
this.encoder = robot.shooterEncoder; this.encoder = robot.shooterEncoder;
} }
public void telemetryUpdate() { public void telemetryUpdate() {
@@ -111,34 +98,31 @@ public class Shooter implements Subsystem {
return (hoodServo.getPosition()); return (hoodServo.getPosition());
} }
public void sethoodPosition(double pos) {hoodPos = pos;} public void sethoodPosition(double pos) { hoodPos = pos; }
public double getTurretPosition() { public double getTurretPosition() {
return ((turret1.getPosition()+ (1-turret2.getPosition()))/2); return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2);
} }
public void setTurretPosition(double pos) {turretPos = pos;} public void setTurretPosition(double pos) { turretPos = pos; }
public double getVelocity(double cpr) { public double getVelocity(double cpr) {
return 60*(fly1.getVelocity() / (2*cpr)); return 60 * (fly1.getVelocity() / (2 * cpr));
} }
public void setVelocity(double vel){velocity = vel;} public void setVelocity(double vel) { velocity = vel; }
public void setPosPower(double power) { posPower = power; }
public void setTargetPosition(int pos) {
public void setPosPower(double power){posPower = power;}
public void setTargetPosition(int pos){
targetPosition = pos; targetPosition = pos;
} }
public void setTolerance(int tolerance){ public void setTolerance(int tolerance) {
controller.setTolerance(tolerance); controller.setTolerance(tolerance);
} }
public void setControllerCoefficients(double kp, double ki, double kd){ public void setControllerCoefficients(double kp, double ki, double kd) {
p = kp; p = kp;
i = ki; i = ki;
d = kd; d = kd;
@@ -146,45 +130,35 @@ public class Shooter implements Subsystem {
} }
public PIDCoefficients getControllerCoefficients(){ public PIDCoefficients getControllerCoefficients() {
return new PIDCoefficients(p, i, d); return new PIDCoefficients(p, i, d);
} }
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);
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);
} }
public double getMCPRPosition(){ public double getMCPRPosition() {
return (fly1.getCurrentPosition() * 720) / mcpr; return (fly1.getCurrentPosition() * 720) / mcpr;
} }
public void setShooterMode(String mode){ shooterMode = mode;} public void setShooterMode(String mode) { shooterMode = mode; }
public void setTurretMode(String mode){ turretMode = mode;} public void setTurretMode(String mode) { turretMode = mode; }
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) {
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
Pose2d deltaPose = new Pose2d( Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x, goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y, goalPose.position.y - robotPose.position.y,
@@ -193,8 +167,8 @@ public class Shooter implements Subsystem {
double distance = Math.sqrt( double distance = Math.sqrt(
deltaPose.position.x * deltaPose.position.x deltaPose.position.x * deltaPose.position.x
+ deltaPose.position.y * deltaPose.position.y + deltaPose.position.y * deltaPose.position.y
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight + Poses.relativeGoalHeight * Poses.relativeGoalHeight
); );
telemetry.addData("dst", distance); telemetry.addData("dst", distance);
@@ -203,39 +177,28 @@ public class Shooter implements Subsystem {
double hoodAngle = getAngleByDist(distance); double hoodAngle = getAngleByDist(distance);
// hoodServo.setPosition(hoodAngle); // hoodServo.setPosition(hoodAngle);
moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
return distance; return distance;
//0.9974 * 355 //0.9974 * 355
} }
public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ public double getTurretPosByDeltaPose(Pose2d dPose, double offset) {
double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x));
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
telemetry.addData("deltaAngle", deltaAngle); telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) { if (deltaAngle > 90) {
deltaAngle -=360; deltaAngle -= 360;
} }
// deltaAngle += aTanAngle; // deltaAngle += aTanAngle;
deltaAngle /= (335); deltaAngle /= (335);
@@ -244,10 +207,7 @@ public class Shooter implements Subsystem {
telemetry.addData("AtanAngle", aTanAngle); telemetry.addData("AtanAngle", aTanAngle);
return ((0.30 - deltaAngle) + offset);
return ((0.30-deltaAngle) + offset);
} }
@@ -255,77 +215,56 @@ public class Shooter implements Subsystem {
//56.5, 0.5 //56.5, 0.5
public double getPowerByDist(double dist) {
public double getPowerByDist(double dist){
//TODO: ADD LOGIC //TODO: ADD LOGIC
return dist; return dist;
} }
public double getAngleByDist(double dist){ public double getAngleByDist(double dist) {
double newDist = dist - 56.5; double newDist = dist - 56.5;
double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46; double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
return pos;
return pos;
} }
public void setTelemetryOn(boolean state) { telemetryOn = state; }
public void moveTurret(double pos) {
public void setTelemetryOn(boolean state){telemetryOn = state;}
public void moveTurret(double pos){
turret1.setPosition(pos); turret1.setPosition(pos);
turret2.setPosition(1-pos); turret2.setPosition(1 - pos);
} }
@Override @Override
public void update() { public void update() {
if (Objects.equals(shooterMode, "MANUAL")){ if (Objects.equals(shooterMode, "MANUAL")) {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly1.setPower(manualPower); fly1.setPower(manualPower);
fly2.setPower(manualPower); fly2.setPower(manualPower);
} } else if (Objects.equals(shooterMode, "VEL")) {
else if (Objects.equals(shooterMode, "VEL")){
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
double powPID = controller.calculate(getVelocity(mcpr) * ecpr, velocity); double powPID = controller.calculate(getVelocity(mcpr) * ecpr, velocity);
fly1.setPower(powPID); fly1.setPower(powPID);
fly2.setPower(powPID); fly2.setPower(powPID);
} }
if (Objects.equals(turretMode, "MANUAL")) {
if (Objects.equals(turretMode, "MANUAL")){
// hoodServo.setPosition(hoodPos); // hoodServo.setPosition(hoodPos);
moveTurret(turretPos); moveTurret(turretPos);
} }
if (telemetryOn) { telemetryUpdate(); }
if (telemetryOn) {telemetryUpdate();}
} }
} }

17
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.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@TeleOp @TeleOp
@Config @Config
public class ShooterTest extends LinearOpMode { 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 double p = 0.001, i = 0, d = 0;
public static String flyMode = "MANUAL"; public static String flyMode = "MANUAL";
public static String turrMode = "MANUAL"; public static String turrMode = "MANUAL";
@@ -38,6 +36,7 @@ public class ShooterTest extends LinearOpMode {
public static double servoPosition = 0.501; public static double servoPosition = 0.501;
MultipleTelemetry TELE; MultipleTelemetry TELE;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -53,16 +52,13 @@ public class ShooterTest extends LinearOpMode {
shooter.setShooterMode(flyMode); shooter.setShooterMode(flyMode);
shooter.setControllerCoefficients(p, i, d); shooter.setControllerCoefficients(p, i, d);
waitForStart(); waitForStart();
if(isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) {
while(opModeIsActive()){
shooter.setControllerCoefficients(p, i, d); shooter.setControllerCoefficients(p, i, d);
@@ -80,18 +76,13 @@ public class ShooterTest extends LinearOpMode {
shooter.setPosPower(posPower); shooter.setPosPower(posPower);
if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);} if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
shooter.update(); shooter.update();
TELE.update(); TELE.update();
} }
} }
} }