Merge branch '3rd-time-is-the-charm-for-setpoint-generation-plz' of h…

…ttps://github.com/TitaniumTigers4829/offseason-robot-code-2024 into vision-sim
TitaniumTigers4829 · Jan 3, 2025 · 79a886e · 79a886e
2 parents ad8a3ac + 509eeeb
commit 79a886e
Show file tree

Hide file tree

Showing 13 changed files with 1,723 additions and 132 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
     id "com.peterabeles.gversion" version "1.10"
     id "com.diffplug.spotless" version "6.25.0"
     id "pmd"
@@ -151,49 +151,49 @@ tasks.withType(JavaCompile) {
 }
 
 // Create version file
-// project.compileJava.dependsOn(createVersionFile)
-// gversion {
-//     srcDir       = "src/main/java/"
-//     classPackage = "frc.robot"
-//     className    = "BuildConstants"
-//     dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-//     timeZone     = "America/New_York"
-//     indent       = "  "
-// }
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc.robot"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/New_York"
+    indent       = "  "
+}
 
 // TODO: set this shit up
 // Create commit with working changes on event branches
-// task(eventDeploy) {
-//     doLast {
-//         if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
-//             def branchPrefix = "event"
-//             def branch = 'git branch --show-current'.execute().text.trim()
-//             def commitMessage = "Update at '${new Date().toString()}'"
-
-//             if (branch.startsWith(branchPrefix)) {
-//                 exec {
-//                     workingDir(projectDir)
-//                     executable 'git'
-//                     args 'add', '-A'
-//                 }
-//                 exec {
-//                     workingDir(projectDir)
-//                     executable 'git'
-//                     args 'commit', '-m', commitMessage
-//                     ignoreExitValue = true
-//                 }
-
-//                 println "Committed to branch: '$branch'"
-//                 println "Commit message: '$commitMessage'"
-//             } else {
-//                 println "Not on an event branch, skipping commit"
-//             }
-//         } else {
-//             println "Not running deploy task, skipping commit"
-//         }
-//     }
-// }
-// createVersionFile.dependsOn(eventDeploy)
+task(eventDeploy) {
+    doLast {
+        if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
+            def branchPrefix = "event"
+            def branch = 'git branch --show-current'.execute().text.trim()
+            def commitMessage = "Update at '${new Date().toString()}'"
+
+            if (branch.startsWith(branchPrefix)) {
+                exec {
+                    workingDir(projectDir)
+                    executable 'git'
+                    args 'add', '-A'
+                }
+                exec {
+                    workingDir(projectDir)
+                    executable 'git'
+                    args 'commit', '-m', commitMessage
+                    ignoreExitValue = true
+                }
+
+                println "Committed to branch: '$branch'"
+                println "Commit message: '$commitMessage'"
+            } else {
+                println "Not on an event branch, skipping commit"
+            }
+        } else {
+            println "Not running deploy task, skipping commit"
+        }
+    }
+}
+createVersionFile.dependsOn(eventDeploy)
 
 // Spotless formatting
 project.compileJava.dependsOn(spotlessApply)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -23,22 +23,22 @@ public class Robot extends LoggedRobot {
 
   public Robot() {
     // Record metadata
-    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
-    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
-    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
-    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
-    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
-    switch (BuildConstants.DIRTY) {
-      case 0:
-        Logger.recordMetadata("GitDirty", "All changes committed");
-        break;
-      case 1:
-        Logger.recordMetadata("GitDirty", "Uncomitted changes");
-        break;
-      default:
-        Logger.recordMetadata("GitDirty", "Unknown");
-        break;
-    }
+    // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+    // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+    // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    // switch (BuildConstants.DIRTY) {
+    //   case 0:
+    //     Logger.recordMetadata("GitDirty", "All changes committed");
+    //     break;
+    //   case 1:
+    //     Logger.recordMetadata("GitDirty", "Uncomitted changes");
+    //     break;
+    //   default:
+    //     Logger.recordMetadata("GitDirty", "Unknown");
+    //     break;
+    // }
 
     // Set up data receivers & replay source
     switch (Constants.CURRENT_MODE) {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -81,7 +81,7 @@ public RobotContainer() {
         /* create a swerve drive simulation */
         this.swerveDriveSimulation =
             new SwerveDriveSimulation(
-                SimulationConstants.ROBOT_MASS_KG,
+                60,
                 DriveConstants.TRACK_WIDTH,
                 DriveConstants.WHEEL_BASE,
                 DriveConstants.TRACK_WIDTH + .2,

diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java
@@ -359,14 +359,20 @@ public void update() {
             Voltage voltage =
                 Volts.of(poseVoltController.calculate(getPosition().in(Radians), output));
             Voltage feedforwardVoltage =
-                feedforward.calculate(getVelocity(), velocityForVolts(voltage));
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        velocityForVolts(voltage).in(RadiansPerSecond)));
             driveAtVoltage(feedforwardVoltage.plus(voltage));
           }
           case VELOCITY -> {
             Voltage voltage =
                 Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output));
             Voltage feedforwardVoltage =
-                feedforward.calculate(getVelocity(), RadiansPerSecond.of(output));
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        RadiansPerSecond.of(output).in(RadiansPerSecond)));
             driveAtVoltage(voltage.plus(feedforwardVoltage));
           }
         }
@@ -381,14 +387,20 @@ public void update() {
                 Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output));
             Voltage voltage = voltsForAmps(current, getVelocity());
             Voltage feedforwardVoltage =
-                feedforward.calculate(getVelocity(), velocityForVolts(voltage));
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        velocityForVolts(voltage).in(RadiansPerSecond)));
             driveAtVoltage(feedforwardVoltage.plus(voltage));
           }
           case VELOCITY -> {
             Current current =
                 Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output));
             Voltage feedforwardVoltage =
-                feedforward.calculate(getVelocity(), RadiansPerSecond.of(output));
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        RadiansPerSecond.of(output).in(RadiansPerSecond)));
             Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage);
             driveAtVoltage(voltage);
           }

diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java
@@ -295,8 +295,7 @@ public double getDriveEncoderUnGearedPositionRad() {
    * @return the final position of the drive encoder (wheel rotations), in radians
    */
   public double getDriveEncoderFinalPositionRad() {
-    return getDriveEncoderUnGearedPositionRad();
-    //  / DRIVE_GEAR_RATIO;
+    return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO;
   }
 
   /**
@@ -323,8 +322,7 @@ public double getDriveEncoderUnGearedSpeedRadPerSec() {
    * @return the final speed of the drive wheel, in radians per second
    */
   public double getDriveWheelFinalSpeedRadPerSec() {
-    return getDriveEncoderUnGearedSpeedRadPerSec();
-    // / DRIVE_GEAR_RATIO;
+    return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO;
   }
 
   /**