Feature/pathplanner

.github/workflows/test.yml

@@ -0,0 +1,43 @@
+---
+name: dist
+
+on:
+  pull_request:
+  push:
+    branches:
+    - main
+    - '2027'
+    tags:
+    - '*'
+  workflow_dispatch:
+
+jobs:
+  check:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v5
+    - uses: pre-commit/action@v3.0.1
+
+  test:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: ["ubuntu-22.04", "macos-14", "windows-2022"]
+        python_version:
+        - '3.10'
+        - '3.11'
+        - '3.12'
+        - '3.13'
+        - '3.14'
+
+    steps:
+    - uses: actions/checkout@v5
+    - uses: actions/setup-python@v5
+      with:
+        python-version: ${{ matrix.python_version }}
+    - name: Install deps
+      run: |
+        pip install 'robotpy[commands2,navx,pykit,pathplannerlib,ctre,rev]<2027,>=2026.1.1' numpy pytest photonlibpy limelightlib-python
+    - name: Run tests
+      run: bash run_tests.sh
+      shell: bash

.gitignore

@@ -1,7 +1,13 @@
+# Vim temporarily files
+*.swp
+
 # WPILib default config
 tests/
 .wpilib/
 
+# WPILib default simulator
+simgui-ds.json
+
 # CTRE Simulator
 ctre_sim
 

autonomous/basicauto.py

@@ -0,0 +1,34 @@
+import constants
+from commands2 import Command
+from wpimath.controller import LTVUnicycleController
+from drivetrain import Drivetrain
+from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+
+
+class BasicAuto(Command):
+    def __init__(self, drivetrain: Drivetrain) -> None:
+        super().__init__()
+        self.trajectory = TrajectoryGenerator.generateTrajectory(
+           Pose2d(0, 0, Rotation2d(0)), 
+           [Translation2d(1, 1), Translation2d(2, -1)],
+           Pose2d(3, 0, Rotation2d(0)), 
+           TrajectoryConfig(0.5, 0.5)
+        )
+        self.reference = self.trajectory.sample(3)
+        self.controller = LTVUnicycleController(0.020)
+
+        self.drivetrain = drivetrain
+        self.addRequirements(drivetrain)
+
+    def execute(self) -> None:
+        adjustedSpeeds = self.controller.calculate(
+            self.drivetrain.getPose(), self.reference
+        )
+
+        wheelSpeeds = constants.kDrivetrainKinematics.toWheelSpeeds(adjustedSpeeds)
+
+        self.drivetrain.driveWithWheelSpeeds(wheelSpeeds)
+
+    def end(self, interrupted: bool) -> None:
+        self.drivetrain.stop()

buttons.py

@@ -20,15 +20,15 @@
     "pov-down": 180,
     "pov-left": 270,
     "pov-right": 90,
-    "left-x-axis": 0,
-    "left-y-axis": 1,
-    "right-x-axis": 2,
-    "right-y-axis": 5,
-    "left-trigger-axis": 3, 
-    "right-trigger-axis": 4 
+    "left-x-stick": 0,
+    "left-y-stick": 1,
+    "right-x-stick": 2,
+    "right-y-stick": 5,
+    "left-trigger-axis": 3,
+    "right-trigger-axis": 4,
 }
 
-#Generic dualshock4
+# Generic dualshock4
 
 g_ps4_controller = {
     "square": 4,
@@ -49,12 +49,12 @@
     "pov-down": 180,
     "pov-left": 270,
     "pov-right": 90,
-    "left-x-axis": 0,
-    "left-y-axis": 1,
-    "right-x-axis": 2,
-    "right-y-axis": 3,
-    "left-trigger-axis": 2, 
-    "right-trigger-axis": 3 
+    "left-x-stick": 0,
+    "left-y-stick": 1,
+    "right-x-stick": 2,
+    "right-y-stick": 3,
+    "left-trigger-axis": 2,
+    "right-trigger-axis": 3,
 }
 
 # Generic Nintendo Switch Pro Controller
@@ -81,32 +81,32 @@
     "rb": 6,
     "back": 7,
     "start": 8,
-    "press_left_stick": 9,
-    "press_right_stick": 10,
+    "press-left-stick": 9,
+    "press-right-stick": 10,
     "pov-up": 0,
     "pov-down": 180,
     "pov-left": 270,
     "pov-right": 90,
     "left-x-stick": 0,
     "left-y-stick": 1,
-    "left-trigger-axis": 2, 
-    "right-trigger-axis": 3, 
+    "left-trigger-axis": 2,
+    "right-trigger-axis": 3,
     "right-y-stick": 5,
     "right-x-stick": 4,
 }
 
 # Generic steering wheel controller (generic Xbox 360 controller)
 steering_wheel = {
     "turn-axis": 0,
-    "a" : 3,
-    "b" : 2,
-    "y" : 1,
-    "x" : 4,
-    "lb" : 5,
-    "rb" : 6,
-    "rt" : 8,
-    "lt" : 7,
-    "r3" : 12,
-    "l3" : 11,
-    "back" : 9,
-}
+    "a": 3,
+    "b": 2,
+    "y": 1,
+    "x": 4,
+    "lb": 5,
+    "rb": 6,
+    "rt": 8,
+    "lt": 7,
+    "r3": 12,
+    "l3": 11,
+    "back": 9,
+}

