mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Upgrade to wpilib alpha-6 (#2434)
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
@@ -28,8 +28,7 @@ import math
|
||||
import swervemodule
|
||||
import wpilib
|
||||
import wpilib.simulation
|
||||
import wpimath.geometry
|
||||
import wpimath.kinematics
|
||||
import wpimath
|
||||
|
||||
kMaxSpeed = 3.0 # 3 meters per second
|
||||
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
||||
@@ -41,10 +40,10 @@ class Drivetrain:
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
|
||||
self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
|
||||
self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
|
||||
self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
|
||||
self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
|
||||
self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
|
||||
self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
|
||||
self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)
|
||||
|
||||
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
||||
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
||||
@@ -55,16 +54,17 @@ class Drivetrain:
|
||||
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
||||
|
||||
self.gyro = wpilib.AnalogGyro(0)
|
||||
self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
|
||||
self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro)
|
||||
self._yaw = 0.0
|
||||
|
||||
self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
|
||||
self.kinematics = wpimath.SwerveDrive4Kinematics(
|
||||
self.frontLeftLocation,
|
||||
self.frontRightLocation,
|
||||
self.backLeftLocation,
|
||||
self.backRightLocation,
|
||||
)
|
||||
|
||||
self.odometry = wpimath.kinematics.SwerveDrive4Odometry(
|
||||
self.odometry = wpimath.SwerveDrive4Odometry(
|
||||
self.kinematics,
|
||||
self.gyro.getRotation2d(),
|
||||
(
|
||||
@@ -75,7 +75,7 @@ class Drivetrain:
|
||||
),
|
||||
)
|
||||
|
||||
self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
|
||||
self.targetChassisVelocities = wpimath.ChassisVelocities()
|
||||
|
||||
self.gyro.reset()
|
||||
|
||||
@@ -95,27 +95,24 @@ class Drivetrain:
|
||||
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
||||
:param periodSeconds: Time
|
||||
"""
|
||||
swerveModuleStates = self.kinematics.toSwerveModuleStates(
|
||||
wpimath.kinematics.ChassisSpeeds.discretize(
|
||||
(
|
||||
wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
xSpeed, ySpeed, rot, self.gyro.getRotation2d()
|
||||
)
|
||||
if fieldRelative
|
||||
else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
|
||||
),
|
||||
periodSeconds,
|
||||
chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot)
|
||||
if fieldRelative:
|
||||
chassisVelocities = chassisVelocities.toRobotRelative(
|
||||
self.gyro.getRotation2d()
|
||||
)
|
||||
)
|
||||
wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, kMaxSpeed
|
||||
)
|
||||
self.frontLeft.setDesiredState(swerveModuleStates[0])
|
||||
self.frontRight.setDesiredState(swerveModuleStates[1])
|
||||
self.backLeft.setDesiredState(swerveModuleStates[2])
|
||||
self.backRight.setDesiredState(swerveModuleStates[3])
|
||||
|
||||
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
|
||||
chassisVelocities = chassisVelocities.discretize(periodSeconds)
|
||||
swerveModuleStates = self.kinematics.desaturateWheelVelocities(
|
||||
self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed
|
||||
)
|
||||
self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
|
||||
self.frontRight.setDesiredVelocity(swerveModuleStates[1])
|
||||
self.backLeft.setDesiredVelocity(swerveModuleStates[2])
|
||||
self.backRight.setDesiredVelocity(swerveModuleStates[3])
|
||||
|
||||
self.targetChassisVelocities = self.kinematics.toChassisVelocities(
|
||||
swerveModuleStates
|
||||
)
|
||||
|
||||
def updateOdometry(self) -> None:
|
||||
"""Updates the field relative position of the robot."""
|
||||
@@ -129,26 +126,26 @@ class Drivetrain:
|
||||
),
|
||||
)
|
||||
|
||||
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
|
||||
def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]:
|
||||
return [
|
||||
self.frontLeft.getState(),
|
||||
self.frontRight.getState(),
|
||||
self.backLeft.getState(),
|
||||
self.backRight.getState(),
|
||||
self.frontLeft.getVelocity(),
|
||||
self.frontRight.getVelocity(),
|
||||
self.backLeft.getVelocity(),
|
||||
self.backRight.getVelocity(),
|
||||
]
|
||||
|
||||
def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
|
||||
def getModulePoses(self) -> list[wpimath.Pose2d]:
|
||||
p = self.odometry.getPose()
|
||||
flTrans = wpimath.geometry.Transform2d(
|
||||
flTrans = wpimath.Transform2d(
|
||||
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
||||
)
|
||||
frTrans = wpimath.geometry.Transform2d(
|
||||
frTrans = wpimath.Transform2d(
|
||||
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
||||
)
|
||||
blTrans = wpimath.geometry.Transform2d(
|
||||
blTrans = wpimath.Transform2d(
|
||||
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
||||
)
|
||||
brTrans = wpimath.geometry.Transform2d(
|
||||
brTrans = wpimath.Transform2d(
|
||||
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
||||
)
|
||||
return [
|
||||
@@ -158,8 +155,8 @@ class Drivetrain:
|
||||
p.transformBy(brTrans),
|
||||
]
|
||||
|
||||
def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
|
||||
return self.kinematics.toChassisSpeeds(self.getModuleStates())
|
||||
def getChassisVelocities(self) -> wpimath.ChassisVelocities:
|
||||
return self.kinematics.toChassisVelocities(self.getModuleVelocities())
|
||||
|
||||
def log(self):
|
||||
table = "Drive/"
|
||||
@@ -169,7 +166,7 @@ class Drivetrain:
|
||||
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
||||
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
||||
|
||||
chassisSpeeds = self.getChassisSpeeds()
|
||||
chassisSpeeds = self.getChassisVelocities()
|
||||
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
||||
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
||||
wpilib.SmartDashboard.putNumber(
|
||||
@@ -177,13 +174,13 @@ class Drivetrain:
|
||||
)
|
||||
|
||||
wpilib.SmartDashboard.putNumber(
|
||||
table + "Target VX", self.targetChassisSpeeds.vx
|
||||
table + "Target VX", self.targetChassisVelocities.vx
|
||||
)
|
||||
wpilib.SmartDashboard.putNumber(
|
||||
table + "Target VY", self.targetChassisSpeeds.vy
|
||||
table + "Target VY", self.targetChassisVelocities.vy
|
||||
)
|
||||
wpilib.SmartDashboard.putNumber(
|
||||
table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
|
||||
table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps
|
||||
)
|
||||
|
||||
self.frontLeft.log()
|
||||
@@ -199,5 +196,7 @@ class Drivetrain:
|
||||
self.frontRight.simulationPeriodic()
|
||||
self.backLeft.simulationPeriodic()
|
||||
self.backRight.simulationPeriodic()
|
||||
self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
|
||||
self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
|
||||
rate = -1.0 * self.getChassisVelocities().omega_dps
|
||||
self.simGyro.setRate(rate)
|
||||
self._yaw += rate * 0.02
|
||||
self.simGyro.setYaw(self._yaw)
|
||||
|
||||
@@ -39,9 +39,11 @@ TAG_7_MOUNT_HEIGHT_m = 1.435 # From the 2024 game manual
|
||||
|
||||
|
||||
class MyRobot(wpilib.TimedRobot):
|
||||
def robotInit(self) -> None:
|
||||
def __init__(self) -> None:
|
||||
"""Robot initialization function"""
|
||||
self.controller = wpilib.XboxController(0)
|
||||
super().__init__()
|
||||
|
||||
self.controller = wpilib.NiDsXboxController(0)
|
||||
self.swerve = drivetrain.Drivetrain()
|
||||
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
||||
|
||||
|
||||
@@ -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,14 +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.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
|
||||
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
|
||||
@@ -99,66 +95,60 @@ 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
|
||||
desiredState.optimize(encoderRotation)
|
||||
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.
|
||||
desiredState.speed *= (desiredState.angle - encoderRotation).cos()
|
||||
desiredVelocity.velocity *= (desiredVelocity.angle - encoderRotation).cos()
|
||||
|
||||
# Calculate the drive output from the drive PID controller.
|
||||
driveOutput = self.drivePIDController.calculate(
|
||||
self.driveEncoder.getRate(), desiredState.speed
|
||||
self.driveEncoder.getRate(), desiredVelocity.velocity
|
||||
)
|
||||
|
||||
driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
|
||||
driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
|
||||
|
||||
# Calculate the turning motor output from the turning PID controller.
|
||||
turnOutput = self.turningPIDController.calculate(
|
||||
self.turningEncoder.getDistance(), desiredState.angle.radians()
|
||||
self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
|
||||
)
|
||||
|
||||
turnFeedforward = self.turnFeedforward.calculate(
|
||||
@@ -168,11 +158,11 @@ class SwerveModule:
|
||||
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
||||
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(
|
||||
@@ -183,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:
|
||||
|
||||
Reference in New Issue
Block a user