def main():
brick.sound.beep()
joystickEvent = detectJoystick(['Controller'])
if joystickEvent:
robot = Robot(Motor(Port.D), Motor(Port.A), Motor(Port.B), 55, 200)
t = threading.Thread(target=autoStopLoop, args=(robot, ))
t.start()
def onButtonPressed(code):
if code == BUTTON_X:
robot.inactive()
return False
if code == BUTTON_A:
robot.fire()
return True
def onLeftJoyChanged(x, y):
robot.drive(x, y)
def onRightJoyChanged(x, y):
robot.target(x)
joystick = JoyStick(joystickEvent)
joystick.setButtonHandler(onButtonPressed)
joystick.setJoyLeftHandler(onLeftJoyChanged)
joystick.setJoyRightHandler(onRightJoyChanged)
joystick.startLoop()
else:
brick.sound.beep()
def __init__(self, master):
# create GUI
self.gpio = pigpio.pi()
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root, text='Joystick')
label.pack()
self.gui = GuiRarm(master=root)
self.gui.createCordinateLabelServo()
self.gui.createButton(btn_name='Finish', callback=self.cleanup)
#self.gui.createCircleJs2(200,200,220,220,'blue')
#JoyStick instanse
self.jsR = JoyStick(-1, 4, 27)
self.val_horz = 0
#self.jsL = JoyStick(7,4,22)
self.jsL = JoyStick(2, -1, 22)
self.val_vert = 0
#Volume instance
self.vol = Volume(6)
self.val_vol = 0
self.vol_sum = 0
self.sampleCount = 0
#ServMotor
#self.servo= ServMotor(18,180,0)
self.servo = I2cServo()
#PWM Freq: 60Hz
self.servo.setPWMFreq(60)
# Led instance
self.Led = Led(4)
self.Led.setLedOn()
# PowrSw
self.PowSw = Sw(17, pud='pudup')
self.PowSwCont = 0
def __init__(self, master):
# create GUI
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root, text='Joystick')
label.pack()
self.gui = GuiJs(master=root)
self.gui.createCordinateLabelJs1()
self.gui.createCordinateLabelJs2()
self.gui.createCircleJs1(200, 200, 220, 220, 'red')
self.gui.createButton(btn_name='Finish', callback=self.cleanup)
self.gui.createCircleJs2(200, 200, 220, 220, 'blue')
#JoyStick instanse
self.js1 = JoyStick(0, 3, 27)
self.val_vert1 = 0
self.val_horz1 = 0
self.move_vert1 = 0
self.move_horz1 = 0
self.js2 = JoyStick(7, 4, 22)
self.val_vert2 = 0
self.val_horz2 = 0
self.move_vert2 = 0
self.move_horz2 = 0
#ServMotor
#self.servo= ServMotor(18,180,0)
#self.servo = I2cServo()
# Led instance
self.Led = Led(17)
class App:
def __init__(self,xp,yp,pp,ld):
self.displayDelta = 200 # ms
self.led = ld
self.led.off()
try:
self.js = JoyStick(xp,yp,pp, self.onButton)
print('Starting up...')
except Exception as e:
print(e)
print('JoyStick creation failed!')
def onButton(self):
self.led.toggle()
print('\nPush Button!\n')
def loop(self):
lastDisplayTime = time.ticks_ms()
while True:
self.js.checkPB()
now = time.ticks_ms()
if now - lastDisplayTime > self.displayDelta:
lastDisplayTime = now
line = 'X: ' + str(self.js.readX()) + ' Y: ' + str(self.js.readY())
print(line)
def __init__(self,xp,yp,pp,ld):
self.displayDelta = 200 # ms
self.led = ld
self.led.off()
try:
self.js = JoyStick(xp,yp,pp, self.onButton)
print('Starting up...')
except Exception as e:
print(e)
print('JoyStick creation failed!')
def __init__(self,master):
# create GUI
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root,text='Joystick')
label.pack()
self.gui = GuiJs(master=root)
self.gui.createCordinateLabel()
self.gui.createCircle(200,200,220,220,'red')
self.gui.createButton(btn_name='Finish',callback=self.cleanup)
#JoyStick instanse
self.js = JoyStick()
self.val_vert = 0
self.val_horz = 0
self.move_vert = 0
self.move_horz = 0
#ServMotor
self.sm= ServMotor(18,180,0)
# Led instance
self.Led = Led(17)
self.Sw = Sw(4,'either',self.callback_Sw)
# draw.line((center, (center[0]+coordinates[0]*radius/rescale,center[1]-coordinates[1]*radius/rescale)), fill=255)
# print(coordinates)
circleCenter = (center[0] + coordinates[0] * radius / 2,
center[1] - coordinates[1] * radius / 2)
smallRadius = radius / 3.0
draw.ellipse(
(circleCenter[0] - smallRadius, circleCenter[1] - smallRadius,
circleCenter[0] + smallRadius, circleCenter[1] + smallRadius),
outline=255)
#draw.ellipse((center[0]+coordinates[0]*radius/rescale,center[1]-coordinates[1]*radius/rescale)), fill=255)
screen = Screen()
servos = [Servo(channel) for channel in [0, 1, 2, 3]]
joyStick1 = JoyStick(1, JoyStick.Address_Ox48, [1, 0], 4)
joyStick2 = JoyStick(1, JoyStick.Address_Ox48, [3, 2], 17)
robot = Robot()
for servo in servos:
servo.set_angle(90)
def applyPosition(servo, joyStick, axis, multiplier=1):
position = joyStick.last_position[axis]
print(position)
threshhold = 0.11
if position > threshhold or position < -threshhold:
servo.go_to_angle(servo.angle + robot.speed * multiplier * position)
return True
def testUpAdds10(self): p = JoyStick() self.failUnless(p.moveUp(10)=="PR ,1110")
def testAbs(self): p = JoyStick() self.failUnless(p.moveAbs(10,10)=="PA 28440,1110")
def testRightAdds10(self): p = JoyStick() self.failUnless(p.moveRight(10)=="PR 28440,")
def testLeftAdds10(self): p = JoyStick() self.failUnless(p.moveLeft(10)=="PR -28440,")
def testDownAdds10(self): p = JoyStick() self.failUnless(p.moveDown(10)=="PR ,-1110")
class JoyStickTest(tk.Frame):
def __init__(self, master):
# create GUI
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root, text='Joystick')
label.pack()
self.gui = GuiJs(master=root)
self.gui.createCordinateLabelJs1()
self.gui.createCordinateLabelJs2()
self.gui.createCircleJs1(200, 200, 220, 220, 'red')
self.gui.createButton(btn_name='Finish', callback=self.cleanup)
self.gui.createCircleJs2(200, 200, 220, 220, 'blue')
#JoyStick instanse
self.js1 = JoyStick(0, 3, 27)
self.val_vert1 = 0
self.val_horz1 = 0
self.move_vert1 = 0
self.move_horz1 = 0
self.js2 = JoyStick(7, 4, 22)
self.val_vert2 = 0
self.val_horz2 = 0
self.move_vert2 = 0
self.move_horz2 = 0
#ServMotor
#self.servo= ServMotor(18,180,0)
#self.servo = I2cServo()
# Led instance
self.Led = Led(17)
#display GUI
def cleanup(self):
print 'Test Finished'
self.js1.cleanup()
self.js2.cleanup()
#self.servo.cleanup()
self.Led.cleanup()
sys.exit()
# Flashing LED Test No.1
def setLed(self):
if (self.js1.getSelState()== 'Released') and \
(self.js2.getSelState()== 'Released'):
if self.Led.getState() != 'Off': self.Led.setLedOff()
else:
if self.Led.getState() != 'On': self.Led.setLedOn()
def test(self):
self.val_vert1 = self.js1.getValVert()
self.val_horz1 = self.js1.getValHorz()
self.move_vert1 = self.move_vert(self.val_vert1)
self.move_horz1 = self.move_horz(self.val_horz1)
#print ('Js1'+'vert:%d'% self.val_vert1+'hortz:%d'% self.val_horz1+'sel:%d' % self.val_sel1)
#print (self.val_vert1,self.val_horz1,self.move_vert1,self.move_horz1)
self.val_vert2 = self.js2.getValVert()
self.val_horz2 = self.js2.getValHorz()
self.move_vert2 = self.move_vert(self.val_vert2)
self.move_horz2 = self.move_horz(self.val_horz2)
#print ('Js2'+'vert:%d'% self.val_vert2+'hortz:%d'% self.val_horz2+'sel:%d' % self.val_sel2)
# if self.move_vert1 == 0 and self.move_horz1 == 0:
# self.Led.setLedOff()
# else:self.Led.setLedOn()
self.gui.renewCordinateJs1(self.val_vert1, self.val_horz1)
self.gui.renewCordinateJs2(self.val_vert2, self.val_horz2)
self.gui.moveCircle1(self.move_horz1, self.move_vert1)
self.gui.moveCircle2(self.move_horz2, self.move_vert2)
#self.movePwmServo()
#self.moveI2cServo()
self.setLed()
# check Adc(vert,Horz) and Sw per 50ms
self.master.after(30, self.test)
def move_vert(self, val):
move_val = 0
if val >= 2: move_val = -10
elif val <= 0: move_val = 10
else: move_val = 0
return move_val
def move_horz(self, val):
move_val = 0
if val >= 2: move_val = -10
elif val <= 0: move_val = 10
else: move_val = 0
return move_val
def moveI2cServo(self):
self.servo.setDuty(self.servo._I2CSERVO_HORZ_0, self.val_horz1, 0,
1024)
self.servo.setDuty(self.servo._I2CSERVO_VERT_0, self.val_vert2, 0,
1024)
def movePwmServo(self):
deg = self.sm.getDeg()
if self.val_horz1 > 100:
if deg < 180:
deg += 10
self.sm.setDeg(float(deg))
elif self.val_horz1 < -200:
if deg >= 10:
deg -= 10
self.sm.setDeg(float(deg))
class JoyStickTest(tk.Frame):
def __init__(self,master):
# create GUI
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root,text='Joystick')
label.pack()
self.gui = GuiJs(master=root)
self.gui.createCordinateLabel()
self.gui.createCircle(200,200,220,220,'red')
self.gui.createButton(btn_name='Finish',callback=self.cleanup)
#JoyStick instanse
self.js = JoyStick()
self.val_vert = 0
self.val_horz = 0
self.move_vert = 0
self.move_horz = 0
#ServMotor
self.sm= ServMotor(18,180,0)
# Led instance
self.Led = Led(17)
self.Sw = Sw(4,'either',self.callback_Sw)
#display GUI
def cleanup(self):
print 'Test Finished'
self.js.cleanup()
self.sm.cleanup()
self.Led.cleanup()
sys.exit()
# Flashing LED Test No.1
def callback_Sw(self):
if self.Sw.getState()== 'Released':
if self.Led.getState() != 'Off':self.Led.setLedOff()
else:
if self.Led.getState() != 'On':self.Led.setLedOn()
def test(self):
self.val_vert = self.js.getValVert()
self.val_horz = self.js.getValHorz()
self.move_vert = self.movement(self.val_vert)
self.move_horz = self.movement(self.val_horz)
#:print (self.val_vert,self.val_horz,self.move_vert,self.move_horz)
if self.move_vert == 0 and self.move_horz == 0:
self.Led.setLedOff()
else:self.Led.setLedOn()
self.gui.renewCordinate(self.val_vert,self.val_horz)
self.gui.moveCircle(self.move_vert,self.move_horz)
self.moveServmoter()
self.master.after(50,self.test)
def movement(self,val):
move_val = 0
if val > 255:move_val = -10
elif val > 63:move_val = -5
elif val <= -512:move_val = 0
elif val < -255:move_val = 10
elif val < -63: move_val = 5
return move_val
def moveServmoter(self):
deg = self.sm.getDeg()
if self.val_horz > 100:
if deg < 180:
deg += 10
print deg
self.sm.setDeg(float(deg))
elif self.val_horz < -200:
if deg >= 10:
deg -= 10
print deg
self.sm.setDeg(float(deg))
class RobotArmTest(tk.Frame):
def __init__(self, master):
# create GUI
self.gpio = pigpio.pi()
self.master = master
self.master.title('JoyStick Test')
label = tk.Label(root, text='Joystick')
label.pack()
self.gui = GuiRarm(master=root)
self.gui.createCordinateLabelServo()
self.gui.createButton(btn_name='Finish', callback=self.cleanup)
#self.gui.createCircleJs2(200,200,220,220,'blue')
#JoyStick instanse
self.jsR = JoyStick(-1, 4, 27)
self.val_horz = 0
#self.jsL = JoyStick(7,4,22)
self.jsL = JoyStick(2, -1, 22)
self.val_vert = 0
#Volume instance
self.vol = Volume(6)
self.val_vol = 0
self.vol_sum = 0
self.sampleCount = 0
#ServMotor
#self.servo= ServMotor(18,180,0)
self.servo = I2cServo()
#PWM Freq: 60Hz
self.servo.setPWMFreq(60)
# Led instance
self.Led = Led(4)
self.Led.setLedOn()
# PowrSw
self.PowSw = Sw(17, pud='pudup')
self.PowSwCont = 0
#display GUI
def shutdown(self):
self.Led.setLedOff()
self.jsR.cleanup()
self.jsL.cleanup()
self.vol.cleanup()
self.servo.cleanup()
self.Led.cleanup()
# release pigpio resources
self.gpio.stop()
os.system("sudo shutdown -h now")
def cleanup(self):
print('Test Finished')
self.Led.setLedOff()
self.jsR.cleanup()
self.jsL.cleanup()
self.vol.cleanup()
self.servo.cleanup()
self.Led.cleanup()
# release pigpio resources
self.gpio.stop()
sys.exit()
# Flashing LED Test No.1
def test(self):
if self.PowSw.getState() == 'Pressed':
self.PowSwCont += 1
if self.PowSwCont == 10:
print 'Pressed'
print 'Shutdown!'
self.shutdown()
else:
self.PowSwCont = 0
self.val_horz = self.jsR.getValHorz()
self.val_vol = self.vol.getValVol() / 4
self.val_vert = self.jsL.getValVert()
self.moveI2cServoHorz()
self.moveI2cServoVert()
self.moveI2cServoVol()
self.gui.renewCordinateServo(self.duty_horz, self.duty_vert,
self.val_vol)
#call self.test() per 6msec
self.master.after(6, self.test)
def moveI2cServoHorz(self):
self.duty_horz = self.servo.getDuty(self.servo._I2CSERVO_HORZ_0)
# print ('horzduty :%d' %duty)
# turn to -90
if self.val_horz >= 2:
self.duty_horz += 5
# turn to 90
elif self.val_horz <= 0:
self.duty_horz -= 5
# dont turn
else:
return
self.servo.setDuty(self.servo._I2CSERVO_HORZ_0, self.duty_horz)
def moveI2cServoVert(self):
self.duty_vert = self.servo.getDuty(self.servo._I2CSERVO_VERT_0)
# print ('vertduty :%d' %duty)
# turn to -90
if self.val_vert >= 2:
self.duty_vert -= 5
# turn to 90
elif self.val_vert <= 0:
self.duty_vert += 5
# dont turn
else:
return
self.servo.setDuty(self.servo._I2CSERVO_VERT_0, self.duty_vert)
def moveI2cServoVol(self):
if self.sampleCount < 5:
self.vol_sum += self.val_vol
self.sampleCount += 1
else:
val = self.vol_sum / 5
self.servo.setAdValtoDuty(self.servo._I2CSERVO_VOL_0, val, 0, 255)
self.sampleCount = 0
self.vol_sum = 0
from __future__ import division
from __future__ import print_function
import sys
sys.path.append('..')
from joystick import JoyStick
from servo import ServoController
if __name__ == '__main__':
sc = ServoController(angle_range=80)
js = JoyStick()
while True:
# read serial
v = js.read()
# convert to servo angle
angle = sc.analog2deg(v)
print(angle)
for i in range(len(angle)):
sc.control([i, angle[i]])
