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 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 spin_left(self):
""" Rotate to the left """
global motor_speed
global motor_direction
with self._lock:
pz.spinRight(motor_speed)
motor_direction = 'Spin Left'
return motor_direction
def movebackward():
global pz,speed
try:
pz.spinRight(speed)
time.sleep(0.05)
pz.stop()
except KeyboardInterrupt:
print "quit"
def SeekLine(SeekSpeed, SeekSize):
print("Seeking the line")
# The direction the robot will turn - True = Left
Direction = True
#SeekSize = 0.25 # Turn for 0.25s
SeekCount = 1 # A count of times the robot has looked for the line
MaxSeekCount = 3 # The maximum time to seek the line in one direction
# Turn the robot left and right until it finds the line
# Or it has been searched for long enough
while SeekCount <= MaxSeekCount:
# Set the seek time
SeekTime = SeekSize * SeekCount
# Start the motors turning in a direction
if Direction:
print("Seeking - Looking left")
pz.spinRight(SeekSpeed)
sl.neoPixelLight("left")
sleep(SeekSize / 50)
else:
print("Seeking - Looking Right")
pz.spinLeft(SeekSpeed)
sl.neoPixelLight("right")
sleep(SeekSize / 50)
# Save the time it is now
StartTime = time()
# While the robot is turning for SeekTime seconds,
# check to see whether the line detector is over black
while time() - StartTime <= SeekTime:
if GPIO.input(lineCentre) == 1:
pz.stop()
# Exit the SeekLine() function returning
# True - the line was found
return True
# The robot has not found the black line yet, so stop
pz.stop()
sl.neoPixelLight("off")
# Increase the seek count
SeekCount += 1
# Change direction
Direction = not Direction
# The line wasn't found, so return False
return False
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 speedrun():
pz.stop()
mylcd.lcd_display_string("Speed Run ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
time.sleep(2)
speed = 100
GO = 1
PREP = 1
while PREP == 1: #get ready to go
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":
pz.stop
GO = 0
PREP = 0
while GO == 1: #new simple quit - works perfectly
if button.is_pressed:
pz.stop()
GO = 0
else:
RIGHTIR = pz.readInput(0)
LEFTIR = pz.readInput(1)
if RIGHTIR == 0:
pz.spinLeft(75)
mylcd.lcd_display_string("Right IR on ", 1)
mylcd.lcd_display_string("Use switch=STOP ", 2)
elif LEFTIR == 0:
pz.spinRight(75)
mylcd.lcd_display_string("left IR on ", 1)
mylcd.lcd_display_string("Use switch=STOP ", 2)
elif RIGHTIR == 1 and LEFTIR == 1:
pz.forward(100)
mylcd.lcd_display_string("No IR on ", 1)
mylcd.lcd_display_string("Use switch=STOP ", 2)
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()
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import time
import piconzero as pz
import hcsr04
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
speed = 60
i = 0
try:
while True:
if ((pz.readInput(2) != 1) and (pz.readInput(3) != 1)):
pz.spinLeft(100)
time.sleep(0.25)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) != 1)):
pz.spinRight(100)
time.sleep(0.25)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) == 1)):
pz.reverse(speed)
time.sleep(0.1)
else:
pz.reverse(speed)
except KeyboardInterrupt:
print "Au revoir"
finally:
pz.cleanup()
hcsr04.cleanup()
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
# start recording
ts = str(time())
cam.vflip = True
cam.hflip = True
cam.start_recording("/home/pi/qmpiwars/videos/line-" + ts + ".h264")
try:
while True:
if GPIO.input(lineRight) == 1:
print("Line Right is seeing black. Turn Right")
pz.spinLeft(TURN)
sl.neoPixelLight("right")
sleep(sTime)
elif GPIO.input(lineLeft) == 1:
print("Line Left is seeing black. Turn Left")
pz.spinRight(TURN)
sl.neoPixelLight("left")
sleep(sTime)
elif GPIO.input(lineCentre) == 1:
print("Line Centre is seeing black. Drive Forwards")
pz.forward(FORWARD)
sl.neoPixelLight("forward")
sleep(fTime)
else:
print("Everything is seeing white. Seak left and right")
SeekLine(STURN, sTime)
except KeyboardInterrupt:
GPIO.cleanup()
sl.neoPixelLight("off")
pz.cleanup()
cam.stop_recording()
def automaze():
pz.stop()
mylcd.lcd_display_string("Auto Maze ", 1)
mylcd.lcd_display_string("Press E to End ", 2)
time.sleep(2)
MSPEED = 20
MTURN = 90
GO = 1
PREP = 1
STEP = 0 # start step count
while PREP == 1: #get ready to go
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:
RIGHTIR = pz.readInput(0) #assign right IR to a variable
LEFTIR = pz.readInput(1) #assign left IR to a variable
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
pz.forward(MSPEED)
#steps to follow to complete the maze
if RANGE < 25 and STEP == 0: #first right turn
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 1
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 1: #second right turn
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 2
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 2: #third right
pz.stop()
pz.spinRight(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 3
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 3: #first left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 4
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 4: #second left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 5
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
if RANGE < 25 and STEP == 5: #third left
pz.stop()
pz.spinLeft(MTURN)
time.sleep(0.5)
pz.stop()
pz.forward(MSPEED)
time.sleep(1)
STEP = 6
mylcd.lcd_display_string("Range = %d %%" % RANGE, 1)
mylcd.lcd_display_string("Step = %d %%" % STEP, 2)
RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable
#emergency wall avoidance protocol
if LEFTIR == 0:
pz.spinRight(100)
time.sleep(0.01)
elif RIGHTIR == 0:
pz.spinLeft(100)
time.sleep(0.01)
front = 50
while True:
capteurDevant = Echo(TRIGGER_PIN1, ECHO_PIN1, speed_of_sound)
front = capteurDevant.read('cm',samples)
capteurDevant.stop()
if front > gapAvant:
print("Rien devant")
pz.forward(speed)
capteurGauche = Echo(TRIGGER_PIN2, ECHO_PIN2, speed_of_sound)
side = capteurGauche.read('cm',samples)
capteurGauche.stop()
print("distance gauche = ",side)
elif side > gapGauche:
print("Tournant a gauche detecte")
pz.forward(speed)
time.sleep(0.3)
pz.spinLeft(91)
time.sleep(1)
pz.stop()
time.sleep(0.1)
print("Objet detecte, rotation")
pz.spinRight(65)
time.sleep(1.3)
pz.stop()
capteurDevant = Echo(TRIGGER_PIN1, ECHO_PIN1, speed_of_sound)
front = capteurDevant.read('cm',samples)
capteurDevant.stop()
paroi = False
#boucle une fois avoir rejoint une paroi
if onTrack:
# Si rien devant, et il est centre entre 20 et 30cm du mur
if (front > gapmax and front != 0) and gapmin < side < gapmax:
print("j'avance devant libre mur 20 30")
pz.forward(speed)
# Si contact mur avant
elif front < gapmax / 2 and front != 0:
print("Je suis face a une paroi")
pz.forward(-50)
time.sleep(0.5)
pz.spinRight(90)
time.sleep(0.9)
pz.stop()
# s'ecarte du mur, mais pas trop
elif front > gapmax and 50 > side > gapmax * 0.8:
pz.stop()
print(
"je peux avancer mais je mecarte du mur, donc je me rapproche")
if side < 50:
pz.spinLeft(60)
time.sleep(0.4)
pz.forward(75)
time.sleep(0.8)
pz.spinRight(60)
except: KeyboardInterrupt:
print "quit"
# 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()
print 'Stop'
elif ord(keyp) == 3:
break
mylcd.lcd_display_string("Select Ends ", 2)
time.sleep(2)
RUN = 1
buttons_pressed = joystick.get_and_clear_button_press_history()
while RUN == 1:
print("In Loop")
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif buttons_pressed & 1 << SixAxis.BUTTON_D_UP:
pz.forward(60)
print("Forwards")
elif buttons_pressed & 1 << SixAxis.BUTTON_D_DOWN:
pz.reverse(60)
print("Reverse")
elif buttons_pressed & 1 << SixAxis.BUTTON_D_RIGHT:
pz.spinRight(60)
print("Right")
elif buttons_pressed & 1 << SixAxis.BUTTON_D_LEFT:
pz.spinLeft(60)
print("Left")
else:
RUN = 1
buttons_pressed = joystick.get_and_clear_button_press_history()
pz.stop()
#____________________________________________________________________________________________________________________
elif buttons_pressed & 1 << SixAxis.BUTTON_CIRCLE:
print ("Remote control complex")
mylcd.lcd_display_string("Starting", 1)
mylcd.lcd_display_string("Remote Control C", 2)
import time
import piconzero as pz
import hcsr04
pz.setInputConfig(2, 0)
pz.setInputConfig(3, 0)
pz.init()
hcsr04.init()
speed = 65
i = 2
try:
while True:
if ((pz.readInput(2) != 1) and (pz.readInput(3) != 1) and i % 2 == 0):
pz.stop()
time.sleep(0.1)
pz.spinRight(70)
time.sleep(0.25)
elif ((pz.readInput(2) != 1) and (pz.readInput(3) != 1)
and i % 2 == 1):
pz.stop()
time.sleep(0.1)
pz.spinLeft(70)
time.sleep(0.25)
elif ((pz.readInput(2) == 1) and (pz.readInput(3) == 1)):
i += 1
else:
pz.reverse(speed)
except KeyboardInterrupt:
print "Au revoir"
finally:
pz.cleanup()
if (buttons - cwiid.BTN_PLUS - cwiid.BTN_MINUS == 0):
print '\nClosing connection ...'
wii.rumble = 1
time.sleep(1)
wii.rumble = 0
sl.neoPixelLight("off")
pz.cleanup()
cam.stop_recording()
exit(wii)
# 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.spinRight(90)
time.sleep(button_delay)
sl.neoPixelLight("left")
if (buttons & cwiid.BTN_RIGHT):
print 'Right pressed'
pz.spinLeft(90)
time.sleep(button_delay)
sl.neoPixelLight("right")
if (buttons & cwiid.BTN_UP):
print 'Up pressed'
pz.forward(30)
time.sleep(button_delay)
sl.neoPixelLight("forward")
#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()
print('Stop')
elif ord(keyp) == 3:
break
while gapmax > front > 10:
print("je peux me rapprocher distance = ", front)
toutDroit()
pz.forward(60)
time.sleep(0.2)
pz.stop()
capteurDevant = Echo(TRIGGER_PIN1, ECHO_PIN1,
speed_of_sound)
front = capteurDevant.read('cm', samples)
capteurDevant.stop()
if front < 10:
pz.stop()
pz.forward(-50)
time.sleep(0.5)
droite()
pz.spinRight(95)
time.sleep(1)
toutDroit()
pz.forward(50)
capteurDevant = Echo(TRIGGER_PIN1, ECHO_PIN1,
speed_of_sound)
front = capteurDevant.read('cm', samples)
capteurDevant.stop()
elif front == 0 and side == 0:
gauche()
pz.spinLeft(90)
time.sleep(1)
toutDroit()
pz.forward(60)
#detection coin tournant a auche
def right(self, speed):
pz.spinRight(speed)
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()
# 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)
pz.stop()
showFace(frownData, 0, 0, 255)
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 == '>':
speed = min(100, speed+10)
#statusWin.clear()
statusWin.addstr(1,1, 'Speed+ '+ str(speed)+" ")
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
#statusWin.clear()
statusWin.addstr(1,1, 'Speed- '+ str(speed)+" ")
elif keyp == 'w':
