add FOC support

team4099 · Nov 12, 2023 · 9cbb6ed · 9cbb6ed
1 parent dfb9730
commit 9cbb6ed
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -28,6 +28,7 @@ import org.team4099.lib.units.perSecond
 import kotlin.math.sqrt
 
 object DrivetrainConstants {
+  const val FOC_ENABLED = false
   const val MINIMIZE_SKEW = false
 
   const val WHEEL_COUNT = 4

diff --git a/.../kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/.../kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt
@@ -186,7 +186,7 @@ class SwerveModuleIOFalcon(
   }
 
   override fun setSteeringSetpoint(angle: Angle) {
-    steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle)))
+    steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle), DrivetrainConstants.FOC_ENABLED, 0.0, 0, false))
   }
 
   override fun setClosedLoop(
@@ -197,7 +197,7 @@ class SwerveModuleIOFalcon(
     val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign
     driveFalcon.setControl(
       VelocityDutyCycle(
-        driveSensor.velocityToRawUnits(speed), false, feedforward.inVolts / 12.0, 0, false
+        driveSensor.velocityToRawUnits(speed), DrivetrainConstants.FOC_ENABLED, feedforward.inVolts / 12.0, 0, false
       )
     )
 
@@ -211,7 +211,7 @@ class SwerveModuleIOFalcon(
    * @param speed: Desired speed
    */
   override fun setOpenLoop(steering: Angle, speed: LinearVelocity) {
-    driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX))
+    driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, DrivetrainConstants.FOC_ENABLED, false))
     setSteeringSetpoint(steering)
   }