Made some autos (need to correct PID) (Im tweakin)

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

@@ -40,9 +40,9 @@ public final class Constants {
         public static final double TURBO_SPEED = 4.0;
     
         public static final class DefaultSwerveRotOffsets {
-          public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625;
+          public static final double FRONT_LEFT_ROT_OFFSET = 222.7145469;
-          public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 255.0464068;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
-          public static final double BACK_LEFT_ROT_OFFSET = -277.646484375;
+          public static final double BACK_LEFT_ROT_OFFSET = -277.4284884;
           public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
         }
 
@@ -67,7 +67,7 @@ public final class Constants {
         public static final class PIDConstants {
             public static final int SWERVE_SLOT_IDX = 0;
             public static final int SWERVE_PID_LOOP_IDX = 1;
-            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
+            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.2, 0.0, 0, 0.0);
         }
     
         public static final class AutoConstants {

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

@@ -87,9 +87,9 @@ public class RobotContainer {
 
     private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
         new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        new InstantCommand(() -> m_robotShooter.idle()),
+        new InstantCommand(() -> m_robotShooter.idle())
-        new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0)))
+        // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
-        //new InstantCommand(() -> m_robotShooter.spin())
+        // new InstantCommand(() -> m_robotShooter.spin())
     );
 
     // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
@@ -376,11 +376,11 @@ public class RobotContainer {
     //     new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
     //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-        //     .whileTrue(new InstantCommand(() -> 
+            .whileTrue(new InstantCommand(() -> 
-        //     m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+            m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
-        //                                       new Translation2d(0, 0),
+                                              new Translation2d(0, 0),
-        //                         true), m_robotSwerveDrive));
+                                true), m_robotSwerveDrive));
 
         
        //?  /* Operator Buttons */

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

@@ -81,7 +81,7 @@ public class  SwerveDrive extends SubsystemBase {
       // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1));
     } else {      // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }

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

@@ -18,12 +18,14 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 public class SwerveModule extends SubsystemBase {
     private WPI_TalonFX driveMotor;
     private WPI_TalonFX angleMotor;
     private CANCoder encoder;
-
+    //private ConfigurableDouble offsetGetter;
+    private static int swerveId = 0;
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
     /** Creates a new SwerveModule. */
@@ -31,7 +33,8 @@ public class SwerveModule extends SubsystemBase {
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
         this.encoder = encoder;
-
+        // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
+         swerveId++;
         TalonFXConfiguration angleConfig = new TalonFXConfiguration();
         angleConfig.slot0.kP = swerveGains.kP;
         angleConfig.slot0.kI = swerveGains.kI;
@@ -48,7 +51,11 @@ public class SwerveModule extends SubsystemBase {
         driveMotor.setSelectedSensorPosition(0);
         driveMotor.config_kP(0, 0.2);
     }
-
+    
+    //@Override
+    // public void periodic() {
+    //     encoder.configMagnetOffset(offsetGetter.get());
+    // }
     /**
      * Get the drive motor of the SwerveModule
      * @return the drive motor of the SwerveModule