test

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

@@ -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(); }
 
-
-
     }
 }

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

@@ -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();
 
-
-
         }
 
-
-
     }
 }