82 lines
2.2 KiB
Python
82 lines
2.2 KiB
Python
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) |