2026-robot-code/camera.py at main · TaubatexasRobotics/2026-robot-code · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
import constants
from photonlibpy import PhotonCamera
from typing import Optional, Tuple, List
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 pixy2py.pixy2 import Pixy2
from pixy2py.pixy2ccc import Pixy2CCC
from wpilib import RobotBase, SerialPort, CameraServer
from commands2 import Subsystem


class AprilTagCamera(ABC):
    @abstractmethod
    def getYawFromTag(self, tag: int) -> float:
        pass

    @abstractmethod
    def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
        pass

    @abstractmethod
    def getYawFromBestTarget(self) -> float:
        pass

    @abstractmethod
    def getRangeFromBestTarget(self) -> float:
        pass

class PhotonVisionCamera(AprilTagCamera):
    def __init__(self, camera: str) -> None:
        self.camera = PhotonCamera(camera)

    def getYawFromBestTarget(self) -> float:
        result = self.camera.getLatestResult()
        if result.hasTargets():
            target = result.getBestTarget()
            if target is not None:
                return target.getYaw()
        return 0

    def getRangeFromBestTarget(self) -> float:
        result = self.camera.getLatestResult()
        if result.hasTargets():
            target = result.getBestTarget()
            if target is not None:
                return Utils.calculateDistanceToTargetMeters(
                    constants.kCameraHeightMeters,
                    constants.kTargetHeightMeters,
                    constants.kCameraPitchRadians,
                    degreesToRadians(target.getPitch()),
                )
        return 0

    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(AprilTagCamera):
    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)

        if parsed_result is None:
            return -1

        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

    def getYawFromBestTarget(self) -> float:
        return 0

    def getRangeFromBestTarget(self) -> float:
        return 0

class PixyFuelDetector(Subsystem):
    def __init__(self) -> None:
        self.pixy = Pixy2(Pixy2.LinkType.SPI)
        self.pixy.init()
        self.pixy.setLamp(1, 1)
        self.pixy.setLED(red=255, green=255, blue=255)

    def getBiggestBlock(self) -> Optional[Pixy2CCC.Block]:
        blockCount: int = self.pixy.getCCC().getBlocks(False, Pixy2CCC.CCC_SIG1, 25)
        print("Found " + str(blockCount) + " blocks!")
        if blockCount <= 0:
            return None

        blocks = self.pixy.getCCC().getBlockCache()
        if blocks:
            largestBlock: Optional[Pixy2CCC.Block] = None

            for block in blocks:
                if largestBlock is None:
                    largestBlock = block
                elif block.getWidth() > largestBlock.getWidth():
                    largestBlock = block

            return largestBlock
        return None

    def periodic(self) -> None:
        self.getBiggestBlock()