From 137fa367348e7ba0598b841a998c61ac9c312e8a Mon Sep 17 00:00:00 2001 From: Chris Date: Sat, 7 Sep 2024 18:55:56 -0500 Subject: [PATCH] More Python WIP --- .../poseest/drivetrain.py | 27 ++++++++++ photonlib-python-examples/poseest/robot.py | 4 ++ .../poseest/simgui-ds.json | 14 +++-- photonlib-python-examples/poseest/simgui.json | 51 +++++++++++++++++- .../poseest/swerve_module.png | Bin 0 -> 2954 bytes .../poseest/swervemodule.py | 39 ++++++++++++-- 6 files changed, 128 insertions(+), 7 deletions(-) create mode 100644 photonlib-python-examples/poseest/swerve_module.png diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index 5bb949768a..204da7dab2 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -6,6 +6,7 @@ import math import wpilib +import wpilib.simulation import wpimath.geometry import wpimath.kinematics import swervemodule @@ -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, @@ -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()) @@ -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() \ No newline at end of file diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py index b456dced69..cdf2f416ce 100644 --- a/photonlib-python-examples/poseest/robot.py +++ b/photonlib-python-examples/poseest/robot.py @@ -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: @@ -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() \ No newline at end of file diff --git a/photonlib-python-examples/poseest/simgui-ds.json b/photonlib-python-examples/poseest/simgui-ds.json index 73cc713cb5..addd5860ce 100644 --- a/photonlib-python-examples/poseest/simgui-ds.json +++ b/photonlib-python-examples/poseest/simgui-ds.json @@ -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, @@ -88,5 +91,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/photonlib-python-examples/poseest/simgui.json b/photonlib-python-examples/poseest/simgui.json index 5f9d2754c9..f7cfb3805d 100644 --- a/photonlib-python-examples/poseest/simgui.json +++ b/photonlib-python-examples/poseest/simgui.json @@ -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": { diff --git a/photonlib-python-examples/poseest/swerve_module.png b/photonlib-python-examples/poseest/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index a7b6f5ede2..e3056993dc 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -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 @@ -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( @@ -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. @@ -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()), ) @@ -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) + "/" @@ -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) +