diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java index 592fcb648a..28a54a2843 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java @@ -44,7 +44,7 @@ public class Robot extends TimedRobot { private PhotonCamera camera; private final double VISION_TURN_kP = 0.01; - private final double VISION_DES_ANGLE_rad = 0.0; + private final double VISION_DES_ANGLE_deg = 0.0; private final double VISION_STRAFE_kP = 0.5; private final double VISION_DES_RANGE_m = 1.25; @@ -121,7 +121,7 @@ public void teleopPeriodic() { // Override the driver's turn and fwd/rev command with an automatic one // That turns toward the tag, and gets the range right. turn = - (VISION_DES_ANGLE_rad - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; forward = (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed; } diff --git a/photonlib-python-examples/aimandrange/.gitignore b/photonlib-python-examples/aimandrange/.gitignore new file mode 100644 index 0000000000..fa0d62106f --- /dev/null +++ b/photonlib-python-examples/aimandrange/.gitignore @@ -0,0 +1,175 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +*-window.json +networktables.json + +__pycache__ \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..2b42e34e64 --- /dev/null +++ b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/drivetrain.py b/photonlib-python-examples/aimandrange/drivetrain.py new file mode 100644 index 0000000000..5d524e2b75 --- /dev/null +++ b/photonlib-python-examples/aimandrange/drivetrain.py @@ -0,0 +1,168 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.geometry +import wpimath.kinematics +import swervemodule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style 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.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) + 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, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + + self.gyro.reset() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + Method to drive the robot using joystick info. + :param xSpeed: Speed of the robot in the x direction (forward). + :param ySpeed: Speed of the robot in the y direction (sideways). + :param rot: Angular rate of the robot. + :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, + ) + ) + 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) + + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + return [ + self.frontLeft.getState(), + self.frontRight.getState(), + 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()) + + def log(self): + table = "Drive/" + + pose = self.odometry.getPose() + wpilib.SmartDashboard.putNumber(table + "X", pose.X()) + wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) + wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) + + chassisSpeeds = self.getChassisSpeeds() + wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Omega Degrees", chassisSpeeds.omega_dps) + + wpilib.SmartDashboard.putNumber(table + "Target VX", self.targetChassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "Target VY", self.targetChassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps) + + self.frontLeft.log() + self.frontRight.log() + self.backLeft.log() + self.backRight.log() + + self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) + + def simulationPeriodic(self): + self.frontLeft.simulationPeriodic() + 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) \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/pyproject.toml b/photonlib-python-examples/aimandrange/pyproject.toml new file mode 100644 index 0000000000..07d7ac9694 --- /dev/null +++ b/photonlib-python-examples/aimandrange/pyproject.toml @@ -0,0 +1,33 @@ +# +# Use this configuration file to control what RobotPy packages are installed +# on your RoboRIO +# + +[tool.robotpy] + +# Version of robotpy this project depends on +robotpy_version = "2024.3.2.2" + +# Which extra RobotPy components should be installed +# -> equivalent to `pip install robotpy[extra1, ...] +robotpy_extras = [ + # "all", + # "apriltag", + # "commands2", + # "cscore", + # "navx", + # "pathplannerlib", + # "phoenix5", + # "phoenix6", + # "photonvision", + # "playingwithfusion", + # "rev", + # "romi", + # "sim", + # "xrp", +] + +# Other pip packages to install +requires = [ + "photonlibpy" +] diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py new file mode 100644 index 0000000000..0007825395 --- /dev/null +++ b/photonlib-python-examples/aimandrange/robot.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpimath +import wpimath.geometry +import drivetrain + +from photonlibpy import PhotonCamera + +VISION_TURN_kP = 0.01 +VISION_DES_ANGLE_deg = 0.0 +VISION_STRAFE_kP = 0.5 +VISION_DES_RANGE_m = 1.25 +ROBOT_TO_CAM = wpimath.geometry.Transform3d( + wpimath.geometry.Translation3d(0.5,0.0,0.5), + wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0) +) + +class MyRobot(wpilib.TimedRobot): + def robotInit(self) -> None: + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + self.cam = PhotonCamera("YOUR CAMERA NAME") + + def robotPeriodic(self) -> None: + self.swerve.updateOdometry() + self.swerve.log() + + def teleopPeriodic(self) -> None: + xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed + ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed + rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed + + # Get information from the camera + targetYaw = 0.0 + targetRange = 0.0 + targetVisible = False + results = self.cam.getAllUnreadResults() + if(len(results) > 0): + result = results[-1] #take the most recent result the camera had + if result.hasTargets(): + # At least one apriltag was seen by the camera + for target in result.getTargets(): + if target.getFiducialId() == 7: + # Found tag 7, record its information + targetVisible = True + targetYaw = target.getYaw() + heightDelta = (ROBOT_TO_CAM.translation().Z() - + 1.435) # From 2024 game manual for ID 7 + angleDelta = -1.0 * ROBOT_TO_CAM.rotation().Y() - math.radians(target.getPitch()) + targetRange = heightDelta / math.tan(angleDelta) + + if(self.controller.getAButton() and targetVisible): + # Driver wants auto-alignment to tag 7 + # And, tag 7 is in sight, so we can turn toward it. + # Override the driver's turn and x-vel command with + # an automatic one that turns toward the tag + # and puts us at the right distance + rot = (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * drivetrain.kMaxAngularSpeed + xSpeed = (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * drivetrain.kMaxSpeed + + 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/aimandrange/simgui-ds.json b/photonlib-python-examples/aimandrange/simgui-ds.json new file mode 100644 index 0000000000..addd5860ce --- /dev/null +++ b/photonlib-python-examples/aimandrange/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decayRate": 0.0, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 + } + ], + "axisCount": 6, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/photonlib-python-examples/aimandrange/simgui.json b/photonlib-python-examples/aimandrange/simgui.json new file mode 100644 index 0000000000..2a9feb9f77 --- /dev/null +++ b/photonlib-python-examples/aimandrange/simgui.json @@ -0,0 +1,63 @@ +{ + "HALProvider": { + "Encoders": { + "0": { + "header": { + "open": true + } + }, + "window": { + "visible": true + } + }, + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Drivetrain Debug": "Field2d" + }, + "windows": { + "/SmartDashboard/Drivetrain Debug": { + "SwerveModules": { + "image": "swerve_module.png", + "length": 0.5, + "width": 0.5 + }, + "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/Robot": { + "open": true + } + }, + "Module 1": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/photonlib-python-examples/aimandrange/swerve_module.png b/photonlib-python-examples/aimandrange/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-python-examples/aimandrange/swerve_module.png differ diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py new file mode 100644 index 0000000000..c8c05e4aba --- /dev/null +++ b/photonlib-python-examples/aimandrange/swervemodule.py @@ -0,0 +1,182 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.kinematics +import wpimath.filter +import wpimath.geometry +import wpimath.controller +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) + diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py new file mode 100644 index 0000000000..90ae6731e4 --- /dev/null +++ b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py @@ -0,0 +1,6 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from pyfrc.tests import * diff --git a/photonlib-python-examples/aimattarget/.gitignore b/photonlib-python-examples/aimattarget/.gitignore new file mode 100644 index 0000000000..fa0d62106f --- /dev/null +++ b/photonlib-python-examples/aimattarget/.gitignore @@ -0,0 +1,175 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +*-window.json +networktables.json + +__pycache__ \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..2b42e34e64 --- /dev/null +++ b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/drivetrain.py b/photonlib-python-examples/aimattarget/drivetrain.py new file mode 100644 index 0000000000..5d524e2b75 --- /dev/null +++ b/photonlib-python-examples/aimattarget/drivetrain.py @@ -0,0 +1,168 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.geometry +import wpimath.kinematics +import swervemodule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style 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.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) + 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, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + + self.gyro.reset() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + Method to drive the robot using joystick info. + :param xSpeed: Speed of the robot in the x direction (forward). + :param ySpeed: Speed of the robot in the y direction (sideways). + :param rot: Angular rate of the robot. + :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, + ) + ) + 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) + + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + return [ + self.frontLeft.getState(), + self.frontRight.getState(), + 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()) + + def log(self): + table = "Drive/" + + pose = self.odometry.getPose() + wpilib.SmartDashboard.putNumber(table + "X", pose.X()) + wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) + wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) + + chassisSpeeds = self.getChassisSpeeds() + wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Omega Degrees", chassisSpeeds.omega_dps) + + wpilib.SmartDashboard.putNumber(table + "Target VX", self.targetChassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "Target VY", self.targetChassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps) + + self.frontLeft.log() + self.frontRight.log() + self.backLeft.log() + self.backRight.log() + + self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) + + def simulationPeriodic(self): + self.frontLeft.simulationPeriodic() + 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) \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/pyproject.toml b/photonlib-python-examples/aimattarget/pyproject.toml new file mode 100644 index 0000000000..07d7ac9694 --- /dev/null +++ b/photonlib-python-examples/aimattarget/pyproject.toml @@ -0,0 +1,33 @@ +# +# Use this configuration file to control what RobotPy packages are installed +# on your RoboRIO +# + +[tool.robotpy] + +# Version of robotpy this project depends on +robotpy_version = "2024.3.2.2" + +# Which extra RobotPy components should be installed +# -> equivalent to `pip install robotpy[extra1, ...] +robotpy_extras = [ + # "all", + # "apriltag", + # "commands2", + # "cscore", + # "navx", + # "pathplannerlib", + # "phoenix5", + # "phoenix6", + # "photonvision", + # "playingwithfusion", + # "rev", + # "romi", + # "sim", + # "xrp", +] + +# Other pip packages to install +requires = [ + "photonlibpy" +] diff --git a/photonlib-python-examples/aimattarget/robot.py b/photonlib-python-examples/aimattarget/robot.py new file mode 100644 index 0000000000..60ae2d89c3 --- /dev/null +++ b/photonlib-python-examples/aimattarget/robot.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import drivetrain + +from photonlibpy import PhotonCamera + +VISION_TURN_kP = 0.01 + +class MyRobot(wpilib.TimedRobot): + def robotInit(self) -> None: + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + self.cam = PhotonCamera("YOUR CAMERA NAME") + + def robotPeriodic(self) -> None: + self.swerve.updateOdometry() + self.swerve.log() + + def teleopPeriodic(self) -> None: + xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed + ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed + rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed + + # Get information from the camera + targetYaw = 0.0 + targetVisible = False + results = self.cam.getAllUnreadResults() + if(len(results) > 0): + result = results[-1] #take the most recent result the camera had + if result.hasTargets(): + # At least one apriltag was seen by the camera + for target in result.getTargets(): + if target.getFiducialId() == 7: + # Found tag 7, record its information + targetVisible = True + targetYaw = target.getYaw() + + if(self.controller.getAButton() and targetVisible): + # Driver wants auto-alignment to tag 7 + # And, tag 7 is in sight, so we can turn toward it. + # Override the driver's turn command with an automatic one that turns toward the tag. + rot = -1.0 * targetYaw * VISION_TURN_kP * drivetrain.kMaxAngularSpeed + + 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/aimattarget/simgui-ds.json b/photonlib-python-examples/aimattarget/simgui-ds.json new file mode 100644 index 0000000000..addd5860ce --- /dev/null +++ b/photonlib-python-examples/aimattarget/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decayRate": 0.0, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 + } + ], + "axisCount": 6, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/photonlib-python-examples/aimattarget/simgui.json b/photonlib-python-examples/aimattarget/simgui.json new file mode 100644 index 0000000000..2a9feb9f77 --- /dev/null +++ b/photonlib-python-examples/aimattarget/simgui.json @@ -0,0 +1,63 @@ +{ + "HALProvider": { + "Encoders": { + "0": { + "header": { + "open": true + } + }, + "window": { + "visible": true + } + }, + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Drivetrain Debug": "Field2d" + }, + "windows": { + "/SmartDashboard/Drivetrain Debug": { + "SwerveModules": { + "image": "swerve_module.png", + "length": 0.5, + "width": 0.5 + }, + "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/Robot": { + "open": true + } + }, + "Module 1": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/photonlib-python-examples/aimattarget/swerve_module.png b/photonlib-python-examples/aimattarget/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-python-examples/aimattarget/swerve_module.png differ diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py new file mode 100644 index 0000000000..c8c05e4aba --- /dev/null +++ b/photonlib-python-examples/aimattarget/swervemodule.py @@ -0,0 +1,182 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.kinematics +import wpimath.filter +import wpimath.geometry +import wpimath.controller +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) + diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py new file mode 100644 index 0000000000..90ae6731e4 --- /dev/null +++ b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py @@ -0,0 +1,6 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from pyfrc.tests import * diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index 25be5d19e9..5d524e2b75 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -164,5 +164,5 @@ def simulationPeriodic(self): self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() self.backRight.simulationPeriodic() - self.simGyro.setRate(self.getChassisSpeeds().omega_dps) + self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) \ No newline at end of file diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index c8c05e4aba..2995fb6b9a 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -59,7 +59,6 @@ def __init__( # 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 @@ -141,12 +140,8 @@ def setDesiredState( self.turningEncoder.getDistance(), state.angle.radians() ) - turnFeedforward = self.turnFeedforward.calculate( - self.turningPIDController.getSetpoint() - ) - self.driveMotor.setVoltage(driveOutput + driveFeedforward) - self.turningMotor.setVoltage(turnOutput + turnFeedforward) + self.turningMotor.setVoltage(turnOutput) def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) @@ -170,7 +165,7 @@ 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) + turnSpdRaw = turnVoltage / 0.7 driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw) turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw) self.simDrivingEncoderPos += 0.02 * driveSpd