Skip to content

Commit

Permalink
More Python WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@gerth2
gerth2 committed Sep 7, 2024
1 parent d8b6b7c commit 137fa36
Show file tree
Hide file tree
Showing 6 changed files with 128 additions and 7 deletions.
27 changes: 27 additions & 0 deletions photonlib-python-examples/poseest/drivetrain.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import math
import wpilib
import wpilib.simulation
import wpimath.geometry
import wpimath.kinematics
import swervemodule
Expand All @@ -30,7 +31,11 @@ def __init__(self) -> None:
self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11, 3)
self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15, 4)

self.debugField = wpilib.Field2d()
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)

self.gyro = wpilib.AnalogGyro(0)
self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)

self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
self.frontLeftLocation,
Expand Down Expand Up @@ -112,6 +117,19 @@ def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
self.backLeft.getState(),
self.backRight.getState(),
]

def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
p = self.odometry.getPose()
flTrans = wpimath.geometry.Transform2d(self.frontLeftLocation, self.frontLeft.getAbsoluteHeading())
frTrans = wpimath.geometry.Transform2d(self.frontRightLocation, self.frontRight.getAbsoluteHeading())
blTrans = wpimath.geometry.Transform2d(self.backLeftLocation, self.backLeft.getAbsoluteHeading())
brTrans = wpimath.geometry.Transform2d(self.backRightLocation, self.backRight.getAbsoluteHeading())
return [
p.transformBy(flTrans),
p.transformBy(frTrans),
p.transformBy(blTrans),
p.transformBy(brTrans),
]

def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
return self.kinematics.toChassisSpeeds(self.getModuleStates())
Expand All @@ -137,3 +155,12 @@ def log(self):
self.frontRight.log()
self.backLeft.log()
self.backRight.log()

self.debugField.getObject("EstimatedRobot").setPose(self.odometry.getPose())
self.debugField.getObject("EstimatedRobotModules").setPoses(self.getModulePoses())

def simulationPeriodic(self):
self.frontLeft.simulationPeriodic()
self.frontRight.simulationPeriodic()
self.backLeft.simulationPeriodic()
self.backRight.simulationPeriodic()
4 changes: 4 additions & 0 deletions photonlib-python-examples/poseest/robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ def robotInit(self) -> None:
self.swerve = drivetrain.Drivetrain()

def robotPeriodic(self) -> None:
self.swerve.updateOdometry()
self.swerve.log()

def teleopPeriodic(self) -> None:
Expand All @@ -25,3 +26,6 @@ def teleopPeriodic(self) -> None:

self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())

def _simulationPeriodic(self) -> None:
self.swerve.simulationPeriodic()
return super()._simulationPeriodic()
14 changes: 11 additions & 3 deletions photonlib-python-examples/poseest/simgui-ds.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,16 @@
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
}
],
"axisCount": 3,
"axisCount": 6,
"buttonCount": 4,
"buttonKeys": [
90,
Expand Down Expand Up @@ -88,5 +91,10 @@
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}
51 changes: 50 additions & 1 deletion photonlib-python-examples/poseest/simgui.json
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,56 @@
{
"HALProvider": {
"Encoders": {
"0": {
"header": {
"open": true
}
},
"window": {
"visible": true
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo"
"/FMSInfo": "FMSInfo",
"/SmartDashboard/Drivetrain Debug": "Field2d"
},
"windows": {
"/SmartDashboard/Drivetrain Debug": {
"EstimatedRobotModules": {
"image": "D:\\Projects\\photonvision\\photonlib-python-examples\\poseest\\swerve_module.png",
"length": 0.4000000059604645,
"width": 0.4000000059604645
},
"bottom": 1476,
"height": 8.210550308227539,
"left": 150,
"right": 2961,
"top": 79,
"width": 16.541748046875,
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"transitory": {
"SmartDashboard": {
"Drivetrain Debug": {
"double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobot": {
"open": true
},
"double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobotModules": {
"open": true
},
"double[]##v_/SmartDashboard/Drivetrain Debug/Robot": {
"open": true
}
},
"open": true
}
}
},
"NetworkTables Info": {
Expand Down
Binary file added photonlib-python-examples/poseest/swerve_module.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
39 changes: 36 additions & 3 deletions photonlib-python-examples/poseest/swervemodule.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,9 @@

import math
import wpilib
import wpilib.simulation
import wpimath.kinematics
import wpimath.filter
import wpimath.geometry
import wpimath.controller
import wpimath.trajectory
Expand Down Expand Up @@ -54,7 +56,7 @@ def __init__(

# Gains are for example purposes only - must be determined for your own robot!
self.turningPIDController = wpimath.controller.ProfiledPIDController(
1,
3,
0,
0,
wpimath.trajectory.TrapezoidProfile.Constraints(
Expand Down Expand Up @@ -83,6 +85,17 @@ def __init__(
# 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.
Expand All @@ -99,7 +112,7 @@ def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
:returns: The current position of the module.
"""
return wpimath.kinematics.SwerveModulePosition(
self.driveEncoder.getRate(),
self.driveEncoder.getDistance(),
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
)

Expand Down Expand Up @@ -143,7 +156,10 @@ def setDesiredState(
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
self.turningMotor.setVoltage(turnOutput + turnFeedforward)

def log(self):
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) + "/"
Expand All @@ -155,3 +171,20 @@ def log(self):
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)

0 comments on commit 137fa36

Please sign in to comment.