伺服控制xbox树莓派

伺服控制xbox树莓派

#!/usr/bin/env python

import pigpio
import math
import xbox

GPIO_SERVO_PIN  = 20

MIN_ANG=-180.0 #degrees
MAX_ANG=180.0  #degrees

MIN_PW=1000 # microseconds
MAX_PW=2000 # microseconds

ANG_RANGE=MAX_ANG-MIN_ANG
PW_RANGE=MAX_PW-MIN_PW
PWAR=float(PW_RANGE)/ANG_RANGE

RAD2DEG=180.0/math.pi

def angleToPulseWidth(angle):
   """
   angle is mapped to valid pulse widths for servo
   which are determined by experiment.
   """
   assert MIN_ANG <= angle <= MAX_ANG
   return MIN_PW + ((angle - MIN_ANG) * PWAR)


if __name__ == '__main__':

   import pigpio
   import time

   pi = pigpio.pi()
   if not pi.connected:
      exit()

   joy = xbox.Joystick()

   while not joy.Back():
      x, y = joy.leftStick()
      angle = math.atan2(y, x) * RAD2DEG
      pw = angleToPulseWidth(angle)
      pi.set_servo_pulsewidth(GPIO_SERVO_PIN, pw)
      time.sleep(0.01)

   joy.close()
   pi.stop()