mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 00:28:06 -06:00
220 lines
9.1 KiB
Python
220 lines
9.1 KiB
Python
|
###################################################################################
|
||
|
|
# MIT License
|
||
|
|
#
|
||
|
|
# Copyright (c) PhotonVision
|
||
|
|
#
|
||
|
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
|
# of this software and associated documentation files (the "Software"), to deal
|
||
|
|
# in the Software without restriction, including without limitation the rights
|
||
|
|
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
|
|
# copies of the Software, and to permit persons to whom the Software is
|
||
|
|
# furnished to do so, subject to the following conditions:
|
||
|
|
#
|
||
|
|
# The above copyright notice and this permission notice shall be included in all
|
||
|
|
# copies or substantial portions of the Software.
|
||
|
|
#
|
||
|
|
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||
|
|
# SOFTWARE.
|
||
|
|
###################################################################################
|
||
|
|
|
||
|
|
import math
|
||
|
|
|
||
|
|
import wpilib
|
||
|
|
import wpilib.simulation
|
||
|
|
import wpimath.controller
|
||
|
|
import wpimath.filter
|
||
|
|
import wpimath.geometry
|
||
|
|
import wpimath.kinematics
|
||
|
|
import wpimath.trajectory
|
||
|
|
import wpimath.units
|
||
|
|
|
||
|
|
kWheelRadius = 0.0508
|
||
|
|
kEncoderResolution = 4096
|
||
|
|
kModuleMaxAngularVelocity = math.pi
|
||
|
|
kModuleMaxAngularAcceleration = math.tau
|
||
|
|
|
||
|
|
|
||
|
|
class SwerveModule:
|
||
|
|
def __init__(
|
||
|
|
self,
|
||
|
|
driveMotorChannel: int,
|
||
|
|
turningMotorChannel: int,
|
||
|
|
driveEncoderChannelA: int,
|
||
|
|
driveEncoderChannelB: int,
|
||
|
|
turningEncoderChannelA: int,
|
||
|
|
turningEncoderChannelB: int,
|
||
|
|
moduleNumber: int,
|
||
|
|
) -> None:
|
||
|
|
"""Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
|
||
|
|
|
||
|
|
:param driveMotorChannel: PWM output for the drive motor.
|
||
|
|
:param turningMotorChannel: PWM output for the turning motor.
|
||
|
|
:param driveEncoderChannelA: DIO input for the drive encoder channel A
|
||
|
|
:param driveEncoderChannelB: DIO input for the drive encoder channel B
|
||
|
|
:param turningEncoderChannelA: DIO input for the turning encoder channel A
|
||
|
|
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
||
|
|
"""
|
||
|
|
self.moduleNumber = moduleNumber
|
||
|
|
self.desiredState = wpimath.kinematics.SwerveModuleState()
|
||
|
|
|
||
|
|
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
||
|
|
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
||
|
|
|
||
|
|
self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
|
||
|
|
self.turningEncoder = wpilib.Encoder(
|
||
|
|
turningEncoderChannelA, turningEncoderChannelB
|
||
|
|
)
|
||
|
|
|
||
|
|
# Gains are for example purposes only - must be determined for your own robot!
|
||
|
|
self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
|
||
|
|
|
||
|
|
# Gains are for example purposes only - must be determined for your own robot!
|
||
|
|
self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
|
||
|
|
|
||
|
|
# Gains are for example purposes only - must be determined for your own robot!
|
||
|
|
self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
|
||
|
|
self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
|
||
|
|
|
||
|
|
# Set the distance per pulse for the drive encoder. We can simply use the
|
||
|
|
# distance traveled for one rotation of the wheel divided by the encoder
|
||
|
|
# resolution.
|
||
|
|
self.driveEncoder.setDistancePerPulse(
|
||
|
|
math.tau * kWheelRadius / kEncoderResolution
|
||
|
|
)
|
||
|
|
|
||
|
|
# Set the distance (in this case, angle) in radians per pulse for the turning encoder.
|
||
|
|
# This is the the angle through an entire rotation (2 * pi) divided by the
|
||
|
|
# encoder resolution.
|
||
|
|
self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)
|
||
|
|
|
||
|
|
# Limit the PID Controller's input range between -pi and pi and set the input
|
||
|
|
# to be continuous.
|
||
|
|
self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
|
||
|
|
|
||
|
|
# Simulation Support
|
||
|
|
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
||
|
|
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
||
|
|
self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
|
||
|
|
self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
|
||
|
|
self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
||
|
|
0.1, 0.02
|
||
|
|
)
|
||
|
|
self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
||
|
|
0.0001, 0.02
|
||
|
|
)
|
||
|
|
self.simTurningEncoderPos = 0
|
||
|
|
self.simDrivingEncoderPos = 0
|
||
|
|
|
||
|
|
def getState(self) -> wpimath.kinematics.SwerveModuleState:
|
||
|
|
"""Returns the current state of the module.
|
||
|
|
|
||
|
|
:returns: The current state of the module.
|
||
|
|
"""
|
||
|
|
return wpimath.kinematics.SwerveModuleState(
|
||
|
|
self.driveEncoder.getRate(),
|
||
|
|
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
||
|
|
)
|
||
|
|
|
||
|
|
def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
|
||
|
|
"""Returns the current position of the module.
|
||
|
|
|
||
|
|
:returns: The current position of the module.
|
||
|
|
"""
|
||
|
|
return wpimath.kinematics.SwerveModulePosition(
|
||
|
|
self.driveEncoder.getDistance(),
|
||
|
|
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
||
|
|
)
|
||
|
|
|
||
|
|
def setDesiredState(
|
||
|
|
self, desiredState: wpimath.kinematics.SwerveModuleState
|
||
|
|
) -> None:
|
||
|
|
"""Sets the desired state for the module.
|
||
|
|
|
||
|
|
:param desiredState: Desired state with speed and angle.
|
||
|
|
"""
|
||
|
|
self.desiredState = desiredState
|
||
|
|
|
||
|
|
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
||
|
|
|
||
|
|
# Optimize the reference state to avoid spinning further than 90 degrees
|
||
|
|
state = wpimath.kinematics.SwerveModuleState.optimize(
|
||
|
|
self.desiredState, encoderRotation
|
||
|
|
)
|
||
|
|
|
||
|
|
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||
|
|
# direction of travel that can occur when modules change directions. This results in smoother
|
||
|
|
# driving.
|
||
|
|
state.speed *= (state.angle - encoderRotation).cos()
|
||
|
|
|
||
|
|
# Calculate the drive output from the drive PID controller.
|
||
|
|
driveOutput = self.drivePIDController.calculate(
|
||
|
|
self.driveEncoder.getRate(), state.speed
|
||
|
|
)
|
||
|
|
|
||
|
|
driveFeedforward = self.driveFeedforward.calculate(state.speed)
|
||
|
|
|
||
|
|
# Calculate the turning motor output from the turning PID controller.
|
||
|
|
turnOutput = self.turningPIDController.calculate(
|
||
|
|
self.turningEncoder.getDistance(), state.angle.radians()
|
||
|
|
)
|
||
|
|
|
||
|
|
turnFeedforward = self.turnFeedforward.calculate(
|
||
|
|
self.turningPIDController.getSetpoint()
|
||
|
|
)
|
||
|
|
|
||
|
|
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
||
|
|
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
||
|
|
|
||
|
|
def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
|
||
|
|
return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
||
|
|
|
||
|
|
def log(self) -> None:
|
||
|
|
state = self.getState()
|
||
|
|
|
||
|
|
table = "Module " + str(self.moduleNumber) + "/"
|
||
|
|
wpilib.SmartDashboard.putNumber(
|
||
|
|
table + "Steer Degrees",
|
||
|
|
math.degrees(wpimath.angleModulus(state.angle.radians())),
|
||
|
|
)
|
||
|
|
wpilib.SmartDashboard.putNumber(
|
||
|
|
table + "Steer Target Degrees",
|
||
|
|
math.degrees(self.turningPIDController.getSetpoint()),
|
||
|
|
)
|
||
|
|
wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
|
||
|
|
wpilib.SmartDashboard.putNumber(
|
||
|
|
table + "Drive Velocity Target Feet", self.desiredState.speed_fps
|
||
|
|
)
|
||
|
|
wpilib.SmartDashboard.putNumber(
|
||
|
|
table + "Drive Voltage", self.driveMotor.get() * 12.0
|
||
|
|
)
|
||
|
|
wpilib.SmartDashboard.putNumber(
|
||
|
|
table + "Steer Voltage", self.turningMotor.get() * 12.0
|
||
|
|
)
|
||
|
|
|
||
|
|
def simulationPeriodic(self) -> None:
|
||
|
|
driveVoltage = (
|
||
|
|
self.simDrivingMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
|
||
|
|
)
|
||
|
|
turnVoltage = (
|
||
|
|
self.simTurningMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
|
||
|
|
)
|
||
|
|
driveSpdRaw = (
|
||
|
|
driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
|
||
|
|
)
|
||
|
|
turnSpdRaw = (
|
||
|
|
turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0)
|
||
|
|
)
|
||
|
|
driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
|
||
|
|
turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
|
||
|
|
self.simDrivingEncoderPos += 0.02 * driveSpd
|
||
|
|
self.simTurningEncoderPos += 0.02 * turnSpd
|
||
|
|
self.simDriveEncoder.setDistance(self.simDrivingEncoderPos)
|
||
|
|
self.simDriveEncoder.setRate(driveSpd)
|
||
|
|
self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
|
||
|
|
self.simTurningEncoder.setRate(turnSpd)
|