使用 python 編寫樂高 EV3機器人的循線機器人，使用 MindSensors 的 LightSensor Array 光感測器模組。遇到十字路口會自動迴轉180度。
[pastacode lang=”python” message=”[python on lego EV3] Line follower with MindSensors LightSensor Array ” highlight=”” provider=”manual”]from ev3.ev3dev import Key,Msensor,Motor
import time
key=Key()
mRight = Motor(port= Motor.PORT.B) #motor right
mLeft = Motor(port= Motor.PORT.A) #motor left
light=Msensor(port = 1)
delaytime = 0.001
lmotor = 0
rmotor = 0
MOTOR_POWER = -20
thr = 50
lightVal = []
stat = 0
#0 -> search line
#1 -> follow line
#2 -> cross
#3 -> turn 180 degree
lstat = 0
#line sensor state use binary,for example:0,1,1,0,0,0,1,1= 2+4+64+128= 198
def linestate():
sum=0
for index in range(8):
sum = sum + lightVal[index]*2**(index)
return sum
def linePrint():
print(len(lightVal))
for index in range(7):print(lightVal[index]),
print(lightVal[7])
def getVal():
val=[]
binVal=[]
val.append(light.value0)
val.append(light.value1)
val.append(light.value2)
val.append(light.value3)
val.append(light.value4)
val.append(light.value5)
val.append(light.value6)
val.append(light.value7)
for i in range(8):
binVal.append(level(val[i],thr))
return binVal
def level(value,threshold):
if(value < threshold):
return 1
else:
return 0
#main loop
while key.backspace != True:
lmotor = 0
rmotor = 0
lightVal = getVal()
lstat = linestate()
if(stat == 0):
print("Search Line!!"),
linePrint()
if lightVal.count(1) == 0:#no line -> turn
lmotor=MOTOR_POWER
rmotor=MOTOR_POWER
else:#has line
stat = 1
#linefollow()
elif(stat == 1):
print("Follow Line!!"),
print("state:"+str(lstat)),
linePrint()
if(lstat == 0):#out of line
stat = 0
if(lstat == 255):#find cross
stat = 2
elif(lstat == 24):#mid
print("Mid!!")
lmotor=MOTOR_POWER
rmotor=MOTOR_POWER
elif(lstat == 48):
lmotor = MOTOR_POWER*0.4
rmotor = MOTOR_POWER
elif(lstat == 12):
lmotor = MOTOR_POWER
rmotor = MOTOR_POWER*0.4
elif(lstat > 24):#line right side
print("Right!!")
lmotor=MOTOR_POWER*0.4
rmotor=MOTOR_POWER*1.4
elif(lstat < 24):#line left side
print("Left!!")
lmotor=MOTOR_POWER*1.4
rmotor=MOTOR_POWER*0.4
elif(stat == 2):
print("Find Cross!!")
mLeft.stop()
mRight.stop()
#if(lstat != 255):
# stat = 0
#if(key.right == True):
# stat = 0
time.sleep(0.5)
stat = 3
elif(stat == 3):#Turn 180 degree
mRight.run_forever(MOTOR_POWER)
mLeft.run_forever(-MOTOR_POWER)
time.sleep(3.5)
stat = 1
mRight.run_forever(lmotor)
mLeft.run_forever(rmotor)
time.sleep(delaytime)
mRight.stop()
mLeft.stop()
