Functional PathPlanner for Larry (Command-Based)

Larry/.pathplanner/settings.json

@@ -0,0 +1,7 @@
+{
+  "robotWidth": 0.74,
+  "robotLength": 0.73678862,
+  "holonomicMode": true,
+  "generateJSON": false,
+  "generateCSV": false
+}

Larry/autos/base.py

@@ -0,0 +1,24 @@
+from commands2 import SequentialCommandGroup
+from constants import *
+from math import pi
+from pathplannerlib import PathPlannerTrajectory
+from subsystems.swerve_drive import SwerveDrive
+from autos.swerve_drive_controller import SwerveDriveControllerCommand
+from wpimath.controller import ProfiledPIDControllerRadians, PIDController
+
+class AutoBase(SequentialCommandGroup):
+    profiledThetaController = ProfiledPIDControllerRadians(AutoConstants.kPTheta, 0.0, 0.0, AutoConstants.kThetaControllerRestraints)
+    thetaController = PIDController(AutoConstants.kPTheta, 0, 0)
+
+    def __init__(self, swerve: SwerveDrive) -> None:
+        super().__init__()
+        self.swerve = swerve
+        self.addRequirements(swerve)
+        self.thetaController.enableContinuousInput(-pi, pi)
+
+    def baseSwerveCommand(self, trajectory: PathPlannerTrajectory) -> SwerveDriveControllerCommand:
+        command = SwerveDriveControllerCommand(trajectory, lambda: self.swerve.getPose(), self.swerve.kinematics,
+                                               PIDController(AutoConstants.kPXController, 0, 0),
+                                               PIDController(AutoConstants.kPYController, 0, 0), self.profiledThetaController,
+                                               lambda moduleStates: self.swerve.setModuleStates(moduleStates), [self.swerve])
+        return command

Larry/autos/swerve_drive_controller.py

@@ -0,0 +1,51 @@
+from commands2 import CommandBase, Subsystem
+from typing import Callable
+from pathplannerlib import PathPlannerTrajectory
+from wpilib import SmartDashboard, Timer
+from wpimath.controller import HolonomicDriveController, ProfiledPIDControllerRadians, PIDController
+from wpimath.geometry import *
+from wpimath.kinematics import SwerveDrive4Kinematics, SwerveModuleState
+
+class SwerveDriveControllerCommand(CommandBase):
+    """
+    Takes 2 PID controllers and 1 ProfiledPIDController to follow a trajectory using swerve drive.
+    """
+
+    def __init__(self, trajectory: PathPlannerTrajectory, pose: Callable[[], Pose2d], kinematics: SwerveDrive4Kinematics,
+                 xController: PIDController, yController: PIDController, thetaController: ProfiledPIDControllerRadians,
+                 outputModuleStates: Callable[[tuple[SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState]], None], requirements: list[Subsystem]) -> None:
+        super().__init__()
+
+        self.timer = Timer()
+        self.trajectory = trajectory
+        self.pose = pose
+        self.kinematics = kinematics
+        self.controller = HolonomicDriveController(xController, yController, thetaController)
+        self.outputModuleStates = outputModuleStates
+
+        self.addRequirements(requirements)
+
+    def initialize(self) -> None:
+        self.timer.reset()
+        self.timer.start()
+
+    def execute(self) -> None:
+        currentTime = self.timer.get()
+
+        desiredState = self.trajectory.sample(currentTime)
+
+        targetChassisSpeeds = self.controller.calculate(self.pose(), desiredState.asWPILibState(), desiredState.holonomicRotation)
+        targetModuleStates = self.kinematics.toSwerveModuleStates(targetChassisSpeeds)
+
+        SmartDashboard.putNumber("Velocity", desiredState.velocity)
+        SmartDashboard.putNumber("Angular Velocity", desiredState.holonomicAngularVelocity)
+        SmartDashboard.putNumber("Acceleration", desiredState.acceleration)
+
+        self.outputModuleStates(targetModuleStates)
+
+    def end(self, interrupted: bool) -> None:
+        self.timer.stop()
+
+    def isFinished(self) -> bool:
+        return self.timer.hasElapsed(self.trajectory.getTotalTime())
+

