raspberrypi python_raspberryPi四路寻迹程序python

importRPi.GPIO as GPIO

import time

GPIO.setmode(GPIO.BOARD)

line=[31,33,35,37]

#define GPIO which connected get obstacle

col_fg=0

#1 follow white line and 0 follow black line

deffindline():

forintr in line:

GPIO.setup(intr,GPIO.IN)

flag='c'

lin1=GPIO.input(line[0])

lin2=GPIO.input(line[1])

lin3=GPIO.input(line[2])

lin4=GPIO.input(line[3])

ifcol_fg:

line1=not line1

line2=not line2

line3=not line3

line4=not line4

if lin1 and lin2 and lin3 and lin4:

#line overflow

return flag

elif lin4 and lin1:

#error line

return flag

elif lin1:

flag='d'

#turn rigth

return flag

elif lin4:

flag='a'

#turn left

return flag

elif lin3 or lin2:

flag='w'

#go forward

return flag

else:

flag='e'

return flag

#lost the line!!!

defgpcl():

GPIO.cleanup

print('All GPIO has been cleanup')

if __name__=='__main__':

try:

while True:

printfindline()