2025RobotCode/swervemodule.py at main · frc-2175/2025RobotCode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
import math

import rev
import wpimath.geometry
from wpimath.geometry import Rotation2d
from wpimath.kinematics import SwerveModulePosition, SwerveModuleState

import constants

driveMotorConfig = rev.SparkMaxConfig()
driveMotorConfig.smartCurrentLimit(40)
driveMotorConfig.encoder.velocityConversionFactor(
    (math.pi *constants.kWheelDiameter / constants.kDriveMotorReduction) / 60.0
).positionConversionFactor(
    math.pi * constants.kWheelDiameter / constants.kDriveMotorReduction
)
driveMotorConfig.closedLoop.setFeedbackSensor(rev.ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder).pidf(0,0,0,1 / constants.kMaxSpeed)

steerMotorConfig = rev.SparkMaxConfig()
steerMotorConfig.absoluteEncoder.positionConversionFactor(2 * math.pi).inverted(True)
steerMotorConfig.smartCurrentLimit(40)
steerMotorConfig.closedLoop.setFeedbackSensor(
    rev.ClosedLoopConfig.FeedbackSensor.kAbsoluteEncoder
).pid(
    1, 0.0, 0.0
).positionWrappingEnabled(True).positionWrappingInputRange(-math.pi,math.pi)

class SwerveModule:
    def __init__(self, driveMotorId: int, steerMotorId: int, angleOffset: float):
        self.driveMotor: rev.SparkMax = rev.SparkMax(driveMotorId, rev.SparkLowLevel.MotorType.kBrushless)
        self.steerMotor: rev.SparkMax = rev.SparkMax(steerMotorId, rev.SparkLowLevel.MotorType.kBrushless)
        self.angleOffset: float = angleOffset

        self.driveMotor.configure(driveMotorConfig, rev.SparkMax.ResetMode.kResetSafeParameters, rev.SparkMax.PersistMode.kPersistParameters)
        self.steerMotor.configure(steerMotorConfig, rev.SparkMax.ResetMode.kResetSafeParameters, rev.SparkMax.PersistMode.kPersistParameters)

        self.driveEncoder: rev.SparkRelativeEncoder = self.driveMotor.getEncoder()
        self.steerEncoder: rev.SparkAbsoluteEncoder = self.steerMotor.getAbsoluteEncoder()

        self.drivePidController: rev.SparkClosedLoopController = self.driveMotor.getClosedLoopController()
        self.steerPidController: rev.SparkClosedLoopController = self.steerMotor.getClosedLoopController()

    def setState(self, state: SwerveModuleState):
        state.angle += Rotation2d(self.angleOffset)
        encoderRotation = Rotation2d(self.steerEncoder.getPosition())
        state.optimize(encoderRotation)
        state.cosineScale(encoderRotation)
        self.drivePidController.setReference(state.speed, rev.SparkLowLevel.ControlType.kVelocity)
        self.steerPidController.setReference(state.angle.radians(), rev.SparkLowLevel.ControlType.kPosition)

    def getState(self) -> SwerveModuleState:
        return SwerveModuleState(
            self.driveEncoder.getVelocity(),
            Rotation2d(self.steerEncoder.getPosition()-self.angleOffset)
        )

    def getPosition(self) -> SwerveModulePosition:
        return SwerveModulePosition(
            self.driveEncoder.getPosition(),
            Rotation2d(self.steerEncoder.getPosition()-self.angleOffset)
        )