Larry/autos/tests.py

@@ -0,0 +1,50 @@
+from autos.base import AutoBase
+from commands2 import CommandBase, InstantCommand
+from constants import *
+from pathplannerlib import PathPlanner
+from subsystems.swerve_drive import SwerveDrive
+from wpilib import Timer
+from wpimath.geometry import Translation2d, Rotation2d
+
+class TestForward(AutoBase):
+    """
+    Moves the robot 1 meter forward using a PathPlanner trajectory.
+    """
+
+    def __init__(self, swerve: SwerveDrive) -> None:
+        super().__init__(swerve)
+
+        path = PathPlanner.loadPath("TestForward", kdefaultPathConstraints)
+        command1 = self.baseSwerveCommand(path)
+        initialState = command1.pose()
+
+        self.addCommands(
+            InstantCommand(lambda: swerve.navX.reset()),
+            InstantCommand(lambda: swerve.resetOdometry()),
+            command1
+        )
+
+class TestForwardNoPP(CommandBase):
+    """
+    Moves the robot 1 meter forward
+    """
+    timer = Timer()
+
+    def __init__(self, swerve: SwerveDrive) -> None:
+        super().__init__()
+
+        self.swerve = swerve
+
+    def initialize(self) -> None:
+        self.timer.reset()
+        self.timer.start()
+
+    def execute(self) -> None:
+        self.swerve.drive(Translation2d(10, 0), Rotation2d(), fieldRelative=False)
+
+    def end(self, interrupted: bool) -> None:
+        self.swerve.stopAllMotors()
+        self.timer.stop()
+
+    def isFinished(self) -> bool:
+        return self.timer.hasElapsed(1)

Larry/commands/autonomous.py

