def loop():
car_dir.dis_home(pwm1) #Ultrasound initial position
time.sleep(0.5)
a = ultrasonic.checkdist() #Get the ultrasonic detection distance
b = ultrasonic.checkdist() #Get the ultrasonic detection distance
c = ultrasonic.checkdist() #Get the ultrasonic detection distance
homedis = min(a, b, c) #Get the ultrasonic detection distance
print('homedis = %0.2f m' % homedis)
motor.motorStop() #Stop the car
if homedis > dis: #No obstacles
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, t_spd)
elif homedis < dis: #Obstacles
car_dir.dis_left(pwm1)
time.sleep(1)
a = ultrasonic.checkdist()
b = ultrasonic.checkdist()
c = ultrasonic.checkdist()
leftdis = min(a, b, c)
print('leftdis = %0.2f m' % leftdis)
car_dir.dis_right(pwm1)
time.sleep(1)
a = ultrasonic.checkdist()
b = ultrasonic.checkdist()
c = ultrasonic.checkdist()
rightdis = min(a, b, c)
print('rightdis = %0.2f m' % rightdis)
if leftdis < dis and rightdis < dis: #Judgment left and right
if leftdis >= rightdis: #There are obstacles on the right
motor.motor(status, backward, b_spd)
#motor.motor(status,backward,left)
#motor.motor1(status,backward,t_spd)
motor.motor1(status, backward, right)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #There are obstacles on the left
motor.motor(status, backward, left)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif leftdis > dis and rightdis <= dis: #There are obstacles on the right
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
elif rightdis > dis and leftdis <= dis: #There are obstacles on the left
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif rightdis > dis and leftdis > dis: #There are no obstacles
if rightdis > leftdis: #The distance to the right is greater than the left
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif rightdis < leftdis: #The distance to the left is greater than the right
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #NO Obstacle ahead
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
elif leftdis == rightdis:
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
def run():
while True:
print 'waiting for connection...'
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ)
if not data:
continue
if data == ctrl_cmd[0]:
global spd
print 'motor moving forward'
motor.motor(status, forward, spd)
direction = forward
print 'spd = %d' % spd
ledon = threading.Thread(target=ledOn)
ledon.start()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.motor(status, backward, spd)
direction = backward
print 'spd = %d' % spd
breathingon = threading.Thread(target=breathingOn)
breathingon.start()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
rgbLed.setup(leftRgb)
car_dir.dir_left(pwm0)
car_dir.dis_left(pwm1)
leftrgb = threading.Thread(target=leftrgbCtrl)
leftrgb.start()
rgbLed.stop(rightRgb)
continue
elif data == ctrl_cmd[3]:
print 'recv right cmd'
rgbLed.setup(rightRgb)
car_dir.dir_right(pwm0)
car_dir.dis_right(pwm1)
rightrgb = threading.Thread(target=rightrgbCtrl)
rightrgb.start()
rgbLed.stop(leftRgb)
continue
elif data == ctrl_cmd[4]:
print 'recv home cmd'
car_dir.dir_home(pwm0)
car_dir.dis_home(pwm1)
rgbLed.stop(rightRgb)
rgbLed.stop(leftRgb)
leftrgb = Process(target=homeRgbCtrl1)
leftrgb.start()
rightrgb = Process(target=homeRgbCtrl2)
rightrgb.start()
continue
elif data == ctrl_cmd[5]:
print 'recv distance cmd'
print 'Distance: %0.2fm' % ultrasonic.checkdist()
continue
elif data == ctrl_cmd[6]:
print 'recv whistle cmd'
activeBuzzer.loop()
continue
elif data == ctrl_cmd[7]:
print 'recv stop cmd'
setup()
motor.motorStop()
breathingLed.stop(b_LedPin)
rgbLed.stop(leftRgb)
rgbLed.stop(rightRgb)
activeBuzzer.stop()
GPIO.cleanup()
setup()
continue
elif data == ctrl_cmd[8]:
print 'recv exit cmd'
GPIO.cleanup()
tcpSerSock.close()
os.system('sudo init 0')
elif data == ctrl_cmd[9]:
print 'recev auto cmd'
auto = threading.Thread(target=autoMode)
auto.start()
continue
elif data[0:5] == 'speed':
print 'recv speed cmd'
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
print 'tmp(str) = %s' % tmp
spd = int(tmp)
print 'spd(int) = %d' % spd
print 'direction = %d' % direction
if spd < minspd:
spd = minspd
elif spd > maxspd:
spd = maxspd
motor.motor(status, direction, spd)
continue
else:
print 'Command Error! Cannot recongnize command: ' + data
def loop(): while True: while True: car_dir.dis_home(pwm1) car_dir.dir_home(pwm0) time.sleep(0.5) homedis = ultrasonic.checkdist() print 'homedis = %0.2f m' %homedis motor.motorStop() if homedis > dis: motor.motor(status, forward, f_spd) break elif homedis < dis: car_dir.dis_left(pwm1) time.sleep(0.5) leftdis = ultrasonic.checkdist() print 'leftdis = %0.2f m' %leftdis car_dir.dis_right(pwm1) time.sleep(0.5) rightdis = ultrasonic.checkdist() print 'rightdis = %0.2f m' %rightdis if leftdis < dis and rightdis < dis: if leftdis >= rightdis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) break else: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) break elif leftdis > dis and rightdis <= dis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis > dis and leftdis <= dis: if homedis < mindis: car_dir.dir_left(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis > dis and leftdis > dis: if rightdis > leftdis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis < leftdis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif leftdis == rightdis: motor.motor(status, backward, b_spd) time.sleep(1) break