IDK

src/main/deploy/pathplanner/autos/HUB_START.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HUB_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LEFT_TRENCH_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RIGHT_TRENCH_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path

@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path

@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path

@@ -83,14 +83,14 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -1.2282908533168926
+    "rotation": -1.2282908533168928
   },
   "reversed": false,
   "folder": null,

src/main/deploy/pathplanner/paths/HUB_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.519556041182133,
+        "y": 4.059804750409042
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.769556041182133,
+        "y": 4.059804750409042
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.519556041182133,
+        "y": 4.059804750409042
+      },
+      "prevControl": {
+        "x": 3.269556041182133,
+        "y": 4.059804750409042
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.423545951843262
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.25,
+        "y": 7.423545951843262
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.423545951843262
+      },
+      "prevControl": {
+        "x": 3.75,
+        "y": 7.423545951843262
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/M. Depot. .path

@@ -88,7 +88,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.6236959156237152
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.25,
+        "y": 0.6236959156237152
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.6236959156237152
+      },
+      "prevControl": {
+        "x": 3.75,
+        "y": 0.6236959156237152
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Relay. L-Tr..path

@@ -174,7 +174,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/Relay. R-Tr..path

@@ -85,7 +85,7 @@
       },
       "prevControl": {
         "x": 6.020824888067715,
-        "y": 0.8531028227756466
+        "y": 0.8531028227756465
       },
       "nextControl": {
         "x": 5.667271497474452,
@@ -178,7 +178,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -68,7 +68,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path

@@ -24,7 +24,7 @@
         "y": 4.024072289156626
       },
       "nextControl": {
-        "x": 2.421654288011767,
+        "x": 2.421654288011768,
         "y": 3.9837489460910525
       },
       "isLocked": false,
@@ -63,7 +63,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path

@@ -68,7 +68,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/paths/Spin.path

@@ -72,7 +72,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false

src/main/deploy/pathplanner/settings.json

@@ -6,7 +6,7 @@
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngVel": 360.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 44.4521,

src/main/java/frc/robot/RobotContainer.java

@@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutoShootCommandChassisTurret;
 import frc.robot.commands.AutoTurretCommand;
+import frc.robot.commands.AutoTurretShootCommand;
 import frc.robot.commands.Reverse;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
@@ -80,7 +81,8 @@ public class RobotContainer {
         m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake()));
         //m_driverController.a().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
-        m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        // m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        m_driverController.x().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
         m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
         m_driverController.y().onTrue(intakeSubsystem.deploy());
     }

src/main/java/frc/robot/Utils.java

