moved in shooter

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

@@ -1,7 +1,5 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
-import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDController;
@@ -22,6 +20,8 @@ import java.util.Objects;
 public class Shooter implements Subsystem {
     private final DcMotorEx fly1;
     private final DcMotorEx fly2;
+
+    private final DcMotorEx encoder;
     private final Servo hoodServo;
 
     private final Servo turret1;
@@ -50,9 +50,9 @@ public class Shooter implements Subsystem {
 
 
 
-    private String shooterMode = "MANUAL";
+    private String shooterMode = "AUTO";
 
-    private String turretMode = "MANUAL";
+    private String turretMode = "AUTO";
 
 
     public Shooter(Robot robot, MultipleTelemetry TELE) {
@@ -78,6 +78,8 @@ public class Shooter implements Subsystem {
 
         this.turret2 = robot.turr2;
 
+        this.encoder = robot.shooterEncoder;
+
 
 
 
@@ -170,7 +172,7 @@ public class Shooter implements Subsystem {
     public void setTurretMode(String mode){ turretMode = mode;}
 
 
-    public void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){
+    public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
 
         fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -180,51 +182,74 @@ public class Shooter implements Subsystem {
         Pose2d deltaPose = new Pose2d(
                 goalPose.position.x - robotPose.position.x,
                 goalPose.position.y - robotPose.position.y,
-                goalPose.heading.toDouble() - robotPose.heading.toDouble()
+                goalPose.heading.toDouble() - (robotPose.heading.toDouble())
         );
 
         double distance = Math.sqrt(
                 deltaPose.position.x * deltaPose.position.x
-                + deltaPose.position.y * deltaPose.position.y
-                + Poses.relativeGoalHeight * Poses.relativeGoalHeight
+                        + deltaPose.position.y * deltaPose.position.y
+                        + Poses.relativeGoalHeight * Poses.relativeGoalHeight
         );
 
+        telemetry.addData("dst", distance);
+
         double shooterPow = getPowerByDist(distance);
 
         double hoodAngle = getAngleByDist(distance);
 
-        if (shooterOn){
 
-            fly1.setVelocity(shooterPow);
-            fly2.setPower(fly1.getPower());
+//        hoodServo.setPosition(hoodAngle);
 
-        } else {
-            fly1.setPower(0);
-            fly2.setPower(0);
-        }
+        moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
 
-        hoodServo.setPosition(hoodAngle);
+        return distance;
+
+
+
+
+
+        //0.9974 * 355
 
-        moveTurret(getTurretPosByDeltaPose(deltaPose));
 
     }
 
-    public double getTurretPosByDeltaPose (Pose2d dPose){
+    public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
 
         double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
 
 
-        if (deltaAngle < -180) {
-            deltaAngle +=360;
+        double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
+
+        telemetry.addData("deltaAngle", deltaAngle);
+
+
+
+        if (deltaAngle > 90) {
+            deltaAngle -=360;
         }
 
-        deltaAngle /= (turret_GearRatio*turret_Range);
 
-        return (0.5+deltaAngle) ;
+
+//        deltaAngle += aTanAngle;
+
+        deltaAngle /= (335);
+
+        telemetry.addData("dAngle", deltaAngle);
+
+        telemetry.addData("AtanAngle", aTanAngle);
+
+
+        return ((0.30-deltaAngle) + offset);
+
 
 
     }
 
+    //62, 0.44
+
+    //56.5, 0.5
+
+
     public double getPowerByDist(double dist){
 
         //TODO: ADD LOGIC
@@ -233,11 +258,20 @@ public class Shooter implements Subsystem {
 
     public double getAngleByDist(double dist){
 
-        //TODO: ADD LOGIC
-        return  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){
@@ -262,9 +296,16 @@ public class Shooter implements Subsystem {
             fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
+            fly1.setVelocity(velocity);
+
+            fly2.setPower(fly1.getPower());
+
+
+
+
+
+
 
-            fly1.setVelocity(velocity, AngleUnit.DEGREES);
-            fly2.setVelocity(velocity, AngleUnit.DEGREES);
         }
 
         else if (Objects.equals(shooterMode, "POS")){
@@ -279,7 +320,7 @@ public class Shooter implements Subsystem {
         }
 
         if (Objects.equals(turretMode, "MANUAL")){
-            hoodServo.setPosition(hoodPos);
+//            hoodServo.setPosition(hoodPos);
 
             moveTurret(turretPos);