Testy stuffs fot the Shoot

src/main/java/frc4388/robot/RobotContainer.java

@@ -179,13 +179,13 @@ public class RobotContainer {
     // Swerve Drive with Input
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> {
-          if (this.currentControlMode.equals(ControlMode.SHOOTER)) {
+          if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
             m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), 
                                                getDriverController().getLeftY(),
                                                getDriverController().getRightX(),
                                                getDriverController().getRightY(),
                                                true); }
-          if (this.currentControlMode.equals(ControlMode.CLIMBER)) {
+          if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
             m_robotSwerveDrive.driveWithInput( 0, 
                                                0,
                                                0,
@@ -223,14 +223,14 @@ public class RobotContainer {
 
     m_robotTurret.setDefaultCommand(
        new RunCommand(() -> {
-        if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
+        if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
-        if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
+        if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
        }, m_robotTurret));
 
     m_robotHood.setDefaultCommand(
        new RunCommand(() -> {
-        if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
+        if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
-        if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
+        if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
        }, m_robotHood));
 
     m_robotClimber.setDefaultCommand(
@@ -279,9 +279,9 @@ public class RobotContainer {
         .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
         
     new JoystickButton(getDriverController(), XboxController.Button.kY.value)
-        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false))
+        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false));
-        .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
+        //!.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
-        .whenReleased(() -> m_robotSwerveDrive.stopModules());
+        //!.whenReleased(() -> m_robotSwerveDrive.stopModules());
 
     new JoystickButton(getDriverController(), XboxController.Button.kX.value)
         .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -148,7 +148,7 @@ public class Shoot extends CommandBase {
     SmartDashboard.putNumber("Normalized Output", this.normOutput);
 
     this.turret.m_boomBoomRotateMotor.set(normOutput);
-    this.swerve.driveWithInput(0, 0, normOutput, true);
+    this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative
 
     if (this.toShoot) {
       this.hood.runAngleAdjustPID(this.targetHood);
@@ -164,7 +164,11 @@ public class Shoot extends CommandBase {
     // swerve.driveWithInput(0, 0, initialSwerveRotation, true);
 
     // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
-    this.turret.m_boomBoomRotateMotor.set(0.0);
+    // this.turret.m_boomBoomRotateMotor.set(0.0);
+
+    // ? should stop the turret and the swerve
+    ////this.swerve.stopModules();
+    ////this.turret.runTurretWithInput(0);
 
     if (this.toShoot) {
       this.hood.runHood(0.0);

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -97,6 +97,7 @@ public class SwerveDrive extends SubsystemBase {
       ignoreAngles = true;
     else
       ignoreAngles = false;
+    
     Translation2d speed = new Translation2d(speedX, speedY);
     double mag = speed.getNorm();
     speed = speed.times(mag * speedAdjust);
@@ -111,6 +112,7 @@ public class SwerveDrive extends SubsystemBase {
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
     setModuleStates(states);
   }
+
   // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
   public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
     ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;