#!/usr/bin/python
#
# Tests the connectivity of the Wii nunchuck over I2C
#
import smbus
import time
from Nunchuck import Nunchuck
nun = Nunchuck()
while True:
nun.update()
print 'Jx: %s Jy: %s Ax: %s Ay: %s Az: %s Bc: %s Bz: %s' % (nun.joy_x, nun.joy_y, nun.accel_x, nun.accel_y, nun.accel_z, nun.button_c, nun.button_z)
pos += 1
# get mid positions and move arms to those locations
midPositions = []
print '[#, min, max] mid'
for i in range(0,6):
midPositions.append((servoTable[i][2]-servoTable[i][1])/2+servoTable[i][1])
print servoTable[i], midPositions[i]
#pwm.setPWM(i,0,midPositions[i])
currentPositions = [445,385,465,220,395,270]
updatePos(currentPositions)
nunchuck = Nunchuck()
mode = 1
while (1):
nunchuck.update()
#change mode if button c pressed
if (nunchuck.button_c==True):
if (mode==1):
mode = 2
else:
mode =1
print mode
time.sleep(1)
#mode 1
if (mode==1 and nunchuck.button_z==False):
if (nunchuck.joy_x < 60):
currentPositions[0] -= 20
if (nunchuck.joy_x > 170):
currentPositions[0] += 20
if (nunchuck.joy_y < 60):