swerve wrokz

src/main/java/frc/robot/commands/SwerveDriveCommand.java

@@ -3,18 +3,20 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.SwerveModuleMK3;
 
 public class SwerveDriveCommand extends CommandBase {
 
   private final SwerveDrivetrain drivetrain;
   private final XboxController controller;
 
   // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
-  private final SlewRateLimiter xspeedLimiter = new SlewRateLimiter(3);
-  private final SlewRateLimiter yspeedLimiter = new SlewRateLimiter(3);
-  private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter xspeedLimiter = new SlewRateLimiter(6);
+  private final SlewRateLimiter yspeedLimiter = new SlewRateLimiter(6);
+  private final SlewRateLimiter rotLimiter = new SlewRateLimiter(6);
 
   public SwerveDriveCommand(SwerveDrivetrain drivetrain, XboxController controller) {
     this.drivetrain = drivetrain;
@@ -46,9 +48,10 @@ public void execute() {
       -rotLimiter.calculate(controller.getX(GenericHID.Hand.kRight))
         * SwerveDrivetrain.kMaxAngularSpeed;
 
-    boolean fieldRelative = controller.getBumper(GenericHID.Hand.kLeft);
+    boolean calibrate = controller.getBumper(GenericHID.Hand.kLeft);
 
-    drivetrain.drive(xSpeed, ySpeed, rot, fieldRelative);
+    drivetrain.drive(xSpeed, ySpeed, rot, true, calibrate);
+
   }
 
 }

src/main/java/frc/robot/subsystems/SwerveDrivetrain.java

@@ -12,14 +12,22 @@
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 public class SwerveDrivetrain extends SubsystemBase {
 
-  public static final double kMaxSpeed = Units.feetToMeters(13.6); // 13.6 feet per second
+  public static final double kMaxSpeed = Units.feetToMeters(20); // 13.6 feet per second
   public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+  public static double feildCalibration = 0;
 
+  public static double frontLeftOffset = 281.689;
+  public static double frontRightOffset = 342.86;
+  public static double backLeftOffset = 279.1;
+  public static double backRightOffset = 89.64;
   /**
    * TODO: These are example values and will need to be adjusted for your robot!
    * Modules are in the order of -
@@ -51,13 +59,13 @@ public class SwerveDrivetrain extends SubsystemBase {
     )
   );
 
-  private final AnalogGyro gyro = new AnalogGyro(0);
+  private final AHRS gyro = new AHRS(SerialPort.Port.kMXP);
 
   private SwerveModuleMK3[] modules = new SwerveModuleMK3[] {
-    new SwerveModuleMK3(new TalonFX(1), new TalonFX(2), new CANCoder(0)), // Front Left
-    new SwerveModuleMK3(new TalonFX(3), new TalonFX(4), new CANCoder(1)), // Front Right
-    new SwerveModuleMK3(new TalonFX(5), new TalonFX(6), new CANCoder(2)), // Back Left
-    new SwerveModuleMK3(new TalonFX(7), new TalonFX(8), new CANCoder(3))  // Back Right
+    new SwerveModuleMK3(new TalonFX(1), new TalonFX(3), new CANCoder(2), frontLeftOffset), // Front Left
+    new SwerveModuleMK3(new TalonFX(4), new TalonFX(6), new CANCoder(5), frontRightOffset), // Front Right
+    new SwerveModuleMK3(new TalonFX(10), new TalonFX(12), new CANCoder(11), backLeftOffset), // Back Left
+    new SwerveModuleMK3(new TalonFX(7), new TalonFX(9), new CANCoder(8), backRightOffset)  // Back Right
   };
 
   public SwerveDrivetrain() {
@@ -72,16 +80,22 @@ public SwerveDrivetrain() {
    * @param rot Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
-  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean calibrateGyro) {
+
+    if(calibrateGyro){
+      double feildCalibration = -gyro.getAngle(); //recalibrates gyro offset
+    }
+
     SwerveModuleState[] states =
       kinematics.toSwerveModuleStates(
         fieldRelative
-          ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
+          ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Rotation2d.fromDegrees((-gyro.getAngle() + feildCalibration)))
           : new ChassisSpeeds(xSpeed, ySpeed, rot));
     SwerveDriveKinematics.normalizeWheelSpeeds(states, kMaxSpeed);
     for (int i = 0; i < states.length; i++) {
       SwerveModuleMK3 module = modules[i];
       SwerveModuleState state = states[i];
+      SmartDashboard.putNumber(String.valueOf(i), module.getAngle());
       module.setDesiredState(state);
     }
   }

src/main/java/frc/robot/subsystems/SwerveModuleMK3.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
@@ -9,6 +10,7 @@
 
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Units;
 
 public class SwerveModuleMK3 {
@@ -29,11 +31,13 @@ public class SwerveModuleMK3 {
   private TalonFX driveMotor;
   private TalonFX angleMotor;
   private CANCoder canCoder;
+  private double offset;
 
-  public SwerveModuleMK3(TalonFX driveMotor, TalonFX angleMotor, CANCoder canCoder) {
+  public SwerveModuleMK3(TalonFX driveMotor, TalonFX angleMotor, CANCoder canCoder, double offset) {
     this.driveMotor = driveMotor;
     this.angleMotor = angleMotor;
     this.canCoder = canCoder;
+    this.offset = offset;
 
     TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
 
@@ -44,8 +48,11 @@ public SwerveModuleMK3(TalonFX driveMotor, TalonFX angleMotor, CANCoder canCoder
     // Use the CANCoder as the remote sensor for the primary TalonFX PID
     angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
     angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+
     angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+    angleTalonFXConfiguration.integratedSensorOffsetDegrees = offset; //sets angle offset for PID within the talons
     angleMotor.configAllSettings(angleTalonFXConfiguration);
+    angleMotor.setNeutralMode(NeutralMode.Brake);
 
     TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
 
@@ -55,6 +62,7 @@ public SwerveModuleMK3(TalonFX driveMotor, TalonFX angleMotor, CANCoder canCoder
     driveTalonFXConfiguration.slot0.kF = kDriveF;
 
     driveMotor.configAllSettings(driveTalonFXConfiguration);
+    driveMotor.setNeutralMode(NeutralMode.Brake);
   }
 
 
@@ -63,14 +71,15 @@ public SwerveModuleMK3(TalonFX driveMotor, TalonFX angleMotor, CANCoder canCoder
    * @return The relative rotational position of the angle motor in degrees
    */
   public double getAngle() {
-    return canCoder.getAbsolutePosition();
+    return Math.toDegrees(Math.toRadians(canCoder.getAbsolutePosition()) - Math.toRadians(offset)); //include angle offset
   }
 
   /**
    * Set the speed + rotation of the swerve module from a SwerveModuleState object
    * @param desiredState - A SwerveModuleState representing the desired new state of the module
    */
   public void setDesiredState(SwerveModuleState desiredState) {
+
     Rotation2d currentRotation = Rotation2d.fromDegrees(getAngle());
     SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
 

vendordeps/navx_frc.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "navx_frc.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "4.0.414",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://repo1.maven.org/maven2/"
+    ],
+    "jsonUrl": "https://www.kauailabs.com/dist/frc/2021/navx_frc.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-java",
+            "version": "4.0.414"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-cpp",
+            "version": "4.0.414",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}