Swerve drive proto

.github/workflows/build.yml

@@ -0,0 +1,18 @@
+name: Build
+
+on:
+  push:
+  pull_request:
+
+jobs:
+  build:
+    name: Build
+    runs-on: ubuntu-latest
+    container: wpilib/roborio-cross-ubuntu:2022-18.04
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v2
+      - name: Grant execute permission
+        run: chmod +x gradlew
+      - name: Build robot code
+        run: ./gradlew build

src/main/java/frc/mechtechsupport/modules/SwerveModuleConfig.java

@@ -0,0 +1,139 @@
+package frc.mechtechsupport.modules;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.TalonFXSensorCollection;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.mechtechsupport.util.SwerveModuleMath;
+import frc.mechtechsupport.util.TalonFXUtils;
+import frc.robot.Constants;
+
+public class SwerveModuleConfig {
+    //Module Variables
+    private final int idDrive;
+    private final int idSteer;
+    private final double degOffSet;
+
+    //Drive objects for the Module
+    private final WPI_TalonFX drive;
+    private TalonFXSensorCollection driveEncoder;
+
+    //Steering objects for the Module (encoder will be analogInput OR canCoder)
+    private final WPI_TalonFX steer;
+    private TalonFXSensorCollection steerEncoder;
+
+    //PID Vars
+    private double kPSteer;
+    private double kISteer;
+    private double kDSteer;
+    private double kFSteer;
+    private double kPDrive;
+    private double kIDrive;
+    private double kDDrive;
+    private double kFDrive;
+
+    public SwerveModuleConfig(int idDrive, int idSteer, double degOffSet, double kPSteer,
+            double kISteer, double kDSteer, double kFSteer, double kPDrive, double kIDrive, double kDDrive,
+            double kFDrive) {
+        this.idDrive = idDrive;
+        this.idSteer = idSteer;
+        this.degOffSet = degOffSet;
+        this.kPSteer = kPSteer;
+        this.kISteer = kISteer;
+        this.kDSteer = kDSteer;
+        this.kFSteer = kFSteer;
+        this.kPDrive = kPDrive;
+        this.kIDrive = kIDrive;
+        this.kDDrive = kDDrive;
+        this.kFDrive = kFDrive;
+
+        //Motor Config
+        drive = new WPI_TalonFX(idDrive);
+        steer = new WPI_TalonFX(idSteer);
+
+        //PID Config
+        drive.config_kP(0, kPDrive);
+        drive.config_kI(0, kIDrive);
+        drive.config_kD(0, kDDrive);
+        drive.config_kF(0, kFDrive);
+        steer.config_kP(0, kPSteer);
+        steer.config_kI(0, kISteer);
+        steer.config_kD(0, kDSteer);
+        steer.config_kF(0, kFSteer);
+
+        //Encoder Config
+        driveEncoder = drive.getSensorCollection();
+        steerEncoder = steer.getSensorCollection();
+    }
+
+    public void configSteerPID(double kp, double ki, double kd, double kf) {
+        steer.config_kP(0, kp);
+        steer.config_kI(0, ki);
+        steer.config_kD(0, kd);
+        steer.config_kF(0, kf);
+    }
+
+    public void configDrivePID(double kp, double ki, double kd, double kf) {
+        steer.config_kP(0, kp);
+        steer.config_kI(0, ki);
+        steer.config_kD(0, kd);
+        steer.config_kF(0, kf);
+    }
+
+    public double getDriveVelocity() {
+        return driveEncoder.getIntegratedSensorVelocity();
+    }
+
+    public double getAbsoluteEncoderRad() {
+        return Math.toRadians(getAngle());
+    }
+
+    public void setSteer(double heading) {
+        steer.set(ControlMode.Position,
+                TalonFXUtils.wheelDegreesToTicks(SwerveModuleMath.boundPM180(heading), Constants.steerGearRatio));
+    }
+
+    public void setDrive(double speed) {
+        steer.set(ControlMode.Velocity, speed);
+    }
+
+    public void setDesiredState(SwerveModuleState state) {
+        if (Math.abs(state.speedMetersPerSecond) < 0.01) {
+            steer.set(0);
+            drive.set(0);
+            return;
+        }
+        double target = state.angle.getDegrees();
+        double angle = SwerveModuleMath.boundPM180(target - getAngle());
+        double invAngle = SwerveModuleMath.boundPM180(target + 180 - getAngle());
+        double optomizedAngleMagnitude = Math.min(Math.abs(angle), Math.abs(invAngle));
+        if (Math.abs(angle) != optomizedAngleMagnitude) {
+            double pos = steer.getSelectedSensorPosition() + (invAngle) * (Constants.encoderTalonFXTicksPerRev / 360);
+            steer.set(ControlMode.Position, pos);
+            drive.set(ControlMode.Velocity, -state.speedMetersPerSecond);
+        } else {
+            double pos = steer.getSelectedSensorPosition() + (angle) * (Constants.encoderTalonFXTicksPerRev / 360);
+            steer.set(ControlMode.Position, pos);
+            drive.set(ControlMode.Velocity, state.speedMetersPerSecond);
+        }
+    }
+
+    public void resetEncoders() {
+        driveEncoder.setIntegratedSensorPosition(0, 0);
+    }
+
+    public double getAngle() {
+        double ang = steerEncoder.getIntegratedSensorPosition();
+        ang = ang * 360 / Constants.encoderTalonFXTicksPerRev + degOffSet; //Compassify
+        if (ang > 360) {
+            ang -= 360;
+        }
+        return ang;
+    }
+
+    public void stop() {
+        drive.set(0);
+        steer.set(0);
+    }
+}

