import RPi.GPIO as GPIO
import cwiid
import time 

ESC = 2 # GPIO number of servo control
PERIOD = 20 # Time in ms for one period
STOP = 5 # 5 is absolute min
MAX = 9.7 # 10 is absolute max
INC = 0.1
BUTTON_UP = 2048
BUTTON_DOWN = 1024
BUTTON_B = 4
BUTTON_HOME = 128

#
# Init GPIO
#
GPIO.setmode(GPIO.BCM)
GPIO.setup(ESC, GPIO.OUT)
GPIO.setup(14,GPIO.OUT)
# Init Wii Mote and loop till connected
#
while True:
   try:
      wm = cwiid.Wiimote()
      break
   except:
      pass
wm.rpt_mode = cwiid.RPT_BTN # Enable button data reporting
wm.led=13
#
# Setup ESC limits
#
motor = GPIO.PWM(ESC, 1000 / PERIOD)
motor.start(10)
motor.ChangeDutyCycle(5)
#
# Userloop

dc = STOP
cycling = True
try:
    while cycling:
        motor.ChangeDutyCycle(dc)
        res = wm.state['buttons']
	GPIO.output(14,0)
        #print res, dc
        if res & (BUTTON_UP | BUTTON_B) == BUTTON_UP+BUTTON_B: 
            dc = dc + INC
        if res & (BUTTON_DOWN | BUTTON_B) == BUTTON_DOWN+BUTTON_B:
            dc = dc - INC
	    GPIO.output(14,1)
        if res & BUTTON_B == 0: #B = dead switch is be active or not
            dc = STOP
	    GPIO.output(14,1)
	    motor.ChangeDutyCycle(dc)
	    time.sleep(2)
	    GPIO.output(14,0)
        if res == BUTTON_HOME:
            cycling = False
        if dc > MAX:
            dc = MAX
        if dc < STOP:
            dc = STOP
        time.sleep(0.1)
finally:
    # shut down cleanly
    motor.stop()
    # print ("dc var setting is: ")
    # print (dc)

#
# Shutdown/cleanup
#
motor.stop()
GPIO.cleanup()