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 moveforward():
try:
pz.forward(speed)
time.sleep(0.1)
pz.stop()
except: KeyboardInterrupt:
print "quit"
def forward(self):
""" Move both motors forward """
global motor_speed
global motor_direction
with self._lock:
# pz.forward(speed)
pz.forward(motor_speed)
motor_direction = 'Forward'
return motor_direction
def callback(data):
rospy.loginfo("I receive %s", data.data)
counter = 0
while counter < data.data:
pz.forward(speed)
time.sleep(.05)
pz.stop()
time.sleep(.1)
counter += 1
def moverightturn():
global pz,speed
try:
pz.forward(speed)
time.sleep(0.05)
pz.spinRight(speed)
time.sleep(0.05)
pz.forward(speed)
time.sleep(0.05)
pz.stop()
except KeyboardInterrupt:
print "quit"
def wallstop():
print("Wall Stop Program Active")
mylcd.lcd_display_string("Wall Stop", 1)
mylcd.lcd_display_string("Select Ends", 2)
RUN = 1
while RUN == 1:
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif RANGE > 10:
pz.forward(100)
mylcd.lcd_display_string("Range= %d %%" % RANGE, 2)
elif 5 < RANGE <= 10:
pz.forward(40)
mylcd.lcd_display_string("Range= %d %%" % RANGE, 2)
elif 2 < RANGE <= 5:
pz.forward(20)
mylcd.lcd_display_string("Range= %d %%" % RANGE, 2)
elif 1 <= RANGE <= 2:
pz.forward(10)
mylcd.lcd_display_string("Range= %d %%" % RANGE, 2)
elif RANGE < 1:
pz.stop(0)
mylcd.lcd_display_string("Range= %d %%" % RANGE, 2)
time.sleep(2)
mylcd.lcd_display_string("Ended", 1)
mylcd.lcd_display_string("Press Select", 2)
time.sleep(10)
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 run():
distance = int(hcsr04.getDistance())
while (distance > 10):
distance = int(hcsr04.getDistance())
pz.stop()
while True:
distance = int(hcsr04.getDistance())
print "Distance:", distance
distance2Pixel(distance)
if distance < 10:
pz.stop()
time.sleep(1)
while (distance < 18):
distance = int(hcsr04.getDistance())
pz.spinLeft(speed)
elif distance < 25:
pz.forward(speed - 20)
else:
pz.forward(speed)
time.sleep(0.1)
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 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 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 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
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 = capteurDevant.read('cm', samples)
capteurDevant.stop()
capteurGauche = Echo(TRIGGER_PIN2, ECHO_PIN2, speed_of_sound)
side = capteurGauche.read('cm', samples)
capteurGauche.stop()
print("devant = ", front, " gauche = ", side)
samples = 1
paroi = False
#boucle une fois avoir rejoint une paroi
if onTrack:
#detection paroi en face
if front < 50 and front != 0:
toutDroit()
pz.forward(40)
while front < gapmax and front != 0:
print(front, "Je detecte une paroi en face")
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)
front = capteurDevant.read('cm', samples)
capteurDevant.stop()
capteurGauche = Echo(TRIGGER_PIN2, ECHO_PIN2, speed_of_sound)
side = capteurGauche.read('cm', samples)
capteurGauche.stop()
print("devant = ", front, " gauche = ", side)
samples = 1
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(
def forward(self, speed):
pz.forward(speed)
ts = str(time())
cam.vflip = True
cam.hflip = True
cam.start_recording("/home/pi/qmpiwars/videos/straight-" + 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 "----------------------------------"
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
if(distance <= frontStop): # too close - STOP!
print "Stop!"
#pz.stop()
sideDist.neoPixelLight("off")
elif(LeftDist <= sideStop): # heading left - turn right
print "turning right"
pz.spinLeft(spinSpeed)
sleep(spinTime)
sideDist.neoPixelLight("right")
pz.forward(forwardSpeed)
sleep(forwardTime)
sideDist.neoPixelLight("forward")
if(LeftDist < sideStop): # still too close move forward a bit more
def moveForward(self, newSpeed):
if newSpeed >= 0 and newSpeed <= 1:
self.setDirection(self.DIRECTION_FORWARD, self.DIRECTION_FORWARD)
pz.forward(speed)
# 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")
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")
def forward(self, speed):
"""Both motors forward"""
pz.forward(speed)
#print "Tests the motors by using the arrow keys to control"
#print "Use , or < to slow down"
#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)
# 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()
def backward(self, speed):
pz.forward(-speed)
ir=0
helpWin.addstr(1,1, "Tests the motors by using the arrow keys to control. num keys. 5 to stop. IJLM. K=stop")
helpWin.addstr(2, 1, "Use , or < to slow down. Use . or > to speed up. V to distance scan")
helpWin.addstr(3,1, "Move cam. F and G. Neck. WADZ. S = centre. opencv scan=E. R=save webcam")
helpWin.refresh()
# main loop
try:
while True:
#keyp = readkey()
#keyp = readchar()
keyp = GetChar(False)
if keyp == '2' or keyp == 'm' or ord(keyp) == 16:
pz.forward(speed)
#statusWin.clear()
statusWin.addstr(1,1, 'Reverse '+ str(speed)+" ")
elif keyp == 'r':
prefix=str(time.time())
copyfile("/dev/shm/lastsnap.jpg","/home/pi/"+prefix+"_lastsnap.jpg")
copyfile("/dev/shm/p3.png","/home/pi/"+prefix+"_p3.png")
copyfile("/dev/shm/p4.png","/home/pi/"+prefix+"_p4.png")
statusWin.addstr(1,1, 'Saved webcam and opencv images to prefix '+prefix)
elif keyp == 'e':
img = Image("/dev/shm/lastsnap.jpg")
object = img.hueDistance(Color.BLUE)
wii.led = 1
showFace(pairData, 0, 255, 0)
wii.rpt_mode = cwiid.RPT_BTN
time.sleep(1)
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()
#mylcd.lcd_display_string("Starting", 1)
#mylcd.lcd_display_string("Remote Control B", 2)
#time.sleep(2)
#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
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()
#____________________________________________________________________________________________________________________
