diff --git a/photon-lib/py/deployDevVersion.bat b/photon-lib/py/deployDevVersion.bat new file mode 100644 index 000000000..4068d5bd0 --- /dev/null +++ b/photon-lib/py/deployDevVersion.bat @@ -0,0 +1,33 @@ +@echo off +setlocal + +:: Check if the first argument is provided +if "%~1"=="" ( + echo Error: No example-to-deploy provided. + exit /b 1 +) + +:: To run any example, we want to use photonlib out of the source code in this repo. +:: Build the wheel first +pushd %~dp0..\photon-lib\py +if exist build rmdir /S /Q build +python setup.py bdist_wheel +popd + +:: Add the output directory to PYTHONPATH to make sure it gets picked up ahead of any other installs +set PHOTONLIBPY_ROOT=%~dp0..\photon-lib\py +set PYTHONPATH=%PHOTONLIBPY_ROOT% + +:: make sure pip knows what we just built so it can sync/deploy it +python -m pip config --site set global.find-links %~dp0..\photon-lib\py\dist + +:: Move to to the right example folder +pushd %~1 + +:: Deploy the example +python -m robotpy sync +python -m robotpy deploy + +:: clean up +popd +python -m pip config --site unset global.find-links diff --git a/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..2b42e34e6 --- /dev/null +++ b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} \ No newline at end of file diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py new file mode 100644 index 000000000..c7d4ac466 --- /dev/null +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -0,0 +1,101 @@ +# +# 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.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) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7) + self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11) + self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15) + + self.gyro = wpilib.AnalogGyro(0) + + 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.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]) + + 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(), + ), + ) diff --git a/photonlib-python-examples/poseest/pyproject.toml b/photonlib-python-examples/poseest/pyproject.toml index 4b1a8117d..07d7ac969 100644 --- a/photonlib-python-examples/poseest/pyproject.toml +++ b/photonlib-python-examples/poseest/pyproject.toml @@ -28,4 +28,6 @@ robotpy_extras = [ ] # Other pip packages to install -requires = [] +requires = [ + "photonlibpy" +] diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py index e006e73af..860c737be 100644 --- a/photonlib-python-examples/poseest/robot.py +++ b/photonlib-python-examples/poseest/robot.py @@ -1,15 +1,26 @@ +#!/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 -from photonlibpy import PhotonCamera, version +import drivetrain class MyRobot(wpilib.TimedRobot): def robotInit(self) -> None: - self.cam = PhotonCamera("memes") - - def robotPeriodic(self) -> None: - pass + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + + def teleopPeriodic(self) -> None: - print("memes") - print(version.PHOTONLIB_VERSION) + xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed + ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed + rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed + + self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod()) + diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py new file mode 100644 index 000000000..6c5f69e22 --- /dev/null +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -0,0 +1,166 @@ +# +# 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.kinematics +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(1, 0, 0) + + # Gains are for example purposes only - must be determined for your own robot! + self.turningPIDController = wpimath.controller.ProfiledPIDController( + 1, + 0, + 0, + wpimath.trajectory.TrapezoidProfile.Constraints( + kModuleMaxAngularVelocity, + kModuleMaxAngularAcceleration, + ), + ) + + # 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.5) + + # 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) + + 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.getRate(), + 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().velocity + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) + + def log(self): + state = self.getState() + + table = "Module " + self.moduleNumber + "/" + wpilib.SmartDashboard.putNumber( + table + "Steer Degrees", _radToDeg(wpimath.angleModulus(state.angle.getRadians()))) + wpilib.SmartDashboard.putNumber( + table + "Steer Target Degrees", _radToDeg(self.turningPIDController.getSetpoint())) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Feet", _mToft(state.speedMetersPerSecond)) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", + _mToft(self.desiredState.speedMetersPerSecond)) + + + +def _radToDeg(in_val:float)->float: + return in_val * 180 / math.pi + +def _mToft(in_val:float)->float: + return in_val * 3.28084 diff --git a/photonlib-python-examples/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py new file mode 100644 index 000000000..90ae6731e --- /dev/null +++ b/photonlib-python-examples/poseest/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 *