update GradleRIO version to beta-3 and refactor voltage calculations …

…in simulation classes
TitaniumTigers4829 · Dec 21, 2024 · 4973c6b · 4973c6b
1 parent e33b2a1
commit 4973c6b
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 18 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,15 +151,15 @@ 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

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -1,5 +1,6 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.extras.simulation.field.SimulatedField;
@@ -74,7 +75,7 @@ public Robot() {
   public void robotPeriodic() {
     // TODO: test this!
     // Switch thread to high priority to improve loop timing
-    // Threads.setCurrentThreadPriority(true, 99);
+    Threads.setCurrentThreadPriority(true, 99);
 
     // Runs the Scheduler. This is responsible for polling buttons, adding
     // newly-scheduled commands, running already-scheduled commands, removing
@@ -84,7 +85,7 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
 
     // Return to normal thread priority
-    // Threads.setCurrentThreadPriority(false, 10);
+    Threads.setCurrentThreadPriority(false, 10);
   }
 
   /** This function is called once when the robot is disabled. */
@@ -120,6 +121,7 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    m_robotContainer.teleopInit();
   }
 
   /** This function is called periodically during operator control. */

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/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
@@ -108,13 +108,13 @@ public void setDesiredState(SwerveModuleState desiredState) {
                     RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec())
                         .in(RotationsPerSecond),
                     desiredDriveRPS))
-            .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS))));
+            .plus(Volts.of(driveFF.calculate((desiredDriveRPS)))));
     moduleSimulation.requestTurnVoltageOut(
         Volts.of(
                 turnPID.calculate(
                     moduleSimulation.getTurnAbsolutePosition().getRotations(),
                     desiredState.angle.getRotations()))
-            .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity))));
+            .plus(Volts.of(turnFF.calculate(turnPID.getSetpoint().velocity))));
   }
 
   @Override