五路循迹在小龟小车上的Python代码参考
def scan(): # 扫描函数 返回表示五路的值
return [car.d3(), car.d2(), car.d1(), car.a2(), car.a1()]
def default_go(): # 利用差速修正走直线的go
L = 60
R = 30
car.motor(L,R,L,R)
def diff_go(L,R): # 差速前进
car.motor(L,R,L,R)
def turn_45R(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
if check: # 传入check是确认是否要执行
if track == [0,0,0,1,1]:
return True
else:
return False
else: # 不传入check是执行函数
# car.right(30, 0.5)
# diff_go(30,0) # 转向的代码
car.right(60, 0.5)
car.go(30, 0.2)
if track == [0,1,1,1,0] or track == [0,1,1,0,0]: # 支线任务执行完毕
print("完成支线任务 turn_45R")
return True
else: # 支线任务还要继续执行
return False
def turn_45L(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
if check: # 传入check是确认是否要执行
if track == [1,1,1,1,0]:
return True
else:
return False
else: # 不传入check是执行函数
car.left(60, 0.5)
car.go(30, 0.2) # 转向的代码
if track == [0,1,1,1,0] or track == [0,0,1,1,0]: # 支线任务执行完毕
print("完成支线任务 turn_45L")
return True
else: # 支线任务还要继续执行
return False
def turn_90R(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
if check: # 传入check是确认是否要执行
if track == [0,0,1,1,1] or track == [0, 1, 1, 1, 1]:
return True
else:
return False
else: # 不传入check是执行函数
car.right(90, 0.5)
car.go(30, 0.2) # 转向的代码
if track == [0,1,1,1,0] or track == [0,1,1,0,0]: # 支线任务执行完毕
print("完成支线任务 turn_45R")
return True
else: # 支线任务还要继续执行
return False
def turn_90L(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
if check: # 传入check是确认是否要执行
if track == [1,1,1,1,0] or track == [1, 1, 1, 0, 0]:
return True
else:
return False
else: # 不传入check是执行函数
# car.right(30, 0.5)
diff_go(30,0) # 转向的代码
if track == [0,1,1,1,0] or track == [0,1,1,0,0]: # 支线任务执行完毕
print("完成支线任务 turn_45R")
return True
else: # 支线任务还要继续执行
return False
def main_loop(tasks=[]): # 主线前进任务
turning = False # 是否进入支线转向任务
index = 0 # 任务索引
while True:
sleep(0.01)
track = scan()
if not turning: # 不在转向内
if track == [0,0,0,0,0]: # 没有轨迹了 停止
car.stop()
break
elif tasks[index](track, True): # 判断是否要处理支线任务
print("转到支线任务", index)
turning = True # 转到支线任务
elif track == [0,0,1,1,0]: # 走直线修正 偏左
diff_go(90,30)
elif track == [0,1,1,0,0]: # 走直线修正 偏右
diff_go(30,90)
else: # 默认前进
default_go()
else: # 在支线任务内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# elif track == [0,0,1,1,0]: # 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 偏右
# diff_go(30,90)
# if track == [0,0,1,1,0]: # 直线修正 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 直线修正 偏右
# diff_go(30,90)
# else:
# # 处理支线任务
# if tasks[index](track): # 如果支线任务完成了
# # turning = False # 切换回主线任务
# # if index + 1 > len(tasks):
# # break
# # else:
# # index += 1 # 将支线任务切换到下一个
# car.stop()
# print("结束主线任务")
# break
if tasks[index](track):
if index + 1 >= len(tasks):
car.stop()
print("结束主线任务")
break
else:
turning = False # 切换回主线任务
index += 1
print("进入任务", index)
main_loop([
turn_90R,
turn_90R,
turn_90L,
turn_90L,
# turn_45R,
# turn_90R,
# turn_90L,
# turn_90R()
# turn_90L()
# turn_90L()
])
# '''
# 注释记录传感器和主控板的连接方式
# D3 D2 D1 A2 A1
# '''
# def scan(): # 扫描函数 返回表示五路的值
# return [car.d3(), car.d2(), car.d1(), car.a2(), car.a1()]
# def correct_line(): # 偏移轨道的直线修正函数
# track = scan()
# if track == [0,0,0,1,0]: # 偏左
# car.motor(40,30,40,30)
# elif track == [0,1,0,0,0]: # 偏右
# car.motor(30,40,30,40)
# def turn_45(): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# while True:
# sleep(0.02)
# track = scan()
# # elif track == [0,0,1,1,1]: # 右转直角
# # car.motor(30,0,30,0)
# if track == [0,0,0,0,0]: # 无线状态
# car.stop()
# break
# elif track[2] != 1: # 偏移轨道了
# if track == [0,0,0,1,0]: # 偏左
# car.motor(60,0,60,0)
# elif track == [0,1,0,0,0]: # 偏右
# car.motor(0,60,0,60)
# elif track == [0,1,1,0,0] or [0,1,1,1,0]:# 左转直角
# car.motor(0,30,0,30)
# elif track[2] != 1: # 偏移轨道了
# correct_line()
# else:
# car.go(30)
# def stop_line(): # 停止线
# while True:
# sleep(0.02)
# track = scan()
# if track == [1,1,1,1,1]: # 停止线
# car.stop()
# break
# elif track[2] != 1: # 偏移轨道了
# if track == [0,0,0,1,0]: # 偏左
# car.motor(70,0,70,0)
# elif track == [0,1,0,0,0]: # 偏右
# car.motor(0,70,0,70)
# else:
# car.go(50)
# def default_go(): # 利用差速修正走直线的go
# L = 60
# R = 30
# car.motor(L,R,L,R)
# def diff_go(L,R): # 差速前进
# car.motor(L,R,L,R)
# def line(): # 走直线
# while True:
# sleep(0.02)
# track = scan()
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# elif track == [0,0,1,1,0]: # 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 偏右
# diff_go(30,90)
# else:
# default_go()
# def turn_90L(): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# turning = False
# while True:
# sleep(0.02)
# track = scan()
# if not turning: # 不在转向内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# elif track == [1,1,1,1,0]: # 左直角弯
# # diff_go(-30,30)
# car.stop()
# turning = True
# elif track == [0,0,1,1,0]: # 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 偏右
# diff_go(30,90)
# else:
# default_go()
# else: # 在转向内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# # elif track == [0,0,1,1,0]: # 偏左
# # diff_go(90,30)
# # elif track == [0,1,1,0,0]: # 偏右
# # diff_go(30,90)
# else:
# car.left(30, 0.5)
# diff_go(0,30) # 转向的代码
# # def turn_45R(): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# # turning = False
# # while True:
# # sleep(0.02)
# # track = scan()
# # if not turning: # 不在转向内
# # if track == [0,0,0,0,0]: # 没有轨迹了 停止
# # car.stop()
# # break
# # elif track == [0,0,0,1,1]: # 右45弯
# # # diff_go(-30,30)
# # car.stop()
# # turning = True
# # elif track == [0,0,1,1,0]: # 偏左
# # diff_go(90,30)
# # elif track == [0,1,1,0,0]: # 偏右
# # diff_go(30,90)
# # else:
# # default_go()
# # else: # 在转向内
# # if track == [0,0,0,0,0]: # 没有轨迹了 停止
# # car.stop()
# # break
# # elif track == [0,0,1,1,0]: # 偏左
# # diff_go(90,30)
# # elif track == [0,1,1,0,0]: # 偏右
# # diff_go(30,90)
# # else:
# # car.right(30, 0.5)
# # diff_go(30,0) # 转向的代码
# def turn_90R(): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# turning = False
# while True:
# sleep(0.02)
# track = scan()
# if not turning: # 不在转向内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# elif track == [0,0,1,1,1]: # 右45弯
# # diff_go(-30,30)
# car.stop()
# turning = True
# elif track == [0,0,1,1,0]: # 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 偏右
# diff_go(30,90)
# else:
# default_go()
# else: # 在转向内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# elif track == [0,0,1,1,0]: # 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 偏右
# diff_go(30,90)
# else:
# car.right(30, 0.5)
# diff_go(30,0) # 转向的代码
# def turn_45L(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# if check: # 传入check是确认是否要执行
# if track == [1,1,1,1,0]:
# return True
# else:
# return False
# else: # 不传入check是执行函数
# car.left(30, 0.5)
# diff_go(0,30) # 转向的代码
# if track == [0,1,1,1,0]: # 支线任务执行完毕
# return True
# else: # 支线任务还要继续执行
# return False
# def turn_45R(track, check=False): # 直角弯 (0,0,1,0,0) =>(0,0,1,1,1)
# if check: # 传入check是确认是否要执行
# if track == [0,0,1,1,0]:
# return True
# else:
# return False
# else: # 不传入check是执行函数
# car.right(30, 0.5)
# diff_go(0,30) # 转向的代码
# if track == [0,1,1,1,0]: # 支线任务执行完毕
# return True
# else: # 支线任务还要继续执行
# return False
# def main_loop(tasks): # 主线前进任务
# turning = False # 是否进入支线转向任务
# index = 0 # 任务索引
# while True:
# sleep(0.02)
# track = scan()
# if not turning: # 不在转向内
# if track == [0,0,0,0,0]: # 没有轨迹了 停止
# car.stop()
# break
# # elif tasks[index](track, True): # 判断是否要处理支线任务
# # turning = True # 转到支线任务
# elif track == [0,0,1,1,0]: # 走直线修正 偏左
# diff_go(90,30)
# elif track == [0,1,1,0,0]: # 走直线修正 偏右
# diff_go(30,90)
# else: # 默认前进
# default_go()
# # else: # 在支线任务内
# # # if track == [0,0,0,0,0]: # 没有轨迹了 停止
# # # car.stop()
# # # break
# # # elif track == [0,0,1,1,0]: # 偏左
# # # diff_go(90,30)
# # # elif track == [0,1,1,0,0]: # 偏右
# # # diff_go(30,90)
# # if track == [0,0,1,1,0]: # 直线修正 偏左
# # diff_go(90,30)
# # elif track == [0,1,1,0,0]: # 直线修正 偏右
# # diff_go(30,90)
# # else:
# # # 处理支线任务
# # tasks[index](track): # 如果支线任务完成了
# # # if tasks[index](track): # 如果支线任务完成了
# # # turning = False # 切换回主线任务
# # # if index + 1 > len(tasks):
# # # break
# # # else:
# # # index += 1 # 将支线任务切换到下一个
# # # break
# # 主程序
# # while True:
# # pass
# # default_go(60,30)
# # turn_45R()
# # turn_90L()
# # stop_line()
# # line()
# # print(scan(), scan()==[0,0,0,0,0])
# # turn_90R()
# # turn_90R()
# # turn_90L()
# # turn_90L()
# # turn_45L()
# main_loop([
# turn_45L,
# turn_45R
# # turn_90R()
# # turn_90L()
# # turn_90L()
# ])