Pixy2spi

.gitignore

@@ -1,17 +1,14 @@
 # Vim temporarily files
 *.swp
 
-# WPILib default config
+# RobotPy default config
 tests/
 .wpilib/
 
-# WPILib default simulator
-simgui-ds.json
-
 # CTRE Simulator
 ctre_sim
 
-# Those were created when running `python -m robotpy sim`
+# WPILib default simulator, those were created when running `python -m robotpy sim`
 networktables.json
 simgui-ds.json
 simgui-window.json
@@ -191,5 +188,4 @@ cython_debug/
 
 # PyPI configuration file
 .pypirc
-simgui-window.json
-simgui-ds.json
+

camera.py

@@ -11,6 +11,7 @@
 from pixy2py.pixy2 import Pixy2
 from pixy2py.pixy2ccc import Pixy2CCC
 from wpilib import RobotBase, SerialPort, CameraServer
+from commands2 import Subsystem
 
 
 class AprilTagCamera(ABC):
@@ -22,16 +23,38 @@ def getYawFromTag(self, tag: int) -> float:
     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 -1
 
-    def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
+    def getRangeFromBestTarget(self) -> float:
         result = self.camera.getLatestResult()
         if result.hasTargets():
             target = result.getBestTarget()
-            return target
-        return None
+            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()
@@ -57,8 +80,7 @@ def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
                     )
                     return target.getYaw(), target_range
         return -1, -1
-
-
+
 class LimelightCamera(AprilTagCamera):
     def __init__(self, camera: str) -> None:
         self.limelight = None
@@ -87,27 +109,38 @@ def getYawFromTag(self, tag: int) -> float:
 
     def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
         return 0, 0
-
-class PixyFuelDetector:
+
+    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(255, 255, 255)
+        self.pixy.setLED(red=255, green=255, blue=255)
 
     def getBiggestBlock(self) -> Optional[Pixy2CCC.Block]:
-        blockCount: int = pixy.getCCC().getBlocks(False, Pixy2CCC.CCC_SIG1, 25)
+        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()
-        largestBlock: Optional[PixyCCC.Block] = None
+        if blocks:
+            largestBlock: Optional[Pixy2CCC.Block] = None
+
+            for block in blocks:
+                if largestBlock is None:
+                    largestBlock = block
+                elif block.getWidth() > largestBlock.getWidth():
+                    largestBlock = block
 
-        for block in blocks:
-            if largestBlock is None:
-                largestBlock = block
-            elif block.getWidth() > largestBlock.getWidth():
-                largestBlock = block
+            return largestBlock
+        return None
 
-        return largestBlock
+    def periodic(self) -> None:
+        self.getBiggestBlock()

climber.py

@@ -4,9 +4,8 @@
 
 
 class Climber(Subsystem):
-    climber: WPI_VictorSPX = WPI_VictorSPX(1)
-
     def __init__(self) -> None:
+        self.climber = WPI_VictorSPX(1)
         self.setDefaultCommand(run(lambda: self.stop()))
 
     def clockwise(self) -> None:

constants.py

@@ -21,7 +21,7 @@
 kDrivetrainPID = (0.2, 0, 0, ClosedLoopSlot.kSlot0)  # kP, kI, kD
 
 # PhotonVision
-kCameraName = "Camera7459"
+kCameraName = "camera ps3"
 kCameraHeightMeters = 0.83
 kTargetHeightMeters = 1.12
 kCameraPitchRadians = 0
@@ -63,12 +63,20 @@
 kLEDUSBPort = SerialPort.Port.kUSB1
 
 # Intake
-kIntakeAngleId = 1
-kIntakeTrackId = 12
+kIntakeAngleId = 4
+kIntakeTrackId = 1
 kPivotTimeDown = 0.35
 kPivotTimeUp = 0.5
 
 # Shooter
-kFlywheelId = 11
+kFlywheelId = 51
 kFlywheelFeedForward = (0, 0, 0, ClosedLoopSlot.kSlot0)  # kS, kV, kA
 kFlywheelPID = (0.2, 0.1, 0, ClosedLoopSlot.kSlot0)  # kP, kI, kD
+kHoodId = 53
+
+# Indexer
+kFrontRoller = 5 # 775 RedLine
+kBackRoller = 2
+
+# Turret
+kTurretId = 56

drivetrain.py

@@ -53,6 +53,7 @@ def __init__(self) -> None:
         self.drivetrain.setMaxOutput(1.0)
 
         self.field = Field2d()
+        self.slowMode = False
 
         config = SparkMaxConfig()
 
@@ -208,8 +209,15 @@ def backward(self) -> Command:
     def arcadeDrive(
         self, speed: Callable[[], float], rotate: Callable[[], float]
     ) -> Command:
+        if self.slowMode:
+            return self.run(
+                lambda: self.drivetrain.arcadeDrive(speed() * 0.5, rotate() * 0.5)
+            )
         return self.run(lambda: self.drivetrain.arcadeDrive(speed(), rotate()))
 
