fda

diff --git a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
index a383128ea655ceccae7451f1aa105a80ecd756ed..8be9b9931b559c3b5451619418599fa95d0ed12a 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
@@ -1,10 +1,10 @@
 package frc.robot.commands.gpm;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.controls.BaseDriverConfig;
-import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.turret.Turret;
 
 public class TurretJoyStickAim extends Command{
@@ -16,7 +16,7 @@ public class TurretJoyStickAim extends Command{
     }
 
     Rotation2d rotation2d = new Rotation2d(driver.getRawSideTranslation(), driver.getRawForwardTranslation());
-    double angle = Units.radiansToDegrees(rotation2d.getDegrees());
+    double angle = Units.radiansToDegrees(MathUtil.angleModulus(rotation2d.getDegrees()));
 
     @Override
     public void execute() {

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e7af91cff7a6cd97efcf412f023f8c1ae8ae5718..8c15939c0e78405ca86c689cb0d1c7ad33f4214e 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.TurretAutoShoot;
+import frc.robot.commands.gpm.TurretJoyStickAim;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
@@ -37,6 +38,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
     private Pose2d alignmentPose = null;
     private Command turretAutoShoot;
+    private TurretJoyStickAim turretJoyStickAim;
 
     public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
         super(drive);
@@ -87,6 +89,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                         }
                     })
         );
+
+        driver.get(PS5Button.CROSS).onTrue(
+            new InstantCommand(()->{
+                        turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
+                        CommandScheduler.getInstance().schedule(turretAutoShoot);
+                    })
+        ).onFalse(
+            new InstantCommand(()->{
+                turretAutoShoot.cancel();
+            })
+        );
     }
     
     public void setAlignmentPose(){