bmhhb

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index e98abcd9e7454e0140f7ce2c4bb9fc21a358d292..ccf27c0508c866f5e830abcb9d4ff61912a159c5 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -17,8 +17,6 @@ public class TurretAutoShoot extends Command {
     private Drivetrain drivetrain;
     private TurretVision turretVision;
 
-    public boolean SOTM;
-
     double fieldAngleRad;
     double turretSetpoint;
     double adjustedSetpoint;
@@ -26,20 +24,18 @@ public class TurretAutoShoot extends Command {
     double yawToTag;
 
     private boolean turretVisionEnabled = false;
+    private boolean SOTM = true;
 
-    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision, boolean SOTM){
+    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision){
         this.turret = turret;
         this.drivetrain = drivetrain;
         this.turretVision = turretVision;
         
-        this.SOTM = SOTM;
-
         addRequirements(turret);
     }
 
-    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, boolean SOTM){
-        this(turret, drivetrain, null, SOTM);
-        this.SOTM = SOTM;
+    public TurretAutoShoot(Turret turret, Drivetrain drivetrain){
+        this(turret, drivetrain, null);
     }
 
     public void updateTurretSetpoint() {
@@ -49,12 +45,12 @@ public class TurretAutoShoot extends Command {
         double D_x;
         if (SOTM) {
             ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-            fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
-            double xVel = chassisSpeed.vxMetersPerSecond;
-            double yVel = chassisSpeed.vyMetersPerSecond;
+            ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+            double xVel = fieldRelVel.vxMetersPerSecond;
+            double yVel = fieldRelVel.vyMetersPerSecond;
             
-            D_y = target.getY() - drivepose.getY() + (0.92) * yVel;
-            D_x = target.getX() - drivepose.getX() + (0.92) * xVel;
+            D_y = target.getY() - drivepose.getY() - (0.92) * yVel;
+            D_x = target.getX() - drivepose.getX() - (0.92) * xVel;
         } else {
             D_y = target.getY() - drivepose.getY();
             D_x = target.getX() - drivepose.getX();
@@ -106,7 +102,7 @@ public class TurretAutoShoot extends Command {
 
     @Override
     public void end(boolean interrupted) {
-        
+        turret.setSetpoint(0, 0);
     }
 
     

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index aa4a7cd7084cdae72c739f7999450c3942640f19..3d8eca0610bd3b035990fd0409627158083d6129 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -39,13 +39,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Command turretAutoShoot;
     private TurretJoyStickAim turretJoyStickAim;
 
-    private boolean SOTM;
-
     public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
         super(drive);
         this.shooter = shooter;
         this.turret = turret;
-        SOTM = false;
     }
 
     public void configureControls() { 
@@ -54,8 +51,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
         )));
 
-        driver.get(PS5Button.MUTE).onTrue(new InstantCommand(() -> toggleSOTM()));
-
         // Cancel commands
         driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
             getDrivetrain().setIsAlign(false);
@@ -88,7 +83,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                         if (turretAutoShoot != null && turretAutoShoot.isScheduled()){
                             turretAutoShoot.cancel();
                         } else{
-                            turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain(), SOTM);
+                            turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
                             CommandScheduler.getInstance().schedule(turretAutoShoot);
                         }
                     })
@@ -171,12 +166,4 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     public void endRumble(){
         driver.rumbleOff();
     }
-
-    public void toggleSOTM() {
-        if (SOTM) {
-            SOTM = false;
-        } else {
-            SOTM = true;
-        }
-    }
 }