Merge pull request #1 from FairportRobotics/swerve

Swerve

.gitignore

@@ -1,6 +1,9 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+### Version Files ###
+src/main/deploy/versionfile.txt
+
 ### C++ ###
 # Prerequisites
 *.d

build.gradle

@@ -101,9 +101,41 @@ deployArtifact.jarTask = jar
 wpi.java.configureExecutableTasks(jar)
 wpi.java.configureTestTasks(test)
 
+def saveCommitTask = tasks.register("saveCommit") {
+    println "Current Version: " + getCheckedOutGitCommitHash();
+
+    def deployFolder = "$projectDir/src/main/deploy/"
+    def commitFile = new FileWriter(deployFolder + "versionfile.txt");
+    doLast {
+        commitFile.write(getCheckedOutGitCommitHash() + " - " + new Date().toString());
+        commitFile.close();
+        println "versionfile.txt written";
+    }
+}
+
+def getCheckedOutGitCommitHash() {
+    def gitFolder = "$projectDir/.git/"
+    def takeFromHash = 12
+    /*
+     * '.git/HEAD' contains either
+     *      in case of detached head: the currently checked out commit hash
+     *      otherwise: a reference to a file containing the current commit hash
+     */
+    def head = new File(gitFolder + "HEAD").text.split(":") // .git/HEAD
+    def isCommit = head.length == 1 // e5a7c79edabbf7dd39888442df081b1c9d8e88fd
+    // def isRef = head.length > 1     // ref: refs/heads/master
+
+    if(isCommit) return head[0].trim().take(takeFromHash) // e5a7c79edabb
+
+    def refHead = new File(gitFolder + head[1].trim()) // .git/refs/heads/master
+    refHead.text.trim().take takeFromHash
+}
+
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
+    dependsOn saveCommitTask
 }
 
+
 apply from: 'libs.gradle'

gradle.properties

@@ -1,4 +1,4 @@
 frcYear = 2026
-wpiVersion = 2026.1.1
+wpiVersion = 2026.2.1
 
 ctreVersion = 26.1.0

lib/RoboLib

@@ -1 +1 @@
-Subproject commit 12bef1b6a03a820e7a2d4f68e7f7375e040a9245
+Subproject commit f90e2ac04d38a2dd1bd1dee233d691036d9d2bb3

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "030000004c050000cc09000011810000"
+    }
+  ]
+}

src/main/java/frc/robot/Robot.java

@@ -4,14 +4,17 @@
 
 package frc.robot;
 
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.util.Scanner;
+
 import org.fairportrobotics.frc.posty.PostyManager;
