mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 00:28:06 -06:00
Initial commit
This commit is contained in:
astatin3
@@ -0,0 +1,219 @@
|
||||
###################################################################################
|
||||
# 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)
|
||||
Reference in New Issue
Block a user