@@ -5,6 +5,7 @@ import frc.robot.Constants;
 import frc.robot.Constants.ShootingRegion;
 import frc.robot.Constants.ShootingRegionDimensions;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.units.measure.Angle;
 
 public class Utils {
     
@@ -93,7 +94,16 @@ public class Utils {
         return region;
     }
 
+    public static double getHoodAngleForDistance(double distanceInMeters){
+        if(distanceInMeters >= 0){
+            return 0;
+        }
+        return 0;
+    }
 
+    public static double getLauncherRPMForDistance(double distanceInMeters){ 
+        return (667.557*distanceInMeters) + 1024.699;
+    }
 
     /**
      * Determines the appropriate shooting target for the robot based on its alliance

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -29,9 +29,6 @@ public class AutoTurretCommand extends Command {
 
     private int[] shootingTagFilters;
 
-    private Pose3d closestTag;
-    private Pose3d scoringHubPose;
-
     public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
         this.turretSubsystem = turretSubsystem;
         this.driveSubsystem = driveSubsystem;
@@ -47,12 +44,12 @@ public class AutoTurretCommand extends Command {
         DriverStation.getAlliance().ifPresent((aliance) -> {
             if (aliance == Alliance.Blue) {
                 shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
-                scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
             } else {
                 shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
-                scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
             }
-            Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
+            Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
         });
 
     }
@@ -65,7 +62,7 @@ public class AutoTurretCommand extends Command {
                 new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
         Logger.recordOutput("AutoTurret-TurretPose", turretPose);
 
-        Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
+        Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
         Logger.recordOutput("AutoTurret-Translation", targetTranslation);
 
         if(turretSubsystem.isTurretReady()){

src/main/java/frc/robot/commands/AutoTurretShootCommand.java

@@ -1,43 +1,63 @@
 package frc.robot.commands;
 
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
-public class RelayCommand extends Command{
+public class AutoTurretShootCommand extends Command{
 
     private HopperSubsystem hopperSubsystem;
     private TurretSubsystem turretSubsystem;
     private IntakeSubsystem intakeSubsystem;
+    private DriveSubsystem driveSubsystem;
 
     private Command waitCommand = Commands.waitSeconds(1.5);
 
-    public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
+    private Alliance alliance;
+
+    public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
         this.hopperSubsystem = hopperSubsystem;
         this.turretSubsystem = turretSubsystem;
         this.intakeSubsystem = intakeSubsystem;
+        this.driveSubsystem = driveSubsystem;
 
         addRequirements(hopperSubsystem, intakeSubsystem);
     }
     
     @Override
     public void initialize() {
-        // turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
-        turretSubsystem.setLauncher(3000);
         hopperSubsystem.spindexerOn();
         CommandScheduler.getInstance().schedule(waitCommand);
-        turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
+        alliance = DriverStation.getAlliance().get();
     }
 
     @Override
     public void execute() {
+
+        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), alliance);
+
+        // if(shootingRegion == )
+
+        double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
+
+        turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(0));
+        turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(0));
+
         if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
             hopperSubsystem.feedKicker();
         }else{
@@ -58,7 +78,7 @@ public class RelayCommand extends Command{
 
         turretSubsystem.setLauncher(0);
         turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
-        turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+        // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
     }
 
 }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -163,15 +163,15 @@ public class DriveSubsystem extends TestableSubsystem {
                     driveSystem::getRobotPose2d,
                     driveSystem::setPose2d,
                     driveSystem::getRobotRelativeSpeeds,
-                    (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
+                    (speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());},
                     new PPHolonomicDriveController(
-                            new PIDConstants(3, 0, 0),
+                            new PIDConstants(5, 0, 0),
                             new PIDConstants(Math.PI, 0, 0)),
                     config,
                     () -> {
                         var alliance = DriverStation.getAlliance();
                         if (alliance.isPresent()) {
-                            return false;//alliance.get() == DriverStation.Alliance.Red;
+                            return alliance.get() == DriverStation.Alliance.Red;
                         }
                         return false;
                     },
@@ -188,6 +188,8 @@ public class DriveSubsystem extends TestableSubsystem {
             e.printStackTrace();
         }
 
+        
+
     }
 
     @Override

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -62,6 +62,7 @@ public class TurretSubsystem extends TestableSubsystem {
   private StatusSignal<Angle> turretMotorPosition;
   private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
   private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
+  private StatusSignal<Double> turretPositionError;
   private double forwardLimit = 0;
   private double reverseLimit = 0;
 
@@ -72,6 +73,8 @@ public class TurretSubsystem extends TestableSubsystem {
 
   private Servo hood;
 
+  private Pose3d turretTarget;
+
   public TurretSubsystem() {
 
     // Turret configuration
@@ -103,6 +106,7 @@ public class TurretSubsystem extends TestableSubsystem {
     turretMotorPosition = turretMotor.getPosition();
     turretMotorForwardLimit = turretMotor.getForwardLimit();
     turretMotorReverseLimit = turretMotor.getReverseLimit();
+    turretPositionError = turretMotor.getClosedLoopError();
 
     turretSim = turretMotor.getSimState();
 
@@ -185,6 +189,14 @@ public class TurretSubsystem extends TestableSubsystem {
     return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
   }
 
+  public void setTurretTargetPose(Pose3d targetPose){
+    turretTarget = targetPose;
+  }
+
+  public Pose3d getTurretTargetPose(){
+    return turretTarget;
+  }
+
   private double getTurretMotorPosition(){
     return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
   }
@@ -215,7 +227,7 @@ public class TurretSubsystem extends TestableSubsystem {
             .withReverseSoftLimitEnable(true)
             .withReverseSoftLimitThreshold(reverseLimit)
         );
-        setTurretRobotRelative(0);
+        // setTurretRobotRelative(0);
         // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
       }
     }
@@ -292,6 +304,10 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation
     turretSim.setRawRotorPosition(posWithOffset);
   }
 
+  public boolean isTurretAtTarget(){
+    return turretPositionError.getValueAsDouble() <= 0.5;
+  }
+
   private double getRequestedPosTurretRelative() {
     return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
   }