+    def setSlowMode(self) -> Command:
+        return self.run(lambda: setattr(self, "slowMode", not self.slowMode))
+
     def cheesyDrive(
         self,
         speed: Callable[[], float],
@@ -265,7 +273,7 @@ def aim(self, camera: AprilTagCamera, tag: int) -> Command:
         if yaw != -1:
             return PIDCommand(
                 PIDController(*constants.kDrivetrainPID[:3]),
-                yaw,
+                lambda: yaw,
                 0,
                 lambda output: self.drivetrain.arcadeDrive(0, output),
                 self,
@@ -278,31 +286,30 @@ def aimAndRange(self, camera: AprilTagCamera, tag: int) -> Command:
 
         if yaw != -1:
             return SequentialCommandGroup(
-                [
-                    PIDCommand(
-                        PIDController(*constants.kDrivetrainPID),
-                        yaw,
-                        0,
-                        lambda output: self.drivetrain.arcadeDrive(0, output),
-                        self,
-                    ),
-                    PIDCommand(
-                        PIDController(*constants.kDrivetrainPID),
-                        range,
-                        constants.kGoalRangeMeters,
-                        lambda output: self.drivetrain.arcadeDrive(output, 0),
-                        self,
-                    ),
-                ]
+                PIDCommand(
+                    PIDController(*constants.kDrivetrainPID[:3]),
+                    lambda: yaw,
+                    0,
+                    lambda output: self.drivetrain.arcadeDrive(0, output),
+                    self,
+                ),
+                PIDCommand(
+                    PIDController(*constants.kDrivetrainPID[:3]),
+                    lambda:range,
+                    constants.kGoalRangeMeters,
+                    lambda output: self.drivetrain.arcadeDrive(output, 0),
+                    self,
+                ),
             )
 
         return self.run(lambda: self.drivetrain.arcadeDrive(0, 0))
 
     def rotate(self, angle: float) -> Command:
         return PIDCommand(
-            PIDController(*constants.kDrivetrainPID),
-            self.navx.getAngle(),
+            PIDController(*constants.kDrivetrainPID[:3]),
+            lambda: self.navx.getAngle(),
             angle,
             lambda output: self.drivetrain.arcadeDrive(0, output),
             self,
         )
+

indexer.py

@@ -0,0 +1,24 @@
+from commands2 import Command, Subsystem
+from phoenix5 import WPI_VictorSPX, ControlMode
+import constants
+
+class Indexer(Subsystem):
+    def __init__(self) -> None:
+        self.front_roller = WPI_VictorSPX(constants.kFrontRoller)
+        self.back_roller = WPI_VictorSPX(constants.kBackRoller)
+
+        self.setDefaultCommand(self.activateExpulse())
+
+    def feed(self) -> None:
+        self.front_roller.set(ControlMode.PercentOutput, -0.5)
+        self.back_roller.set(ControlMode.PercentOutput, -0.7)
+
+    def expulse(self) -> None:
+        self.front_roller.set(ControlMode.PercentOutput, 0)
+        self.back_roller.set(ControlMode.PercentOutput, 0)
+
+    def activateFeed(self) -> Command:
+        return self.run(lambda: self.feed())
+
+    def activateExpulse(self) -> Command:
+        return self.run(lambda: self.expulse())

intake.py

@@ -5,15 +5,17 @@
 
 
 class Intake(Subsystem):
-    pivot: WPI_VictorSPX = WPI_VictorSPX(constants.kIntakeAngleId)
-    roller: WPI_VictorSPX = WPI_VictorSPX(constants.kIntakeTrackId)
-    pivotUp: bool = False
-    lastBurstTime: float = 0.0
-
     def __init__(self) -> None:
+        self.pivot = WPI_VictorSPX(constants.kIntakeAngleId)
+        self.roller = WPI_VictorSPX(constants.kIntakeTrackId)
+        self.pivotUp = False
+        self.lastBurstTime = 0.0
+
         SmartDashboard.putNumber("up", 0.5)
         SmartDashboard.putNumber("down", 0.5)
+
         self.pivot.setInverted(True)
+        self.setDefaultCommand(self.stopGamePieceCollector())
 
     def isPivotUp(self) -> bool:
         return self.pivotUp
@@ -55,11 +57,11 @@ def up(self) -> Command:
     def down(self) -> Command:
         return self.run(lambda: self.startPivotDown())
 
-    def collectGamePiece(self) -> None:
-        self.roller.set(ControlMode.PercentOutput, -1)
+    def collectGamePiece(self) -> Command:
+        return self.run(lambda: self.roller.set(ControlMode.PercentOutput, -1))
 
-    def stopGamePieceCollector(self) -> None:
-        self.roller.set(ControlMode.PercentOutput, 0)
+    def stopGamePieceCollector(self) -> Command:
+        return self.run(lambda: self.roller.set(ControlMode.PercentOutput, 0))
 
-    def releaseGamePiece(self) -> None:
-        self.roller.set(ControlMode.PercentOutput, 1)
+    def releaseGamePiece(self) -> Command:
+        return self.run(lambda: self.roller.set(ControlMode.PercentOutput, 1))