src/main/java/frc/mechtechsupport/modules/SwerveOdometer.java

@@ -0,0 +1,25 @@
+package frc.mechtechsupport.modules;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/**
+ * Keeps track of the robot position relative to the field
+ */
+public class SwerveOdometer {
+    public double x = 0, y = 0;
+
+    public void update(double x, double y) {
+        this.x += x;
+        this.y += y;
+    }
+
+    public Translation2d getPoseMeters() {
+        return new Translation2d(x, y);
+    }
+
+    public void resetPosition(Pose2d pose2d) {
+        x = pose2d.getX();
+        y = pose2d.getY();
+    }
+}

src/main/java/frc/mechtechsupport/util/PIDTuning.java

@@ -0,0 +1,127 @@
+package frc.mechtechsupport.util;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.mechtechsupport.modules.SwerveModuleConfig;
+import frc.robot.Constants;
+
+public class PIDTuning {
+    //TODO: make PID Tuner for swerve
+    private static double errorVel;
+    private static double setPointVel;
+    private static double errorPos;
+    private static double setPointPos;
+    private static SwerveModuleConfig module;
+    private static double pSteer = 0;
+    private static double iSteer = 0;
+    private static double dSteer = 0;
+    private static double fSteer = 0;
+    private static double pDrive = 0;
+    private static double iDrive = 0;
+    private static double dDrive = 0;
+    private static double fDrive = 0;
+    private static int driveID = 0;
+    private static int steerID = 0;
+    private static TunableNumber tunableSteerP;
+    private static TunableNumber tunableSteerI;
+    private static TunableNumber tunableSteerD;
+    private static TunableNumber tunableSteerF;
+    private static TunableInt tunableSteerID;
+    private static TunableNumber tunableHeading;
+    private static TunableNumber tunableDriveP;
+    private static TunableNumber tunableDriveI;
+    private static TunableNumber tunableDriveD;
+    private static TunableNumber tunableDriveF;
+    private static TunableInt tunableDriveID;
+    private static TunableNumber tunableSpeed;
+
+    public PIDTuning() {
+        if (Constants.tuningMode) {
+            module = new SwerveModuleConfig(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+
+            errorVel = 0;
+            setPointVel = 0;
+            errorPos = 0;
+            setPointPos = 0;
+            tunableNumberInit();
+            shuffleboardMagik();
+            changeListener();
+        } else {
+            return;
+        }
+    }
+
+    public void shuffleboardMagik() {
+        if (Constants.tuningMode) {
+            SmartDashboard.putNumber("Velocity Error", errorVel);
+            SmartDashboard.putNumber("Velocity Set Point", setPointVel);
+            SmartDashboard.putNumber("Position Error", errorPos);
+            SmartDashboard.putNumber("Position Set Point", setPointPos);
+        } else {
+            return;
+        }
+    }
+
+    private void tunableNumberInit() {
+        tunableSteerID = new TunableInt("Steer Motor ID", 0);
+        tunableDriveID = new TunableInt("Drive Motor ID", 0);
+        tunableSteerP = new TunableNumber("Steer Motor P Gain", 0);
+        tunableDriveP = new TunableNumber("Drive Motor P Gain", 0);
+        tunableSteerI = new TunableNumber("Steer Motor I Gain", 0);
+        tunableDriveI = new TunableNumber("Drive Motor I Gain", 0);
+        tunableSteerD = new TunableNumber("Steer Motor D Gain", 0);
+        tunableDriveD = new TunableNumber("Drive Motor D Gain", 0);
+        tunableSteerF = new TunableNumber("Steer Motor F Gain", 0);
+        tunableDriveF = new TunableNumber("Drive Motor F Gain", 0);
+        tunableHeading = new TunableNumber("Heading set point", 0);
+        tunableSpeed = new TunableNumber("Speed set point", 0);
+    }
+
+    private void changeListener() {
+        tunableSteerID.addChangeListener((val) -> module = new SwerveModuleConfig(driveID, val, 0.0, pSteer,
+                iSteer, dSteer, fSteer, pDrive, iDrive, dDrive, fDrive));
+        tunableDriveID.addChangeListener((val) -> module = new SwerveModuleConfig(val, steerID, 0.0, pSteer,
+                iSteer, dSteer, fSteer, pDrive, iDrive, dDrive, fDrive));
+        tunableSteerP.addChangeListener((val) -> module.configSteerPID(val, iSteer, dSteer, fSteer));
+        tunableDriveP.addChangeListener((val) -> module.configDrivePID(val, iDrive, dDrive, fDrive));
+        tunableSteerI.addChangeListener((val) -> module.configSteerPID(pSteer, val, dSteer, fSteer));
+        tunableDriveI.addChangeListener((val) -> module.configDrivePID(pDrive, val, dDrive, fDrive));
+        tunableSteerD.addChangeListener((val) -> module.configSteerPID(pSteer, iSteer, val, fSteer));
+        tunableDriveD.addChangeListener((val) -> module.configDrivePID(pDrive, iDrive, val, fDrive));
+        tunableSteerF.addChangeListener((val) -> module.configSteerPID(pSteer, iSteer, dSteer, val));
+        tunableDriveF.addChangeListener((val) -> module.configDrivePID(pDrive, iDrive, dDrive, val));
+        tunableHeading.addChangeListener((val) -> module.setSteer(val));
+        tunableSpeed.addChangeListener((val) -> module.setDrive(val));
+    }
+
+    public void Tune() {
+        if (Constants.tuningMode) {
+            pSteer = tunableSteerP.get();
+            iSteer = tunableSteerI.get();
+            dSteer = tunableSteerD.get();
+            fSteer = tunableSteerF.get();
+            pDrive = tunableDriveP.get();
+            iDrive = tunableDriveI.get();
+            dDrive = tunableDriveD.get();
+            fDrive = tunableDriveF.get();
+            driveID = tunableDriveID.get();
+            steerID = tunableSteerID.get();
+            setPointVel = tunableSpeed.get();
+            setPointPos = tunableHeading.get();
+            errorVel = setPointVel - module.getDriveVelocity();
+            errorPos = setPointPos - module.getAngle();
+        }
+    }
+
+    public void printFeeds() {
+        if (Constants.tuningMode) {
+            System.out.println("Steer P Gain" + pSteer);
+            System.out.println("Steer I Gain" + iSteer);
+            System.out.println("Steer D Gain" + dSteer);
+            System.out.println("Steer F Gain" + fSteer);
+            System.out.println("Drive P Gain" + pDrive);
+            System.out.println("Drive I Gain" + iDrive);
+            System.out.println("Drive D Gain" + dDrive);
+            System.out.println("Drive F Gain" + fDrive);
+        }
+    }
+}

src/main/java/frc/mechtechsupport/util/SwerveModuleMath.java

@@ -0,0 +1,69 @@
+package frc.mechtechsupport.util;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+
+public class SwerveModuleMath {
+    public static double restrictAngle(double angle) {
+        while (angle > 360) {
+            angle -= 360;
+        }
+        while (angle < 0) {
+            angle += 360;
+        }
+        return angle;
+    }
+
+    public static final SwerveDriveKinematics swerveMagik = new SwerveDriveKinematics(
+            new Translation2d(Units.inchesToMeters(24) / 2, Units.inchesToMeters(24) / 2), //FR
+            new Translation2d(Units.inchesToMeters(24) / 2, -Units.inchesToMeters(24) / 2), //FL
+            new Translation2d(-Units.inchesToMeters(24) / 2, Units.inchesToMeters(24) / 2), //BR
+            new Translation2d(-Units.inchesToMeters(24) / 2, -Units.inchesToMeters(24) / 2) //BL
+    );
+
+    public static double boundPM180(double angle) {
+        while (angle >= 180.0)
+            angle -= 360.0;
+        while (angle < -180.0)
+            angle += 360.0;
+        return angle;
+    }
+
+    public static double getXHeading(double angle) {
+        while (angle > 360) {
+            angle -= 360;
+        }
+        while (angle < 0) {
+            angle += 360;
+        }
+
+        if (angle >= 0 && angle <= 90) {
+            return Math.cos(Math.toRadians(90 - angle));
+        } else if (angle >= 270 && angle <= 360) {
+            return -Math.cos(Math.toRadians(270 - angle));
+        } else if (angle >= 90 && angle <= 270) {
+            return Math.cos(-Math.toRadians(angle - 90));
+        }
+
+        return 0.0;
+    }
+
+    public static double getYHeading(double angle) {
+        while (angle > 360) {
+            angle -= 360;
+        }
+        while (angle < 0) {
+            angle += 360;
+        }
+
+        if (angle >= 0 && angle <= 90) {
+            return Math.sin(Math.toRadians(90 - angle));
+        } else if (angle >= 270 && angle <= 360) {
+            return -Math.sin(Math.toRadians(270 - angle));
+        } else if (angle >= 90 && angle <= 270) {
+            return Math.sin(-Math.toRadians(angle - 90));
+        }
+        return 0.0;
+    }
+}