factor in antiGravitationalAcceleration into drivePeriodic

DeepBlueRobotics · Feb 11, 2025 · dbfca96 · dbfca96
1 parent 0e27e13
commit dbfca96
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java
@@ -32,6 +32,7 @@
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -94,8 +95,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkM
         final double neoStallCurrentAmps = 166;
         double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps);
         // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps);
-        drive.configure(new SparkMaxConfig().smartCurrentLimit(Math.min(50,(int)currentLimitAmps)),
-            ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+        driveConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps));
 
         this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex],
                                                                 config.kForwardVels[arrIndex],
@@ -193,10 +193,11 @@ public void periodic() {
     public void drivePeriodic() {
         String moduleString = type.toString();
         double actualSpeed = getCurrentSpeed();
+        double extraAccel = calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get());
         double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : backwardSimpleMotorFF)
             .calculateWithVelocities(
                 actualSpeed, 
-                desiredSpeed
+                desiredSpeed + extraAccel * TimedRobot.kDefaultPeriod//m/s + ( m/s^2 * s )
             );//clippedAcceleration);
         //calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())