camera.py

@@ -1,43 +1,91 @@
-from photonlibpy import PhotonCamera
-import wpimath.units
-import constants
-from typing import Optional, Tuple
-from utils import Utils
-
-from photonlibpy.targeting.photonTrackedTarget import PhotonTrackedTarget
-
-class AprilTagCamera(PhotonCamera):
-    def __init__(self, camera: str) -> None:
-        self.camera = PhotonCamera(camera)
-
-    def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
-        result = self.camera.getLatestResult()
-        if result.hasTargets():
-            target = result.getBestTarget()
-            return target
-        return None
-
-    def getYaw(self, tag: int) -> float:
-        results = self.camera.getAllUnreadResults()
-        if len(results) > 0:
-            result = results[-1]
-            for target in result.getTargets():
-                if target.getFiducialId() == tag:
-                    return target.getYaw()
-        return -1
-
-    def getYawWithRange(self, tag: int) -> Tuple[float, float]:
-        results = self.camera.getAllUnreadResults()
-        target_range = 0
-        if len(results) > 0:
-            result = results[-1]
-            for target in result.getTargets():
-                if target.getFiducialId() == tag:
-                    target_range = Utils.calculateDistanceToTargetMeters(
-                        constants.kCameraHeightMeters,
-                        constants.kTargetHeightMeters,
-                        constants.kCameraPitchRadians,
-                        wpimath.units.degreesToRadians(target.getPitch())
-                    )
-                    return target.getYaw(), target_range
-        return -1, -1
+import constants
+from photonlibpy import PhotonCamera
+from typing import Optional, Tuple
+from utils import Utils
+from abc import ABC, abstractmethod
+from limelight import Limelight
+from limelightresults import parse_results
+from wpinet import PortForwarder
+from photonlibpy.targeting.photonTrackedTarget import PhotonTrackedTarget
+from wpimath.units import degreesToRadians
+from wpilib import RobotBase
+
+class Camera(ABC):
+    @abstractmethod
+    def getYawFromTag(self, tag: int) -> float:
+        pass
+
+    @abstractmethod
+    def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
+        pass
+
+
+class PhotonVisionCamera(Camera):
+    def __init__(self, camera: str) -> None:
+        self.camera = PhotonCamera(camera)
+
+    def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
+        result = self.camera.getLatestResult()
+        if result.hasTargets():
+            target = result.getBestTarget()
+            return target
+        return None
+
+    def getYawFromTag(self, tag: int) -> float:
+        results = self.camera.getAllUnreadResults()
+        if len(results) > 0:
+            result = results[-1]
+            for target in result.getTargets():
+                if target.getFiducialId() == tag:
+                    return target.getYaw()
+        return -1
+
+    def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
+        results = self.camera.getAllUnreadResults()
+        target_range = 0
+        if len(results) > 0:
+            result = results[-1]
+            for target in result.getTargets():
+                if target.getFiducialId() == tag:
+                    target_range = Utils.calculateDistanceToTargetMeters(
+                        constants.kCameraHeightMeters,
+                        constants.kTargetHeightMeters,
+                        constants.kCameraPitchRadians,
+                        degreesToRadians(target.getPitch()),
+                    )
+                    return target.getYaw(), target_range
+        return -1, -1
+
+
+class LimelightCamera(Camera):
+    def __init__(self, camera: str) -> None:
+        self.limelight = None
+
+        if not RobotBase.isSimulation():
+            self.limelight = Limelight(camera)
+            self.limelight.pipeline_switch(0)
+
+        PortForwarder.getInstance().add(*constants.kLimelightPortForwarder)
+
+    def getYawFromTag(self, tag: int) -> float:
+        if self.limelight is None:
+            return -1
+
+        result = self.limelight.get_results()
+        parsed_result = parse_results(result)
+
+        for target in parsed_result.fiducialResults:
+            if target.fiducial_id == tag:
+                return target.target_x_degrees  # yaw
+
+        return -1
+
+    def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
+        return 0, 0
+
+class Pixy2(Camera):
+    pass
+
+
+class DriverCamera(Camera):
+    pass

climber.py

@@ -3,6 +3,7 @@
 import phoenix6
 import phoenix5
 
+
 class Climber:
     def __init__(self):
         self.left_motor = phoenix5.WPI_VictorSPX(1)
@@ -16,4 +17,4 @@ def stop(self):
         self.climber.set(0)
 
     def down(self):
-        self.climber.set(-1)
+        self.climber.set(-1)