Committing some more work

16 - NetworkTables and Limelight/Limelight.py

@@ -0,0 +1,116 @@
+from enum import Enum
+import networktables
+
+class LEDModes(Enum):
+    PIPELINE_MODE = 0
+    FORCE_OFF = 1
+    FORCE_BLINK = 2
+    FORCE_ON = 3
+
+class CameraModes(Enum):
+    VISION_PROCESSOR = 0
+    DRIVER_CAMERA = 1 # THIS OPTION DISABLES VISION PROCESSING!
+
+class Pipelines(Enum):
+    PIPELINE_0 = 0
+    PIPELINE_1 = 1
+    PIPELINE_2 = 2
+    PIPELINE_3 = 3
+    PIPELINE_4 = 4
+    PIPELINE_5 = 5
+    PIPELINE_6 = 6
+    PIPELINE_7 = 7
+    PIPELINE_8 = 8
+    PIPELINE_9 = 9
+
+class Limelight():
+    def __init__(self):
+        self.limelightTable = networktables.NetworkTablesInstance.getDefault().getTable("limelight")
+
+    def hasTargets(self):
+        return self.limelightTable.getEntry("tv").getDouble(0) == 1
+    
+    def getHorizontalOffset(self):
+        return self.limelightTable.getEntry("tx").getDouble(0)
+
+    def getVerticalOffset(self):
+        return self.limelightTable.getEntry("ty").getDouble(0)
+
+    def getTargetArea(self):
+        return self.limelightTable.getEntry("ta").getDouble(0)
+
+    def setLEDMode(self, mode):
+        if not isinstance(mode, LEDModes):
+            raise TypeError("setLEDMode must be passed an LEDModes enumeration value")
+
+        self.__setLEDMode(mode.value)
+    
+    def setCameraMode(self, mode):
+        if not isinstance(mode, CameraModes):
+            raise TypeError("setCameraMode must be passed a CameraModes enumeration value")
+
+        self.__setCameraMode(mode.value)
+
+    def setCurrentPipeline(self, pipeline):
+        if not isinstance(pipeline, Pipelines):
+            raise TypeError("setCurrentPipeline must be passed a Pipelines enumeration value")
+
+        self.__setCurrentPipeline(pipeline.value)
+
+    def getLEDMode(self):
+        for mode in (LEDModes):
+            if mode.value == self.__getLEDModeRAW():
+                return mode
+
+        return None
+
+    def getCameraMode(self):
+        for mode in (CameraModes):
+            if mode.value == self.__getCameraModeRAW():
+                return mode
+
+        return None
+
+    def getCurrentPipeline(self):
+        for mode in (Pipelines):
+            if mode.value == self.__getCurrentPipelineRAW():
+                return mode
+
+        return None
+
+    def __getLEDModeRAW(self):
+        return self.limelightTable.getEntry("ledMode").getDouble(0)
+
+    def __getCameraModeRAW(self):
+        return self.limelightTable.getEntry("camMode").getDouble(0)
+
+    def __getCurrentPipelineRAW(self):
+        return self.limelightTable.getEntry("getpipe").getDouble(0)
+
+    def __setLEDMode(self, mode):
+        if mode > 3:
+            mode = 3
+        elif mode < 0:
+            mode = 0
+
+        self.limelightTable.getEntry("ledMode").setDouble(mode)
+
+    def __setCameraMode(self, mode):
+        if mode > 1:
+            mode = 1
+        elif mode < 0:
+            mode = 0
+
+        self.limelightTable.getEntry("camMode").setDouble(mode)
+
+    def __setCurrentPipeline(self, pipeline):
+        if pipeline > 9:
+            pipeline = 9
+        elif pipeline < 0:
+            pipeline = 0
+
+        self.limelightTable.getEntry("pipeline").setDouble(pipeline)
+
+    # The initSendable portion of the original training has been removed
+    # from this example for simplicity, it may be restored at a later
+    # time

16 - NetworkTables and Limelight/robot.py

@@ -0,0 +1,82 @@
+import wpilib
+import wpilib.drive
+import wpimath.controller
+import ctre
+import Limelight
+
+def clamp(x, low, high):
+    if x < low:
+        return low
+    elif x > high:
+        return high
+    else:
+        return x
+
+class MyRobot(wpilib.TimedRobot):
+    def robotInit(self):
+        self.leftFront = ctre.WPI_VictorSPX(0)
+        self.leftRear = ctre.WPI_VictorSPX(1)
+        self.rightFront = ctre.WPI_VictorSPX(2)
+        self.rightRear = ctre.WPI_VictorSPX(3)
+
+        self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
+        self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
+        self.right.setInverted(True)
+
+        self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)
+
+        self.primary = wpilib.XboxController(0)
+
+        self.limelight = Limelight.Limelight()
+
+        self.PID_P = .1
+        self.PID_I = 0
+        self.PID_D = .01
+        self.PID_TOLERANCE = 1
+
+        self.pidController = wpimath.controller.PIDController(
+            self.PID_P,
+            self.PID_I,
+            self.PID_D
+        )
+        self.pidController.setTolerance(self.PID_TOLERANCE)
+
+        self.useLimelightTargeting = False
+
+
+    def autonomousInit(self):
+        pass
+
+    def autonomousPeriodic(self):
+        pass
+
+    def teleopInit(self):
+        pass
+
+    def teleopPeriodic(self):
+        if self.primary.getAButtonPressed():
+            self.useLimelightTargeting = True
+            self.pidController.reset()
+        elif self.primary.getAButtonReleased():
+            self.useLimelightTargeting = False
+
+        if self.useLimelightTargeting:
+            self.getOnTarget(.5)
+        else:
+            self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())
+
+    def getOnTarget(self, maxSpeed):
+        if self.limelight.hasTargets():
+            outputSpeed = clamp(
+                self.pidController.calculate(
+                    self.limelight.getHorizontalOffset(), 0),
+                -maxSpeed,
+                maxSpeed
+            )
+
+            self.drivetrain.tankDrive(outputSpeed, -outputSpeed)
+        else:
+            self.drivetrain.tankDrive(maxSpeed, -maxSpeed)
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)