-import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
-import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -31,6 +34,19 @@ public class Robot extends LoggedRobot {
 
   private RobotContainer m_robotContainer;
 
+  public static String getVersion() {
+    try {
+      Scanner scan = new Scanner(new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/versionfile.txt"));
+      String ver = scan.nextLine();
+      scan.close();
+
+      return ver;
+    } catch (FileNotFoundException e) {
+      e.printStackTrace();
+    }
+    return "";
+  }
+
   /**
    * This function is run when the robot is first started up and should be used
    * for any
@@ -40,6 +56,7 @@ public class Robot extends LoggedRobot {
   public void robotInit() {
 
     Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
+    Logger.recordMetadata("Software Version", getVersion());
 
     if (isReal()) {
       Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
@@ -50,6 +67,7 @@ public class Robot extends LoggedRobot {
       // String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
       // Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
       // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
     }
 
     // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in
@@ -57,6 +75,8 @@ public class Robot extends LoggedRobot {
     Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
                     // be added.
 
+    Logger.recordOutput("Software Version", getVersion());
+
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our
     // autonomous chooser on the dashboard.
@@ -110,7 +130,7 @@ public class Robot extends LoggedRobot {
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
+      CommandScheduler.getInstance().schedule(m_autonomousCommand);
     }
   }
 

src/main/java/frc/robot/RobotContainer.java

@@ -4,13 +4,14 @@
 
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.Autos;
+import frc.robot.commands.ExampleCommand;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ExampleSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -26,10 +27,15 @@ public class RobotContainer {
   private final CommandXboxController m_driverController =
       new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
+  // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
+
+  public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
+  
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
+
   }
 
   /**
@@ -48,7 +54,7 @@ public class RobotContainer {
 
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
     // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
 
   /**

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

@@ -0,0 +1,166 @@
+package frc.robot.subsystems;
+
+import org.fairportrobotics.frc.posty.TestableSubsystem;
+import org.fairportrobotics.frc.posty.test.PostTest;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
+import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
+import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
+import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
+import org.littletonrobotics.junction.Logger;
+
+import com.fasterxml.jackson.annotation.JsonCreator.Mode;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+public class DriveSubsystem extends TestableSubsystem {
+
+    private SwerveBuilder swerveBuilder = new SwerveBuilder();
+    private SwerveDriveSystem driveSystem;
+
+    private double driveP = 0.1;
+    private double driveI = 0.0;
+    private double driveD = 0.01;
+    private double driveKV = 0.2;
+
+    private double steerP = 20;
+    private double steerI = 0;
+    private double steerD = 0.0;
+    private double steerKV = 0;
+
+    public DriveSubsystem(CommandXboxController xboxController) {
+
+        driveSystem = swerveBuilder
+                .withCanbusName("Drive")
+                .withPigeonId(20)
+                .withMaxLinearVelocity(3.0)
+                .withMaxAngularVelocity(Math.PI * 2)
+                .withSwerveModule(
+                        swerveBuilder.new SwerveModuleBuilder()
+                                .withDriveMotorId(12)
+                                .withDriveKP(driveP)
+                                .withDriveKI(driveI)
+                                .withDriveKD(driveD)
+                                .withDriveKV(driveKV)
+                                .withSteerMotorId(3)
+                                .withSteerKP(steerP)
+                                .withSteerKI(steerI)
+                                .withSteerKD(steerD)
+                                .withSteerKV(steerKV)
+                                .withSteerOffset(0.359375)
+                                .withSteerEncoderId(10)
+                                .withModuleLocation(new Translation2d(1, 1))
+                                .withModuleName("Front Left")
+                                .withGearRatio(8.14)
+                                .withWheelDiameter(0.1016)
+                                .build())
+                .withSwerveModule(
+                        swerveBuilder.new SwerveModuleBuilder()
+                                .withDriveMotorId(5)
+                                .withDriveKP(driveP)
+                                .withDriveKI(driveI)
+                                .withDriveKD(driveD)
+                                .withDriveKV(driveKV)
+                                .withDriveInverted()
+                                .withSteerMotorId(6)
+                                .withSteerKP(steerP)
+                                .withSteerKI(steerI)
+                                .withSteerKD(steerD)
+                                .withSteerKV(steerKV)
+                                .withSteerOffset(0.406494)
+                                .withSteerEncoderId(1)
+                                .withModuleLocation(new Translation2d(1, -1))
+                                .withModuleName("Front Right")
+                                .withGearRatio(8.14)
+                                .withWheelDiameter(0.1016)
+                                .build())
+                .withSwerveModule(
+                        swerveBuilder.new SwerveModuleBuilder()
+                                .withDriveMotorId(8)
+                                .withDriveKP(driveP)
+                                .withDriveKI(driveI)
+                                .withDriveKD(driveD)
+                                .withDriveKV(driveKV)
+                                .withSteerMotorId(2)
+                                .withSteerKP(steerP)
+                                .withSteerKI(steerI)
+                                .withSteerKD(steerD)
+                                .withSteerKV(steerKV)
+                                .withSteerOffset(0.211670)
+                                .withSteerEncoderId(7)
+                                .withModuleLocation(new Translation2d(-1, 1))
+                                .withModuleName("Back Left")
+                                .withGearRatio(8.14)
+                                .withWheelDiameter(0.1016)
+                                .build())
+                .withSwerveModule(
+                        swerveBuilder.new SwerveModuleBuilder()
+                                .withDriveMotorId(9)
+                                .withDriveKP(driveP)
+                                .withDriveKI(driveI)
+                                .withDriveKD(driveD)
+                                .withDriveKV(driveKV)
+                                .withDriveInverted()
+                                .withSteerMotorId(11)
+                                .withSteerKP(steerP)
+                                .withSteerKI(steerI)
+                                .withSteerKD(steerD)
+                                .withSteerKV(steerKV)
+                                .withSteerOffset(-0.235107)
+                                .withSteerEncoderId(4)
+                                .withModuleLocation(new Translation2d(-1, -1))
+                                .withModuleName("Back Right")
+                                .withGearRatio(8.14)
+                                .withWheelDiameter(0.1016)
+                                .build())
+                .build();
+
+        this.setDefaultCommand(Commands.run(new Runnable() {
+
+            @Override
+            public void run() {
+                driveSystem.setChassisSpeedsFromJoystickRobotRelative(
+                        -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())),
+                        -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
+                        -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
+
+                // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
+
+                Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
+                Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
+                Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose());
+            }
+
+        }, this));
+
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+        driveSystem.periodic();
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        super.simulationPeriodic();
+        driveSystem.simulationPeriodic();
+    }
+
+    @PostTest()
+    public void DriveSubsystem_CanDevicesConnected(){
+
+        SwerveModule[] modules = driveSystem.getModules();
+
+        for(int i=0;i < modules.length; i++){
+            assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
+            assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
+            assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
+        }
+
+        assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();
+
+    }
+
+}