Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrade to wpilib alpha-6 (#2434)

Browse Source 
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Ryanforce08 <rradtke1208@gmail.com>
Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com>
Co-authored-by: Jade Turner <spacey-sooty@proton.me>
Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
Sam Freund
2026-05-26 21:47:48 -04:00
committed by GitHub
parent d3de87f72b
commit e9006a2803
97 changed files with 732 additions and 1139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
photonlib-python-examples/poseest/swervemodule.py
View File

@@ -1,4 +1,4 @@
###################################################################################
Speeds ###################################################################################
# MIT License
#
# Copyright (c) PhotonVision
@@ -26,11 +26,7 @@ import math
import wpilib
import wpilib.simulation
import wpimath.controller
import wpimath.filter
import wpimath.geometry
import wpimath.kinematics
import wpimath.trajectory
import wpimath
import wpimath.units
kWheelRadius = 0.0508
@@ -60,7 +56,7 @@ class SwerveModule:
:param turningEncoderChannelB: DIO input for the turning encoder channel B
"""
self.moduleNumber = moduleNumber
self.desiredState = wpimath.kinematics.SwerveModuleState()
self.desiredVelocity = wpimath.SwerveModuleVelocity()
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
@@ -71,13 +67,14 @@ class SwerveModule:
)
# Gains are for example purposes only - must be determined for your own robot!
self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
self.drivePIDController = wpimath.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)
self.turningPIDController = wpimath.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.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
self.turnFeedforward = wpimath.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
@@ -98,76 +95,74 @@ class SwerveModule:
# 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.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel)
self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel)
self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02)
self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02)
self.simTurningEncoderPos = 0
self.simDrivingEncoderPos = 0
def getState(self) -> wpimath.kinematics.SwerveModuleState:
def getVelocity(self) -> wpimath.SwerveModuleVelocity:
"""Returns the current state of the module.
:returns: The current state of the module.
"""
return wpimath.kinematics.SwerveModuleState(
return wpimath.SwerveModuleVelocity(
self.driveEncoder.getRate(),
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
def getPosition(self) -> wpimath.SwerveModulePosition:
"""Returns the current position of the module.
:returns: The current position of the module.
"""
return wpimath.kinematics.SwerveModulePosition(
return wpimath.SwerveModulePosition(
self.driveEncoder.getDistance(),
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
def setDesiredState(
self, desiredState: wpimath.kinematics.SwerveModuleState
) -> None:
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
"""Sets the desired state for the module.
:param desiredState: Desired state with speed and angle.
:param desiredVelocity: Desired state with speed and angle.
"""
self.desiredState = desiredState
self.desiredVelocity = desiredVelocity
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
# Optimize the reference state to avoid spinning further than 90 degrees
self.desiredState.optimize(encoderRotation)
self.desiredVelocity.optimize(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.
self.desiredState.speed *= (self.desiredState.angle - encoderRotation).cos()
self.desiredVelocity.velocity *= (
self.desiredVelocity.angle - encoderRotation
).cos()
# Calculate the drive output from the drive PID controller.
driveOutput = self.drivePIDController.calculate(
self.driveEncoder.getRate(), self.desiredState.speed
self.driveEncoder.getRate(), self.desiredVelocity.velocity
)
driveFeedforward = self.driveFeedforward.calculate(self.desiredState.speed)
driveFeedforward = self.driveFeedforward.calculate(
self.desiredVelocity.velocity
)
# Calculate the turning motor output from the turning PID controller.
turnOutput = self.turningPIDController.calculate(
self.turningEncoder.getDistance(), self.desiredState.angle.radians()
self.turningEncoder.getDistance(), self.desiredVelocity.angle.radians()
)
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
self.turningMotor.setVoltage(turnOutput)
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
def getAbsoluteHeading(self) -> wpimath.Rotation2d:
return wpimath.Rotation2d(self.turningEncoder.getDistance())
def log(self) -> None:
state = self.getState()
state = self.getVelocity()
table = "Module " + str(self.moduleNumber) + "/"
wpilib.SmartDashboard.putNumber(
@@ -178,15 +173,17 @@ class SwerveModule:
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
table + "Drive Velocity Feet", state.velocity_fps
)
wpilib.SmartDashboard.putNumber(
table + "Drive Voltage", self.driveMotor.get() * 12.0
table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps
)
wpilib.SmartDashboard.putNumber(
table + "Steer Voltage", self.turningMotor.get() * 12.0
table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0
)
wpilib.SmartDashboard.putNumber(
table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0
)
def simulationPeriodic(self) -> None:
@@ -199,7 +196,9 @@ class SwerveModule:
driveSpdRaw = (
driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
)
turnSpdRaw = turnVoltage / 0.7
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