more WIP python swerve

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

photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json

@@ -0,0 +1 @@
+{"teamNumber": 1736}

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(),
+            ),
+        )

photonlib-python-examples/poseest/pyproject.toml

@@ -28,4 +28,6 @@ robotpy_extras = [
 ]
 
 # Other pip packages to install
-requires = []
+requires = [
+    "photonlibpy"
+]

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())
+

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

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 *