From 6f907380caf3a99aa518b5df4baf9ec642d78111 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Tue, 13 Dec 2022 20:35:53 -0500 Subject: [PATCH] Committing some more work --- 14 - Gyroscopes/Challenge/README.md | 5 + 14 - Gyroscopes/Challenge/robot.py | 79 +++++++++ 14 - Gyroscopes/README.md | 0 14 - Gyroscopes/robot.py | 40 +++++ 15 - PID Control/Challenge/README.md | 8 + 15 - PID Control/Challenge/robot.py | 161 ++++++++++++++++++ 15 - PID Control/README.md | 0 15 - PID Control/robot.py | 126 ++++++++++++++ 16 - NetworkTables and Limelight/Limelight.py | 116 +++++++++++++ 16 - NetworkTables and Limelight/README.md | 0 16 - NetworkTables and Limelight/robot.py | 82 +++++++++ 17 - Pneumatics/README.md | 0 17 - Pneumatics/robot.py | 51 ++++++ .../Challenge/README.md | 0 .../Challenge/robot.py | 0 18 - CameraServer and Shuffleboard/README.md | 0 18 - CameraServer and Shuffleboard/robot.py | 38 +++++ 17 files changed, 706 insertions(+) create mode 100644 14 - Gyroscopes/Challenge/README.md create mode 100644 14 - Gyroscopes/Challenge/robot.py create mode 100644 14 - Gyroscopes/README.md create mode 100644 14 - Gyroscopes/robot.py create mode 100644 15 - PID Control/Challenge/README.md create mode 100644 15 - PID Control/Challenge/robot.py create mode 100644 15 - PID Control/README.md create mode 100644 15 - PID Control/robot.py create mode 100644 16 - NetworkTables and Limelight/Limelight.py create mode 100644 16 - NetworkTables and Limelight/README.md create mode 100644 16 - NetworkTables and Limelight/robot.py create mode 100644 17 - Pneumatics/README.md create mode 100644 17 - Pneumatics/robot.py create mode 100644 18 - CameraServer and Shuffleboard/Challenge/README.md create mode 100644 18 - CameraServer and Shuffleboard/Challenge/robot.py create mode 100644 18 - CameraServer and Shuffleboard/README.md create mode 100644 18 - CameraServer and Shuffleboard/robot.py diff --git a/14 - Gyroscopes/Challenge/README.md b/14 - Gyroscopes/Challenge/README.md new file mode 100644 index 0000000..dbae949 --- /dev/null +++ b/14 - Gyroscopes/Challenge/README.md @@ -0,0 +1,5 @@ +Combine the use of encoders and gyroscopes to do the following +- Drive forward 60 inches +- Rotate to 45 degrees (assume a left turn is negative rotation, right turn is positive rotation) +- Drive forward 20 inches +- Rotate to -90 degrees \ No newline at end of file diff --git a/14 - Gyroscopes/Challenge/robot.py b/14 - Gyroscopes/Challenge/robot.py new file mode 100644 index 0000000..d46ee0c --- /dev/null +++ b/14 - Gyroscopes/Challenge/robot.py @@ -0,0 +1,79 @@ +import wpilib +import wpilib.drive +import math +import ctre + +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.PULSES_PER_REVOLUTION = 512 + self.WHEEL_DIAMETER = 6 + self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION + + self.leftDrivetrainEncoder = wpilib.Encoder(0, 1) + self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + self.rightDrivetrainEncoder = wpilib.Encoder(2, 3) + self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + + self.gyro = wpilib.ADXRS450_Gyro() + self.gyro.calibrate() + + def autonomousInit(self): + self.leftDrivetrainEncoder.reset() + self.rightDrivetrainEncoder.reset() + self.gyro.reset() + + self.autoState = "DriveForward60" + + def autonomousPeriodic(self): + if self.autoState == "DriveForward60": + if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 60: + self.drivetrain.tankDrive(.5, .5) + else: + self.drivetrain.tankDrive(0, 0) + self.gyro.reset() + self.autoState = "Rotate45" + elif self.autoState == "Rotate45": + if self.gyro.getAngle() < 45: + self.drivetrain.tankDrive(.5, -.5) + else: + self.drivetrain.tankDrive(0, 0) + self.leftDrivetrainEncoder.reset() + self.rightDrivetrainEncoder.reset() + self.autoState = "DriveForward20" + elif self.autoState == "DriveForward20": + if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 20: + self.drivetrain.tankDrive(.5, .5) + else: + self.drivetrain.tankDrive(0, 0) + self.gyro.reset() + self.autoState = "RotateNegative90" + elif self.autoState == "RotateNegative90": + if self.gyro.getAngle() > -90: + self.drivetrain.tankDrive(-.5, .5) + else: + self.drivetrain.tankDrive(0, 0) + self.autoState = "End" + else: + self.drivetrain.tankDrive(0, 0) + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file diff --git a/14 - Gyroscopes/README.md b/14 - Gyroscopes/README.md new file mode 100644 index 0000000..e69de29 diff --git a/14 - Gyroscopes/robot.py b/14 - Gyroscopes/robot.py new file mode 100644 index 0000000..a652140 --- /dev/null +++ b/14 - Gyroscopes/robot.py @@ -0,0 +1,40 @@ +import wpilib +import wpilib.drive +import math +import ctre + +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.gyro = wpilib.ADXRS450_Gyro() + self.gyro.calibrate() + + def autonomousInit(self): + self.gyro.reset() + + def autonomousPeriodic(self): + if self.gyro.getAngle() < 90: + self.drivetrain.tankDrive(.5, -.5) + else: + self.drivetrain.tankDrive(0, 0) + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file diff --git a/15 - PID Control/Challenge/README.md b/15 - PID Control/Challenge/README.md new file mode 100644 index 0000000..88632f1 --- /dev/null +++ b/15 - PID Control/Challenge/README.md @@ -0,0 +1,8 @@ +Repeat the previous challenge from section 14 using PID Control + +Combine the user of encoders and gyroscopes to do the following + +- Drive forward 60 inches +- Rotate to 45 degrees (assume a left turn is negative rotation, right turn is positive rotation) +- Drive forward 20 inches +- Rotate to -90 degrees \ No newline at end of file diff --git a/15 - PID Control/Challenge/robot.py b/15 - PID Control/Challenge/robot.py new file mode 100644 index 0000000..14dfa81 --- /dev/null +++ b/15 - PID Control/Challenge/robot.py @@ -0,0 +1,161 @@ +import wpilib +import wpilib.drive +import wpimath.controller +import math +import ctre + +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.PULSES_PER_REVOLUTION = 512 + self.WHEEL_DIAMETER = 6 + self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION + + self.leftDrivetrainEncoder = wpilib.Encoder(0, 1) + self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + self.rightDrivetrainEncoder = wpilib.Encoder(2, 3) + self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + + self.gyro = wpilib.ADXRS450_Gyro() + self.gyro.calibrate() + + self.ENCODER_PID_P = .1 + self.ENCODER_PID_I = 0 + self.ENCODER_PID_D = .01 + self.ENCODER_PID_TOLERANCE = 1 + self.AUTO_ENCODER_SETPOINT = 60 + + self.GYRO_PID_P = .1 + self.GYRO_PID_I = 0 + self.GYRO_PID_D = .01 + self.GYRO_PID_TOLERANCE = 1 + self.AUTO_GYRO_SETPOINT = 90 + + self.leftDrivetrainController = wpimath.controller.PIDController( + self.ENCODER_PID_P, + self.ENCODER_PID_I, + self.ENCODER_PID_D + ) + self.leftDrivetrainController.setTolerance(self.ENCODER_PID_TOLERANCE) + self.leftDrivetrainController.setSetpoint(self.AUTO_ENCODER_SETPOINT) + + self.rightDrivetrainController = wpimath.controller.PIDController( + self.ENCODER_PID_P, + self.ENCODER_PID_I, + self.ENCODER_PID_D + ) + self.rightDrivetrainController.setTolerance(self.ENCODER_PID_TOLERANCE) + self.rightDrivetrainController.setSetpoint(self.AUTO_ENCODER_SETPOINT) + + self.gyroDrivetrainController = wpimath.controller.PIDController( + self.GYRO_PID_P, + self.GYRO_PID_I, + self.GYRO_PID_D + ) + self.gyroDrivetrainController.setTolerance(self.GYRO_PID_TOLERANCE) + self.gyroDrivetrainController.setSetpoint(self.AUTO_GYRO_SETPOINT) + + + def autonomousInit(self): + self.leftDrivetrainEncoder.reset() + self.rightDrivetrainEncoder.reset() + + self.autoState = "DriveForward60" + + def autonomousPeriodic(self): + if self.autoState == "DriveForward60": + if not self.leftDrivetrainController.atSetpoint() and not self.rightDrivetrainController.atSetpoint(): + self.drivetrain.tankDrive( + clamp( + self.leftDrivetrainController.calculate(self.leftDrivetrainEncoder.getDistance(), 60), + -.5, + .5 + ), + clamp( + self.rightDrivetrainController.calculate(self.rightDrivetrainEncoder.getDistance(), 60), + -.5, + .5 + ) + ) + else: + self.drivetrain.tankDrive(0, 0) + self.gyro.reset() + self.autoState = "Rotate45" + elif self.autoState == "Rotate45": + if not self.gyroDrivetrainController.atSetpoint(): + speedValue = clamp( + self.gyroDrivetrainController.calculate(self.gyro.getAngle(), 45), + -.5, + .5 + ) + + self.drivetrain.tankDrive(speedValue, -speedValue) + else: + self.drivetrain.tankDrive(0, 0) + self.leftDrivetrainEncoder.reset() + self.leftDrivetrainController.reset() + self.rightDrivetrainEncoder.reset() + self.rightDrivetrainController.reset() + self.autoState = "DriveForward20" + elif self.autoState == "DriveForward20": + if not self.leftDrivetrainController.atSetpoint() and not self.rightDrivetrainController.atSetpoint(): + self.drivetrain.tankDrive( + clamp( + self.leftDrivetrainController.calculate(self.leftDrivetrainEncoder.getDistance(), 20), + -.5, + .5 + ), + clamp( + self.rightDrivetrainController.calculate(self.rightDrivetrainEncoder.getDistance(), 20), + -.5, + .5 + ) + ) + else: + self.drivetrain.tankDrive(0, 0) + self.gyro.reset() + self.gyroDrivetrainController.reset() + self.autoState = "RotateNegative90" + elif self.autoState == "RotateNegative90": + if not self.gyroDrivetrainController.atSetpoint(): + speedValue = clamp( + self.gyroDrivetrainController.calculate(self.gyro.getAngle(), -90), + -.5, + .5 + ) + + self.drivetrain.tankDrive(speedValue, -speedValue) + else: + self.drivetrain.tankDrive(0, 0) + self.autoState = "End" + else: + self.drivetrain.tankDrive(0, 0) + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file diff --git a/15 - PID Control/README.md b/15 - PID Control/README.md new file mode 100644 index 0000000..e69de29 diff --git a/15 - PID Control/robot.py b/15 - PID Control/robot.py new file mode 100644 index 0000000..1af0795 --- /dev/null +++ b/15 - PID Control/robot.py @@ -0,0 +1,126 @@ +import wpilib +import wpilib.drive +import wpimath.controller +import math +import ctre + +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.PULSES_PER_REVOLUTION = 512 + self.WHEEL_DIAMETER = 6 + self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION + + self.leftDrivetrainEncoder = wpilib.Encoder(0, 1) + self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + self.rightDrivetrainEncoder = wpilib.Encoder(2, 3) + self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE) + + self.gyro = wpilib.ADXRS450_Gyro() + self.gyro.calibrate() + + self.ENCODER_PID_P = .1 + self.ENCODER_PID_I = 0 + self.ENCODER_PID_D = .01 + self.ENCODER_PID_TOLERANCE = 1 + self.AUTO_ENCODER_SETPOINT = 60 + + self.GYRO_PID_P = .1 + self.GYRO_PID_I = 0 + self.GYRO_PID_D = .01 + self.GYRO_PID_TOLERANCE = 1 + self.AUTO_GYRO_SETPOINT = 90 + + self.leftDrivetrainController = wpimath.controller.PIDController( + self.ENCODER_PID_P, + self.ENCODER_PID_I, + self.ENCODER_PID_D + ) + self.leftDrivetrainController.setTolerance(self.ENCODER_PID_TOLERANCE) + self.leftDrivetrainController.setSetpoint(self.AUTO_ENCODER_SETPOINT) + + self.rightDrivetrainController = wpimath.controller.PIDController( + self.ENCODER_PID_P, + self.ENCODER_PID_I, + self.ENCODER_PID_D + ) + self.rightDrivetrainController.setTolerance(self.ENCODER_PID_TOLERANCE) + self.rightDrivetrainController.setSetpoint(self.AUTO_ENCODER_SETPOINT) + + self.gyroDrivetrainController = wpimath.controller.PIDController( + self.GYRO_PID_P, + self.GYRO_PID_I, + self.GYRO_PID_D + ) + self.gyroDrivetrainController.setTolerance(self.GYRO_PID_TOLERANCE) + self.gyroDrivetrainController.setSetpoint(self.AUTO_GYRO_SETPOINT) + + + def autonomousInit(self): + self.leftDrivetrainEncoder.reset() + self.rightDrivetrainEncoder.reset() + + self.autoState = "Encoder Step" + + def autonomousPeriodic(self): + if self.autoState == "Encoder Step": + if not self.leftDrivetrainController.atSetpoint() and not self.rightDrivetrainController.atSetpoint(): + self.drivetrain.tankDrive( + clamp( + self.leftDrivetrainController.calculate(self.leftDrivetrainEncoder.getDistance()), + -.5, + .5 + ), + clamp( + self.rightDrivetrainController.calculate(self.rightDrivetrainEncoder.getDistance()), + -.5, + .5 + ) + ) + else: + self.drivetrain.tankDrive(0, 0) + self.gyro.reset() + self.autoState = "Gyro Step" + elif self.autoState == "Gyro Step": + if not self.gyroDrivetrainController.atSetpoint(): + speedValue = clamp( + self.gyroDrivetrainController.calculate(self.gyro.getAngle()), + -.5, + .5 + ) + + self.drivetrain(speedValue, -speedValue) + else: + self.drivetrain.tankDrive(0, 0) + self.autoState = "End Step" + else: + self.drivetrain.tankDrive(0, 0) + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file diff --git a/16 - NetworkTables and Limelight/Limelight.py b/16 - NetworkTables and Limelight/Limelight.py new file mode 100644 index 0000000..023d3d7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/16 - NetworkTables and Limelight/README.md b/16 - NetworkTables and Limelight/README.md new file mode 100644 index 0000000..e69de29 diff --git a/16 - NetworkTables and Limelight/robot.py b/16 - NetworkTables and Limelight/robot.py new file mode 100644 index 0000000..7956cba --- /dev/null +++ b/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) \ No newline at end of file diff --git a/17 - Pneumatics/README.md b/17 - Pneumatics/README.md new file mode 100644 index 0000000..e69de29 diff --git a/17 - Pneumatics/robot.py b/17 - Pneumatics/robot.py new file mode 100644 index 0000000..aef9513 --- /dev/null +++ b/17 - Pneumatics/robot.py @@ -0,0 +1,51 @@ +import wpilib +import wpilib.drive +import ctre + +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.compressor = wpilib.Compressor(0, wpilib.PneumaticsModuleType.REVPH) + # self.compressor.enableDigital() + self.compressor.enableAnalog(75, 115) + + self.singleActionMechanism = wpilib.Solenoid(0, wpilib.PneumaticsModuleType.REVPH, 0) + self.doubleActionMechanism = wpilib.DoubleSolenoid(0, wpilib.PneumaticsModuleType.REVPH, 1, 2) + + def autonomousInit(self): + pass + + def autonomousPeriodic(self): + pass + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + + if self.primary.getAButtonPressed(): + self.singleActionMechanism.set(not self.singleActionMechanism.get()) + + if self.primary.getBButtonPressed(): + if self.doubleActionMechanism.get() == wpilib.DoubleSolenoid.Value.kForward: + self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kReverse) + elif self.doubleActionMechanism.get() == wpilib.DoubleSolenoid.Value.kReverse: + self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kForward) + else: + self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kReverse) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file diff --git a/18 - CameraServer and Shuffleboard/Challenge/README.md b/18 - CameraServer and Shuffleboard/Challenge/README.md new file mode 100644 index 0000000..e69de29 diff --git a/18 - CameraServer and Shuffleboard/Challenge/robot.py b/18 - CameraServer and Shuffleboard/Challenge/robot.py new file mode 100644 index 0000000..e69de29 diff --git a/18 - CameraServer and Shuffleboard/README.md b/18 - CameraServer and Shuffleboard/README.md new file mode 100644 index 0000000..e69de29 diff --git a/18 - CameraServer and Shuffleboard/robot.py b/18 - CameraServer and Shuffleboard/robot.py new file mode 100644 index 0000000..d98cc3d --- /dev/null +++ b/18 - CameraServer and Shuffleboard/robot.py @@ -0,0 +1,38 @@ +import wpilib +import wpilib.drive +import wpilib.cameraserver +import ctre + +# THIS EXAMPLE IS INCOMPLETE BECAUSE CSCORE SUPPORT ON WINDOWS IS +# NOT WELL TESTED AND IS VERY DIFFICULT TO GET WORKING, PLEASE IGNORE THIS EXAMPLE +# FOR THE TIME BEING +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) + + + def autonomousInit(self): + pass + + def autonomousPeriodic(self): + pass + + def teleopInit(self): + pass + + def teleopPeriodic(self): + self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX()) + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file