def blueConnection():
while True:
blueMsg = blueConn.readline()
if len(blueMsg) > 0:
screenMessage.printMsg(blueMsg)
if blueMsg == "up":
pz.forward(speed)
sleep(0.5)
pz.stop()
#break
elif blueMsg == "down":
pz.reverse(speed)
sleep(0.5)
pz.stop
elif blueMsg == "right":
pz.spinRight(speed)
sleep(0.5)
pz.stop()
elif blueMsg == "left":
pz.spinLeft(speed)
sleep(0.5)
pz.stop()
else:
pz.stop()
sleep(0.1)
def ControleRobot(action):
global ANGLE
speed = 85
if action == "avance":
pz.reverse(speed)
elif action == "recule":
pz.forward(speed)
time.sleep(0.1)
elif action == "droite":
pz.stop()
time.sleep(1)
pz.spinLeft(95)
time.sleep(ANGLE["droite"])
elif action == "gauche":
time.sleep(0.1)
pz.stop()
time.sleep(1)
pz.spinRight(95)
time.sleep(ANGLE["gauche"])
pz.reverse(speed)
time.sleep(0.3)
elif action == "arrete":
pz.stop()
else:
logger.warning('Une erreur dans les commandes de controle du robot s est produite !')
def reverse(self):
""" Move both motors backwards """
global motor_speed
global motor_direction
with self._lock:
pz.reverse(motor_speed)
motor_direction = 'Reverse'
return motor_direction
def moveleftturn():
global pz,speed
try:
pz.reverse(speed)
time.sleep(0.05)
pz.spinLeft(speed)
time.sleep(0.05)
pz.reverse(speed)
time.sleep(0.05)
pz.spinLeft(speed)
time.sleep(0.1)
pz.stop()
except KeyboardInterrupt:
print "quit"
def linefollow():
print ("Line Following Program Active")
mylcd.lcd_display_string("Line Follow", 1)
mylcd.lcd_display_string("Select Ends", 2)
RUN = 1
while RUN == 1:
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif RIGHTLINE == BACKGROUND and LEFTLINE == BACKGROUND:
pz.forward(100)
elif RIGHTLINE == LINE and LEFTLINE == BACKGROUND:
pz.spinRight(40)
elif RIGHTLINE == BACKGROUND and LEFTLINE == LINE:
pz.spinLeft(40)
elif RIGHTLINE == LINE and LEFTLINE == LINE:
pz.reverse(70)
def remotecontrolbasic():
print ("Remote Control Program Active")
mylcd.lcd_display_string("Remote Control B", 1)
mylcd.lcd_display_string("Select Ends", 2)
time.sleep(2)
RUN = 1
while RUN == 1:
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif buttons_pressed & 1 << SixAxis.BUTTON_D_UP:
pz.forward(100)
elif buttons_pressed & 1 << SixAxis.BUTTON_D_DOWN:
pz.reverse(100)
elif buttons_pressed & 1 << SixAxis.BUTTON_D_RIGHT:
pz.spinRight(100)
elif buttons_pressed & 1 << SixAxis.BUTTON_D_LEFT:
pz.spinLeft(100)
def autospeed():
print("Autospeed Program Active")
mylcd.lcd_display_string("Autospeed", 1)
mylcd.lcd_display_string("Select Ends", 2)
RUN = 1
while RUN == 1:
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif RIGHTIR == 0 and LEFTIR == 0:
pz.forward(100)
elif RIGHTIR == 1 and LEFTIR == 0:
pz.reverse(40)
time.sleep(1)
pz.spinLeft(40)
time.sleep(0.3)
elif RIGHTIR == 0 and LEFTIR == 1:
pz.reverse(40)
time.sleep(1)
pz.spinRight(40)
time.sleep(0.3)
def courseremotecontrol():
SPEEDFR = 60
SPEEDT = 100
mylcd.lcd_display_string("Remote Control C", 1)
mylcd.lcd_display_string("Select Ends ", 2)
time.sleep(2)
RUN = 1
while RUN == 1:
pygame.event.pump()
if pad.get_button(0) == 1: # exit program
RUN = 0
elif pad.get_button(4) == 1:
pz.forward(SPEEDFR)
elif pad.get_button(6) == 1:
pz.reverse(SPEEDFR)
elif pad.get_button(5) == 1:
pz.spinRight(SPEEDT)
elif pad.get_button(7) == 1:
pz.spinLeft(SPEEDT)
else:
pz.stop()
time.sleep(0.1)
def update_robot(state=None):
if state == "Forward" :
pz.forward(100)
elif state == "Reverse" :
pz.reverse(100)
elif state == "Left" :
pz.spinLeft(50)
elif state == "Right" :
pz.spinRight(50)
elif state == "Stop" :
pz.stop()
elif state == "Up":
up()
elif state == "Down":
down()
elif state == "Light":
toggleLight()
elif state == "Center":
center()
else :
print (" no command ")
return('',204)
def linefollower(): #works perfectly on a new set of batteries
mylcd.lcd_display_string("Line Follower ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
PREP = 1
GO = 1
LFSPEED = 75 #only changes cornering speed
pz.stop()
while PREP == 1: #setup ready for line following
for event in get_key():
mylcd.lcd_display_string("Press G to GO ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_G":
PREP = 0
elif event.code == "KEY_E":
GO = 0
PREP = 0
while GO == 1: #new simple quit - works perfectly
if button.is_pressed:
pz.stop()
GO = 0
else:
mylcd.lcd_display_string("GO!!!!!!!!!!! ", 1)
mylcd.lcd_display_string("Switch = stop ", 2)
LEFTLINE = pz.readInput(2) #assign right line sensor to a variable
RIGHTLINE = pz.readInput(3) #assign left line sensor to a variable
if RIGHTLINE == 1:
pz.spinRight(LFSPEED)
#time.sleep(0.5)
elif LEFTLINE == 1:
pz.spinLeft(LFSPEED)
#time.sleep(0.5)
elif LEFTLINE == 0 and RIGHTLINE == 0:
pz.forward(15)
#time.sleep(0.5)
elif LEFTLINE == 1 and RIGHTLINE == 1:
pz.reverse(15)
def remotecontrols():
SPEEDFR = 50
SPEEDT = 45
mylcd.lcd_display_string("Remote Control S", 1)
mylcd.lcd_display_string("Press 1 to End ", 2)
time.sleep(2)
RUN = 1
while RUN == 1:
pygame.event.pump()
keys = pygame.key.get_pressed()
if keys[K_ESC]: # exit program
RUN = 0
if keys[K_UP]:
pz.forward(SPEEDFR)
if keys[K_DOWN]:
pz.reverse(SPEEDFR)
if keys[K_RIGHT]:
pz.spinRight(SPEEDT)
if keys[K_LEFT]:
pz.spinLeft(SPEEDT)
if keys[K_SPACE]:
pz.stop()
def moveBackward(self, newSpeed):
if newSpeed >= 0 and newSpeed <= 1:
self.setSpeed(self.DIRECTION_BACKWARD, self.DIRECTION_BACKWARD)
pz.reverse(speed)
def remotecontrol(): #works perfect
pz.stop()
mylcd.lcd_display_string("Remote Control ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
time.sleep(2)
speed = 100
turnspeed = 100
mylcd.lcd_display_string("Speed = %d " % speed, 1)
mylcd.lcd_display_string("Press E to End ", 2)
GO = 1
BURST = 0.2
while GO == 1:
for event in get_key():
if event.code == "KEY_UP":
if event.state == 1:
pz.forward(speed)
time.sleep(BURST)
pz.stop()
elif event.state == 2:
pz.forward(speed)
elif event.state == 0:
pz.stop()
if event.code == "KEY_DOWN":
if event.state == 1:
pz.reverse(speed)
time.sleep(BURST)
pz.stop()
elif event.state == 2:
pz.reverse(speed)
elif event.state == 0:
pz.stop()
if event.code == "KEY_RIGHT":
if event.state == 1:
pz.spinRight(speed)
time.sleep(BURST)
pz.stop()
elif event.state == 2:
pz.spinRight(speed)
elif event.state == 0:
pz.stop()
if event.code == "KEY_LEFT":
if event.state == 1:
pz.spinLeft(speed)
time.sleep(BURST)
pz.stop()
elif event.state == 2:
pz.spinLeft(speed)
elif event.state == 0:
pz.stop()
if event.code == "KEY_DOT" or event.code == "KEY_>":
if event.state == 1 or event.state == 2:
speed = min(100, speed + 10)
turnspeed = min(100, turnspeed + 10)
mylcd.lcd_display_string("Speed = %d " % speed, 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_COMMA" or event.code == "KEY_<":
if event.state == 1 or event.state == 2:
speed = max(0, speed - 10)
turnspeed = max(0, turnspeed - 10)
mylcd.lcd_display_string("Speed = %d " % speed, 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_Q":
if event.state == 1 or event.state == 2:
speed = 75
turnspeed = 100
mylcd.lcd_display_string("Fast Maze ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_W":
if event.state == 1 or event.state == 2:
speed = 60
turnspeed = 80
mylcd.lcd_display_string("slow Maze ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
if event.code == "KEY_SPACE":
if event.state == 1 or event.state == 2:
pz.stop()
if event.code == "KEY_E":
if event.state == 1 or event.state == 2:
pz.stop()
GO = 0
import time
pz.setInputConfig(2, 0)
pz.init()
try:
while True:
if pz.readInput(2):
print "IR droite : Black"
pz.stop()
time.sleep(1)
break
else:
print "IR droite : White"
time.sleep(1)
pz.reverse(35)
if pz.readInput(3):
print "IR gauche : Black"
pz.stop()
time.sleep(1)
break
else:
print "IR gauche : White"
pz.reverse(35)
time.sleep(1)
except KeyboardInterrupt:
print
finally:
pz.cleanup()
import piconzero as pz
import hcsr04
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
speed = 100
i = 0
try:
while True:
if ((pz.readInput(2) != 1) and (pz.readInput(3) != 1) and i % 2 == 0):
pz.stop()
time.sleep(0.1)
pz.spinLeft(100)
time.sleep(0.2)
elif ((pz.readInput(2) != 1) and (pz.readInput(3) != 1)
and i % 2 == 1):
pz.stop()
time.sleep(0.1)
pz.spinRight(100)
time.sleep(0.2)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) == 1)):
i += 1
else:
pz.reverse(70)
except KeyboardInterrupt:
print "Au revoir"
finally:
pz.cleanup()
cam.hflip = True
cam.start_recording("/home/pi/qmpiwars/videos/maze-" + ts + ".h264")
try:
while True:
distance = int(dist.getDistance())
LeftDist = sideDist.DistSensor(27, 22) #Left
RightDist = sideDist.DistSensor(17, 18) #Right
print "----------------------------------"
print "Front Distance:", distance
print "Left Distance:", LeftDist
print "Right Distance:", RightDist
print "----------------------------------"
if (distance <= frontStop): # too close - Reverse
print "reverse"
pz.reverse(reverseSpeed)
sleep(reverseTime)
sideDist.neoPixelLight("backward")
elif (LeftDist >= leftMax): # heading right - turn left
print "turning left"
pz.spinRight(spinSpeed)
sleep(spinTime)
sideDist.neoPixelLight("left")
if (LeftDist > leftMax): # still too far move forward a bit more
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
elif (LeftDist < leftMax - 7): # turn right
print "turning right"
pz.spinLeft(spinSpeed)
sleep(spinTime)
pz.setOutput (pan, panVal)
pz.setOutput (tilt, tiltVal)
elif keyp == '8' or keyp == 'i' or ord(keyp) == 17:
panVal = fwdPan
tiltVal = fwdTilt
pz.setOutput (pan, panVal)
pz.setOutput (tilt, tiltVal)
pz.stop()
# get a sample for safe value
fwdSafe=0
for c in range(0,20):
fwdSafe = max(fwdSafe, int(hcsr04.getDistance()))
time.sleep(0.1)
pz.reverse(speed)
#statusWin.clear()
statusWin.addstr(1,1,'Forward '+ str(speed)+" ")
elif keyp == '4' or keyp == 'j' or ord(keyp) == 18:
pz.spinRight(turnSpeed)
#statusWin.clear()
statusWin.addstr(1,1, 'Spin Right '+ str(speed)+" ")
elif keyp == '6' or keyp == 'l' or ord(keyp) == 19:
pz.spinLeft(turnSpeed)
#statusWin.clear()
statusWin.addstr(1,1, 'Spin Left '+ str(speed)+" ")
elif keyp == '.' or keyp == '>':
import time
import sys
import tty
import termios
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
try:
while True:
test = pz.readInput(2)
if (pz.readInput(2) != 1 and pz.readInput(3) != 1):
pz.reverse(40)
print "valeur : test", test
elif (pz.readInput(2) != 1 and pz.readInput(3) == 1):
pz.stop()
pz.spinLeft(100)
pz.spinRight(30)
elif (pz.readInput(3) != 1 and pz.readInput(2) == 1):
pz.stop()
pz.spinRight(100)
pz.spinLeft(30)
finally:
pz.cleanup()
if (buttons & cwiid.BTN_UP):
print 'Up pressed'
pz.forward(30)
time.sleep(button_delay)
sl.neoPixelLight("forward")
if (buttons & cwiid.BTN_B):
print 'Turbo pressed'
pz.forward(80)
time.sleep(button_delay)
sl.neoPixelLight("forward")
if (buttons & cwiid.BTN_DOWN):
print 'Down pressed'
pz.reverse(30)
time.sleep(button_delay)
sl.neoPixelLight("backward")
if (buttons & cwiid.BTN_1):
print 'Button 1 pressed'
time.sleep(button_delay)
if (buttons & cwiid.BTN_2):
print 'Button 2 pressed'
time.sleep(button_delay)
if (buttons & cwiid.BTN_A):
print 'Button A pressed'
pz.stop()
sl.neoPixelLight("off")
#print "Use . or > to speed up"
#print "Speed changes take effect when the next arrow key is pressed"
#print "Press Ctrl-C to end"
#print ""
pz.init()
# main loop
try:
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pz.forward(speed)
print('Forward', speed)
elif keyp == 'z' or ord(keyp) == 17:
pz.reverse(speed)
print('Reverse', speed)
elif keyp == 's' or ord(keyp) == 18:
pz.spinRight(speed)
print('Spin Right', speed)
elif keyp == 'a' or ord(keyp) == 19:
pz.spinLeft(speed)
print('Spin Left', speed)
elif keyp == '.' or keyp == '>':
speed = min(100, speed + 10)
print('Speed+', speed)
elif keyp == ',' or keyp == '<':
speed = max(0, speed - 10)
print('Speed-', speed)
elif keyp == ' ':
pz.stop()
def reverse(self, speed):
pz.reverse(speed)
#
# Basic test of HC-SR04 ultrasonic sensor on Picon Zero
import hcsr04, time
import piconzero as pz
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
try:
while True:
distance = int(hcsr04.getDistance())
print("Distance : "), distance
if (((pz.readInput(2) != 1) or (pz.readInput(3) != 1))
and distance > 13):
pz.reverse(60)
else:
pz.stop()
time.sleep(0.2)
pz.spinRight(50)
time.sleep(0.2)
pz.stop()
break
except KeyboardInterrupt:
print
finally:
hcsr04.cleanup()
pz.cleanup()
import hcsr04
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
speed = 100
try:
while True:
if ((pz.readInput(2) != 1) and (pz.readInput(3) == 1)):
pz.stop()
time.sleep(1)
pz.spinLeft(75)
time.sleep(1)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) != 1)):
pz.stop()
time.sleep(1)
pz.spinRight(75)
time.sleep(1)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) == 1)):
pz.stop()
time.sleep(1)
pz.spinLeft(75)
pz.reverse(45)
except KeyboardInterrupt:
print "Au revoir"
finally:
pz.cleanup()
hcsr04.cleanup()
while True:
buttons = wii.state['buttons']
if (buttons & cwiid.BTN_UP):
# Forwards
# stop all motors if too close to an object to prevent face plant
if sonar.getDistance() < 10:
pz.stop()
showFace(oooohData, 0, 255, 0)
else:
time.sleep(button_delay)
pz.forward(50)
showFace(smileData, 255, 0, 0)
elif (buttons & cwiid.BTN_DOWN):
time.sleep(button_delay)
pz.reverse(50)
showFace(grimaceData, 255, 0, 255)
elif (buttons & cwiid.BTN_LEFT):
time.sleep(button_delay)
pz.spinLeft(50)
showFace(oooohData, 0, 255, 0)
elif (buttons & cwiid.BTN_RIGHT):
time.sleep(button_delay)
pz.spinRight(50)
showFace(oooohData, 0, 255, 0)
else:
pz.stop()
showFace(frownData, 0, 0, 255)
# press button A to stop all motors
if (buttons & cwiid.BTN_A):
time.sleep(button_delay)
# Check if other buttons are pressed by
# doing a bitwise AND of the buttons number
# and the predefined constant for that button.
if (buttons & cwiid.BTN_LEFT):
print 'Left pressed'
pz.spinLeft(100)
time.sleep(button_delay)
elif (buttons & cwiid.BTN_RIGHT):
print 'Right pressed'
pz.spinRight(100)
time.sleep(button_delay)
elif (buttons & cwiid.BTN_UP):
print 'Up pressed'
pz.forward(100)
time.sleep(button_delay)
elif (buttons & cwiid.BTN_DOWN):
print 'Down pressed'
pz.reverse(100)
time.sleep(button_delay)
elif (buttons & cwiid.BTN_A):
print 'Button A pressed'
pz.stop()
time.sleep(button_delay)
else:
pz.cleanup()
