ts is not gonna work bruh

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index ee619d7340b456552de17c8977d862357e6398fd..1232c275b52df45250912752073a35c076ad2b22 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.gpm;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -20,7 +21,7 @@ public class TurretAutoShoot extends Command {
     double yawToTagCamera;
     double yawToTag;
 
-    private boolean turretCamEnabled = true;
+    private boolean turretVisionEnabled = true;
 
     public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision){
         this.turret = turret;
@@ -28,26 +29,28 @@ public class TurretAutoShoot extends Command {
         this.turretVision = turretVision;
     }
 
-    public void align() {
+    public void updateTurretSetpoint() {
         Translation2d drivepose = drivetrain.getPose().getTranslation();
         Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
         double D_y = target.getY() - drivepose.getY();
         double D_x = target.getX() - drivepose.getX();
-        double angleRad = Math.atan2(D_y, D_x);
-        System.out.println("Aligning the turn to degree angle: " + Units.radiansToDegrees(angleRad));
-        turretSetpoint = Units.radiansToDegrees(angleRad);
+        double fieldAngle = Math.atan2(D_y, D_x);
+        double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians()));
+        turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngle - robotHeading), -180.0,180.0);
+
+        System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
     }
 
     public void adjustWithTurretCam(){
         if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){
             if(Robot.getAlliance() == Alliance.Blue){
-                yawToTagCamera = -1 * turretVision.getYawToTagRad(26).get();
+                yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get());
             }
             else{
-                yawToTagCamera = -1 * turretVision.getYawToTagRad(10).get();
+                yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get());
             }
             double error = yawToTagCamera - yawToTag;
-            adjustedSetpoint += error;
+            adjustedSetpoint = turretSetpoint + error;
         }
     }
 
@@ -60,20 +63,23 @@ public class TurretAutoShoot extends Command {
 
     @Override
     public void initialize() {
-        // TODO Auto-generated method stub
-        super.initialize();
+        updateTurretSetpoint();
+        turret.setSetpoint(turretSetpoint, drivetrain.getAngularRate(2));
     }
 
     @Override
     public void execute() {
-        // TODO Auto-generated method stub
-        super.execute();
+        updateTurretSetpoint();
+        updateYawToTag();
+        if(turretVisionEnabled && turret.atSetPoint()){
+            adjustWithTurretCam();
+        }
+        turret.setSetpoint(turretSetpoint, drivetrain.getAngularRate(2));
     }
 
     @Override
     public void end(boolean interrupted) {
-        // TODO Auto-generated method stub
-        super.end(interrupted);
+        turret.setSetpoint(0, 0);
     }
 
     

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index b089e16c077e83f9f3038ce225776c5c967a622c..5a8ff99b902070325f5c4f6e877049b4ddd66c13 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -113,10 +113,10 @@ public class Turret extends SubsystemBase {
         SmartDashboard.putData("turret", mechanism2d);
         SmartDashboard.putData("PID", pid);
 
-        SmartDashboard.putData("Set to 0 degrees", new InstantCommand(() -> setSetpoint(0)));
-        SmartDashboard.putData("Set to 90 degrees", new InstantCommand(( )-> setSetpoint(90)));
-        SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180)));
-        SmartDashboard.putData("Set to 270 degrees", new InstantCommand(() -> setSetpoint(270)));
+        SmartDashboard.putData("Set to 0 degrees", new InstantCommand(() -> setSetpoint(0, 0)));
+        SmartDashboard.putData("Set to 90 degrees", new InstantCommand(( )-> setSetpoint(90, 0)));
+        SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180, 0)));
+        SmartDashboard.putData("Set to 270 degrees", new InstantCommand(() -> setSetpoint(270, 0)));
 
         // SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1)));
         // SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1)));
@@ -157,12 +157,19 @@ public class Turret extends SubsystemBase {
          return motor.getPosition().getValueAsDouble() / gearRatio; // Gear ratio
     }
 
-    public void setSetpoint(double setpointDegrees) {
+    public boolean atSetPoint(){
+        return Math.abs(getPosition() - setpoint) < 3.0;
+    }
+
+    public void setSetpoint(double setpointDegrees, double robotRotVel) {
+
+        setpoint = MathUtil.clamp(setpointDegrees, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);
+
         if (infiniteRotation) {
             // 1. Get current position in degrees
             double currentDegrees = (motor.getPosition().getValueAsDouble() / gearRatio) * 360.0;
             // 2. Calculate the error
-            double error = setpointDegrees - currentDegrees;
+            double error = setpoint - currentDegrees;
             // 3. Wrap the error to [-180, 180]
             // This finds the "remainder" of the distance relative to a full circle
             double optimizedError = Math.IEEEremainder(error, 360.0);
@@ -172,8 +179,11 @@ public class Turret extends SubsystemBase {
             motor.setControl(voltageRequest.withPosition(motorTargetRotations));
         } else {
             // normal limited 0,360
-            double motorTargetRotations = (setpointDegrees / 360.0) * gearRatio;
-            motor.setControl(voltageRequest.withPosition(motorTargetRotations));
+            double motorTargetRotations = (setpoint / 360.0) * gearRatio;
+
+            //Tune this with rotating robot
+            double dV = 0;
+            motor.setControl(voltageRequest.withPosition(motorTargetRotations).withFeedForward(dV * robotRotVel));
         }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
new file mode 100644 (file)
index 0000000..c505ce2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -0,0 +1,6 @@
+package frc.robot.subsystems.turret;
+
+public class TurretConstants {
+    public static double MAX_ANGLE = 150;
+    public static double MIN_ANGLE = -150;
+}