From 9cbb6edf89192074fa901412cb4aa240ba69aec6 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sun, 12 Nov 2023 13:38:46 -0500 Subject: [PATCH] add FOC support --- .../robot2023/config/constants/DrivetrainConstants.kt | 1 + .../drivetrain/swervemodule/SwerveModuleIOFalcon.kt | 6 +++--- 2 files changed, 4 insertions(+), 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 index 841dc084..b944808b 100644 --- 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/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 8d337781..8a541c06 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/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) }