shoot is janky

src/main/java/frc4388/robot/Constants.java

@@ -37,7 +37,7 @@ public final class Constants {
   public static final double LOOP_TIME = 0.02;
 
   public static final class SwerveDriveConstants {
-    public static final double ROTATION_SPEED = 3.0;
+    public static final double ROTATION_SPEED = 2.0;
     public static final double WIDTH = 23.75;
     public static final double HEIGHT = 23.75;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -278,7 +278,7 @@ public final class Constants {
     public static final double TURRET_CLIMBING_POS = -3.76;
 
     // Shoot Command Constants
-    public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
+    public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0);
 
     /* Turret Constants */
     // ID

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

@@ -265,7 +265,7 @@ public class RobotContainer {
         .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
-        .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true))
+        .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true))
         .whenReleased(() -> m_robotSwerveDrive.stopModules());
         
     new JoystickButton(getDriverController(), XboxController.Button.kB.value)
@@ -273,7 +273,7 @@ public class RobotContainer {
         .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
         
     new JoystickButton(getDriverController(), XboxController.Button.kY.value)
-        .whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood))
+        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false))
         .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
         .whenReleased(() -> m_robotSwerveDrive.stopModules());
 
@@ -306,9 +306,9 @@ public class RobotContainer {
     new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
         .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
 
-    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-        .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
+    //     .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
-        .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
+    //     .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
 
       // Right Bumper > Storage In
     // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)

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

@@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup {
   public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry));
+    addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry));
   }
 }

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

@@ -82,16 +82,19 @@ public class Shoot extends CommandBase {
     isAimedInTolerance = false;
   }
 
-  public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
+  // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
-    this(swerve, drum, turret, hood, false);
+  //   this(swerve, drum, turret, hood, false);
-  }
+  // }
 
   /**
    * Updates error for custom PID.
    */
   public void updateError() {
     targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
-    error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360;
+    error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
+    if (error > 180) {
+      error = 360 - error;
+      this.inverted = -1; } else { this.inverted = 1; }
     isAimedInTolerance = (Math.abs(error) <= tolerance);
   }
 
@@ -99,7 +102,7 @@ public class Shoot extends CommandBase {
   @Override
   public void initialize() {
 
-    this.turret.gotoMidpoint();
+    // this.turret.gotoMidpoint();
 
     this.odoX = 0;//-m_swerve.getOdometry().getY();
     this.odoY = -8;//-m_swerve.getOdometry().getX();
@@ -124,13 +127,6 @@ public class Shoot extends CommandBase {
    * Run custom PID.
    */
   public void runPID() {
-    if (error > 180) {
-      error = 360 - error;
-      this.inverted = -1;
-    }
-    else{
-      this.inverted = 1;
-    }
     prevError = error;
     updateError();
     
@@ -151,8 +147,8 @@ public class Shoot extends CommandBase {
     SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
     SmartDashboard.putNumber("Normalized Output", this.normOutput);
 
-    this.swerve.driveWithInput(0, 0, normOutput, true);
     this.turret.m_boomBoomRotateMotor.set(normOutput);
+    this.swerve.driveWithInput(0, 0, normOutput, true);
 
     if (this.toShoot) {
       this.hood.runAngleAdjustPID(this.targetHood);
@@ -167,7 +163,7 @@ public class Shoot extends CommandBase {
     // ? return to initial swerve rotation
     // swerve.driveWithInput(0, 0, initialSwerveRotation, true);
 
-    this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
+    // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
     this.turret.m_boomBoomRotateMotor.set(0.0);
 
     if (this.toShoot) {

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

@@ -105,13 +105,13 @@ public class SwerveDrive extends SubsystemBase {
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
+            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
         : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
             -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
     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;
     Translation2d speed = new Translation2d(leftX, leftY);
@@ -119,6 +119,9 @@ public class SwerveDrive extends SubsystemBase {
     if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
       rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
     double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
+    if (ignoreAngles) {
+      rot = 0;
+    }
     double xSpeedMetersPerSecond = speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
@@ -127,14 +130,14 @@ public class SwerveDrive extends SubsystemBase {
         : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
         chassisSpeeds);
-    if (ignoreAngles) {
+    // if (ignoreAngles) {
-      SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
+    //   SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
-      for (int i = 0; i < states.length; i ++) {
+    //   for (int i = 0; i < states.length; i ++) {
-        SwerveModuleState state = states[i];
+    //     SwerveModuleState state = states[i];
-        lockedStates[i]= new SwerveModuleState(0, state.angle);
+    //     lockedStates[i]= new SwerveModuleState(0, state.angle);
-      }
+    //   }
-      setModuleStates(lockedStates);
+    //   setModuleStates(lockedStates);
-    }
+    // }
     setModuleStates(states);
     // SmartDashboard.putNumber("rot", rot);
     // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());