Make swerve drive work. Some stuff on drift correction

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

@@ -40,28 +40,28 @@ 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 = 220;
+          public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25;
-          public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
-          public static final double BACK_LEFT_ROT_OFFSET = -279.08349884;
+          public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25;
-          public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656;
+          public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25;
         }
 
         public static final class IDs {
-          public static final int RIGHT_FRONT_WHEEL_ID = 3;
+          public static final int RIGHT_FRONT_WHEEL_ID = 2;
-          public static final int RIGHT_FRONT_STEER_ID = 2;
+          public static final int RIGHT_FRONT_STEER_ID = 3;
-          public static final int RIGHT_FRONT_ENCODER_ID = 12;
+          public static final int RIGHT_FRONT_ENCODER_ID = 10;
           
-        //   public static final int LEFT_FRONT_WHEEL_ID = 4;
+          public static final int LEFT_FRONT_WHEEL_ID = 4;
-        //   public static final int LEFT_FRONT_STEER_ID = 5;
+          public static final int LEFT_FRONT_STEER_ID = 5;
-        //   public static final int LEFT_FRONT_ENCODER_ID = 11;
+          public static final int LEFT_FRONT_ENCODER_ID = 11;
     
-        //   public static final int LEFT_BACK_WHEEL_ID = 6;
+          public static final int LEFT_BACK_WHEEL_ID = 6;
-        //   public static final int LEFT_BACK_STEER_ID = 7;
+          public static final int LEFT_BACK_STEER_ID = 7;
-        //   public static final int LEFT_BACK_ENCODER_ID = 12;
+          public static final int LEFT_BACK_ENCODER_ID = 12;
           
-        //   public static final int RIGHT_BACK_WHEEL_ID = 8;
+          public static final int RIGHT_BACK_WHEEL_ID = 8;
-        //   public static final int RIGHT_BACK_STEER_ID = 9;
+          public static final int RIGHT_BACK_STEER_ID = 9;
-        //   public static final int RIGHT_BACK_ENCODER_ID = 13;
+          public static final int RIGHT_BACK_ENCODER_ID = 13;
         }
     
         public static final class PIDConstants {

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

@@ -179,7 +179,8 @@ public class RobotContainer {
      * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
-
+        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
     }
 
     /**

src/main/java/frc4388/robot/RobotMap.java

@@ -48,27 +48,27 @@ public class RobotMap {
     // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
 
+    // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+    // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+
+    /* Swreve Drive Subsystem */
+    public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+    public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+    public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
+
+
     public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
     public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
     public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
     
-    /* Swreve Drive Subsystem */
+    public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
-    // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+    public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
-    // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+    public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
-    // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
     
-
+    public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
-    // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+    public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
-    // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
-    // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
-        
-    // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
-    // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
-    // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
-
-    // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
-    // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
-    // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
 
     /* Shooter Subsystem */
     // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
@@ -156,9 +156,9 @@ public class RobotMap {
     //     // initialize SwerveModules
     
         this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
-        this.leftFront = this.rightFront;
+        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
-        this.leftBack = this.rightFront;
+        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
-        this.rightBack = this.rightFront;
+        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
 
     //     this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
         // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);

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

@@ -10,10 +10,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
+import frc4388.utility.RobotUnits;
 
 public class  SwerveDrive extends SubsystemBase {
   
@@ -68,6 +70,9 @@ public class  SwerveDrive extends SubsystemBase {
   boolean stopped = false;
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
 
+    double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0;
+    SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
+
     if (fieldRelative) {
 
       double rot = 0;
@@ -75,7 +80,8 @@ public class  SwerveDrive extends SubsystemBase {
       // ! drift correction
       if (rightStick.getNorm() > 0.05) {
         rotTarget = gyro.getAngle();
-        rot = rightStick.getX();
+        rot_correction = 0;
+        // rot = rightStick.getX();
       //  SmartDashboard.putBoolean("drift correction", false);
         stopped = false;
       } else if(leftStick.getNorm() > 0.05) {
@@ -86,7 +92,7 @@ public class  SwerveDrive extends SubsystemBase {
 
      //   SmartDashboard.putBoolean("drift correction", true);
         
-        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+        // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
       }
 
@@ -95,7 +101,8 @@ 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() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
+      // chassisSpeeds = chassisSpeeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
     } else {      // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
@@ -120,8 +127,8 @@ public class  SwerveDrive extends SubsystemBase {
         }
 
      //   SmartDashboard.putBoolean("drift correction", true);
+        // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
         
-        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
       }
 
@@ -137,9 +144,15 @@ public class  SwerveDrive extends SubsystemBase {
     // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
-  public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
+  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
 
-    Translation2d rightStick = new Translation2d(-rightX, rightY);
+    // Translation2d rightStick = new Translation2d(-rightX, rightY);
+    double rightX = rightStick.getX();
+    double rightY = rightStick.getY();
+
+    double rot_correction = 0;
+
+    // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
     if(fieldRelative) {
       double rot = 0;
@@ -157,7 +170,7 @@ public class  SwerveDrive extends SubsystemBase {
     
     Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
 
-    chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
     } else {      // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
@@ -191,7 +204,7 @@ public class  SwerveDrive extends SubsystemBase {
   }
 
   public double getGyroAngle() {
-    return gyro.getAngle();
+    return -gyro.getAngle();
   }
 
   public void add180() {
@@ -238,7 +251,8 @@ public class  SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run\
-    // SmartDashboard.putNumber("Gyro", getGyroAngle());
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
+    SmartDashboard.putNumber("RotTartget", rotTarget);
   }
 
   public void shiftDown() {

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

@@ -17,7 +17,10 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.DutyCycleOut;
@@ -26,6 +29,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.ctre.phoenix6.hardware.CANcoder;
 
@@ -58,7 +62,34 @@ public class SwerveModule extends SubsystemBase {
         this.angleMotor = angleMotor;
         this.encoder = encoder;
 
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        var motorCfg = new TalonFXConfiguration()
+            .withOpenLoopRamps(
+                new OpenLoopRampsConfigs()
+                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
+            );
+
+        driveMotor.getConfigurator().apply(motorCfg);
+        
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration()
+            .withOpenLoopRamps(
+                new OpenLoopRampsConfigs()
+                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
+            );
+
         angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
 
         angleConfig.Slot0.kP = swerveGains.kP;
@@ -71,6 +102,7 @@ public class SwerveModule extends SubsystemBase {
 
         CANcoderConfiguration canconfig = new CANcoderConfiguration();
         canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
+        canconfig.MagnetSensor.MagnetOffset = offset;
         encoder.getConfigurator().apply(canconfig);
 
         rotateToAngle(0);
@@ -127,17 +159,17 @@ public class SwerveModule extends SubsystemBase {
     
     public double getAngularVel() {
         // return this.angleMotor.getSelectedSensorVelocity();
-        return 0;
+        return angleMotor.getVelocity().getValueAsDouble();
     }
 
     public double getDrivePos() {
         // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
-        return 0;
+        return driveMotor.getPosition().getValueAsDouble();
     }
 
     public double getDriveVel() {
         // return this.driveMotor.getSelectedSensorVelocity(0);
-        return 0.0;
+        return driveMotor.getVelocity().getValueAsDouble();
     }
 
     public void stop() {

src/main/java/frc4388/utility/RobotGyro.java

@@ -186,7 +186,7 @@ public class RobotGyro implements Gyro {
         m_pigeon.getAngle();
         var rotation = m_pigeon.getRotation3d();
 
-        return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)};
+        return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
     }
 
     @Override
@@ -197,7 +197,7 @@ public class RobotGyro implements Gyro {
     @Override
     public double getAngle() {
         if (m_isGyroAPigeon) {
-            return getPigeonAngles()[0];
+            return getPigeonAngles()[2];
         } else {
             return m_navX.getAngle();
         }