@@ -0,0 +1,195 @@
+from math import atan2, cos, degrees, pi, sin
+
+from commands2 import CommandBase
+from constants import *
+from pathplannerlib import PathPlanner
+from subsystems.swerve_drive import SwerveDrive
+from wpilib import RobotBase, SmartDashboard, Timer
+
+
+class FollowPath(CommandBase):
+    """
+    Follows the given path planner made and saved in pathplannerlib.
+    """
+    def __init__(self, swerveDrive: SwerveDrive, pathName: str, 
+                 pathConstraints: PathConstraints=kdefaultPathConstraints, isReversed: bool = False) -> None:
+        super().__init__()
+
+        self.drive = swerveDrive
+        self.pathConstraints = pathConstraints
+        self.path = PathPlanner.loadPath(pathName, pathConstraints, isReversed)
+
+        self.addRequirements([self.drive])
+
+    def initialize(self) -> None:
+        self.startTime = Timer.getFPGATimestamp()
+
+        self.desiredAngle = 0
+        self.desiredPosX = self.path.getInitialPose().X()
+        self.desiredPosY = self.path.getInitialPose().Y()
+        self.drive.navX.reset()
+
+    def execute(self) -> None:
+        self.sampleTime = Timer.getFPGATimestamp() - self.startTime
+        self.pathIndex = self.path.sample(self.sampleTime)
+
+        # Use trig to get translationX and translationY
+        translationHeading = self.pathIndex.pose.rotation().radians()
+        translationX = sin(translationHeading) * -self.pathIndex.velocity
+        translationY = cos(translationHeading) * -self.pathIndex.velocity
+
+        rotationX = self.pathIndex.holonomicAngularVelocity
+
+        self.desiredAngle += degrees(rotationX) / 50 # gets degrees per tick
+        self.desiredPosX = self.pathIndex.pose.X()
+        self.desiredPosY = self.pathIndex.pose.Y()
+
+        # Fix drift
+        """
+        if RobotBase.isReal():
+            angleDrift = self.desiredAngle - self.drive.getYaw()
+            print(angleDrift)
+            rotationX -= 1 / ((180 / krotationMagnitudeMM)**3) * (angleDrift ** 3) # This increases the rotationX depending on how far off we are currently
+
+        """
+        # Convert to magnitudes
+        rotationX /= klarryMaxRotSpeed
+        translationX /= klarryMaxSpeed
+        translationY /= klarryMaxSpeed
+
+        # Clamp magnitudes
+        rotationX = max(-1, min(1, rotationX))
+        translationX = max(-1, min(1, translationX))
+        translationY = max(-1, min(1, translationY))
+
+        # Send to SwerveDrive
+        self.drive.translateAndTurn(translationX, translationY, rotationX,
+                                    applyTranslationMultiplier=False, applyRotationMultiplier=False, applySpeedModifier=False)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return self.sampleTime >= self.path.getTotalTime()
+
+class MoveMeters(CommandBase):
+    """
+    Moves the robot in a straight line the chosen amount of meters.
+    """
+    def __init__(self, drive: SwerveDrive, distanceX: float, distanceY: float) -> None:
+        super().__init__()
+
+        self.drive = drive
+        self.addRequirements([self.drive])
+
+        self.distanceX = distanceX
+        self.distanceY = distanceY
+
+    def initialize(self) -> None:
+        pose = self.drive.getPose()
+        self.finalDispX = self.distanceX - pose.X() # Adjacent angle
+        self.finalDispY = self.distanceY - pose.Y() # Opposite angle
+        self.finalX = self.distanceX + pose.X()
+        self.finalY = self.distanceY + pose.Y()
+
+        # Trajectory angle
+        self.trajAngle = atan2(self.finalDispX, self.finalDispY) * (180 / pi)
+        if self.trajAngle < 0:
+            self.trajAngle += 360
+
+        self.drive.pointWheelsAtAngle(self.trajAngle)
+        self.magnitude = 0
+
+    def execute(self) -> None:
+        if not self.drive.areWheelsAtCorrectAngle():
+            self.drive.stopAllMotors()
+            self.drive.pointWheelsAtAngle(self.trajAngle)
+            return
+
+        self.magnitude += kdefaultMagIncrease
+        self.magnitude = min(kmagMax, max(-kmagMax, self.magnitude))
+        self.drive.moveAtConstantMagnitude(self.magnitude)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        pose = self.drive.getPose()
+        return pose.X() == self.finalX and pose.Y() == self.finalY
+
+class RotateToDegree(CommandBase):
+
+    def __init__(self, drive: SwerveDrive, angle: float) -> None:
+        super().__init__()
+
+        self.drive = drive
+        self.addRequirements([self.drive])
+
+        self.desAngle = angle
+
+    def initialize(self) -> None:
+        self.drive.turnInPlace(0)
+
+    def execute(self) -> None:
+        if not self.drive.areWheelsAtCorrectAngle():
+            return
+
+        angleDiff = (self.drive.getYaw() + 180) - self.desAngle
+        if angleDiff < 0:
+            angleDiff += 360
+
+        if angleDiff > 180:
+            self.drive.turnInPlace(-0.05, applyRotationMultiplier=False, applySpeedMultiplier=False)
+
+        else:
+            self.drive.turnInPlace(0.05, applyRotationMultiplier=False, applySpeedMultiplier=False)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return (round(self.desAngle) == round(self.drive.getYaw() + 180))
+
+class FollowPathNoRotation(CommandBase):
+    """
+    Follows the specified PathPlanner path without rotation.
+
+    Due to current development deadline limitations, following a path from PathPlanner with rotation is currently unavailable.
+    """
+
+    def __init__(self, swerveDrive: SwerveDrive, pathName: str, 
+                 pathConstraints: PathConstraints=kdefaultPathConstraints, isReversed: bool = False) -> None:
+        super().__init__()
+
+    def initialize(self) -> None:
+        pass
+
+    def execute(self) -> None:
+        pass
+
+    def end(self, interrupted: bool) -> None:
+        pass
+
+    def isFinished(self) -> bool:
+        return True
+
+class AutoRotate(CommandBase):
+    """
+    Rotates the robot to the specified degree related to the NavX sensor.
+    """
+
+    def __init__(self) -> None:
+        super().__init__()
+
+    def initialize(self) -> None:
+        pass
+
+    def execute(self) -> None:
+        pass
+
+    def end(self, interrupted: bool) -> None:
+        pass
+
+    def isFinished(self) -> bool:
+        return True
+