2011 中国教育机器人大赛教育机器人创意设计项目报告书

2011 中国教育机器人大赛教育机器人创意设计项目
报告书
项目名称：机器人协助搬运色块
学校：深圳大学
院系：机电与控制工程学院
专业：自动化
学生姓名：张振操陈剑波陈桂强
指导教师：王鑫
时间： 20110年11月 12 日
]
目录
摘要 (4)
关键词 (4)
一、设计任务与要求 (5)
二、方案设计与论证 (5)
三、硬件单元电路设计与参数计算 (5)
1. QTI传感器技术参数电路 (5)
2. 颜色传感器技术参数电路 (6)
3. 无线通讯模块电路 (7)
四、软件设计思路 (11)
五、作品图及元件清单 (12)
1．作品图 (14)
2．元件清单 (14)
六、结论与心得 (17)
七、参考文献 (17)
八、程序清单 (18)
机器人协助搬运色块
摘要：本项目主要是通过短距离无线通讯实现两辆宝贝车之间的信息交流与信息处理，最终完成既定任务的搬运。

此项目通过对QTI传感器、颜色传感器以及无线模块三者进行组装、密切配合调试，加上应用软件编程方面的组合调试，让小车能够识别不同颜色的木块并将该木块搬运到指定的地点。

关键词：闭环反馈调节协助短距离无线通讯颜色识别 QTI巡线
一、设计任务与要求
本项目任务为：
将不同颜色的木块随机摆放在不同位置，让龙人宝贝车将不同颜色的木块推到制定的位置，实现智能搬运。

要求:
在没有人为干涉的情况下，让宝贝车自行智能地完成既定任务。

使用的是欧鹏公司提供的宝贝车，其中包括QTI传感器（实现巡线走）、颜色传感器（识别特定颜色）、无线模块（实现宝贝车之间的互相通讯）。

二、方案设计与论证
硬件搭建：
装配好宝贝车，给龙人宝贝车装配好QTI传感器，颜色传感器，无线模块，让每部分都能够各自正常使用。

方案设计：
本项目是通过对五种不同颜色色块的识别，将其搬运到指定的颜色位置。

设计的思路是让两辆宝贝车通过无线通讯互相协作进行搬运任务。

具体方案如下：
首先A、B两辆车从起点相继出发，A车直走去搬运最左边的木块，识别颜色后给停在候车位置的B车发送无线信息，如果正在被搬运的木块在到达制定位置的最短路线上有其它木块挡道的话，B车会执行清理路障木块程序；如果没有的话，B车在原位置待命，等待下一次信号。

这样通过从左到右逐个对木块搬运，最终达到将五个随机木块都准确搬运到指定位置的目的。

论证：
已经在实验中取得显著成果，基本能够实现以上功能，完成任务。

只是精度问题随着电池能量的消耗会出现稍微的偏差。

三、硬件单元电路设计与参数
1. QTI传感器电路
QTI边线检测传感器性能参数：
工作温度：-40℃～85℃
参考电压：5V
连续电流：50mA
功耗：100mW
最佳距离：10mm
最佳距离最大散射角度65度
探测到黑色物体输出高电平，探测到白色物体输出低电平。

响应时间：（V = 5v ,R = 100, I = 5mA ）
⏹上升沿时间10us
⏹下降沿时间50us
工作电路：
接线电路：
2. 颜色传感器电路
3. 无线模块电路
技术参数
●电源要求：USB 5V供电
●通讯：串行传递到的XBee模块
●管脚兼容其他的Xbee模块
●LED多种状态指示
●工作温度：-40℃至+70℃
●尺寸：38.3mm×25.6mmx 14.8mm
四、主程序设计思路
A、B两辆车从起点相继出发，A车直走去搬运最左边的木块，识别颜色后给停在候车位置的B车发送无线信息，如果正在被搬运的木块在到达制定位置的最短路线上有其它木块挡道的话，B车会执行清理路障木块程序；如果没有的话，B车在原位置待命，等待下一次信号。

这样通过从左到右逐个对木块搬运，最终达到将五个随机木块都准确搬运到指定位置的目的。

五、作品图及元件清单
作品图：
元件清单：
龙人宝贝车工具箱一套
DM-S53401 QTI边线检测传感器
TCS230颜色传感器
DM-S32408 XBee-PRO ZB无线射频模块
以及电路常用元件
六、结论与心得
本次项目实验过程中，遇到很多困难，但是同时也锻炼了我们发现问题，分析问题近而解决问题的能力，无论是编程还是硬件搭建均有大幅度提升，增加了对项目实习等地兴趣，同时也学到了很多专业知识，虽然还不是很全面，但至少可以用来解决当前遇到的问题，是一次不错的项目，我等从中获益匪浅……七、参考文献
龙人宝贝车工具箱一套使用手册
DM-S53401 QTI边线检测传感器使用手册
TCS230颜色传感器使用手册
DM-S32408 XBee-PRO ZB无线射频模块使用手册
智能传感器应用项目教程--基于教育机器人的设计与实现
基础机器人制作与编程(第2版)
现代传感器技术及应用
八、程序清单
A车程序：
' {$STAMP BS2}
' {$PBASIC 2.5}
'---------------颜色传感器变量--------------------------------------- S0 PIN 4
S1 PIN 5
S2 PIN 6
S3 PIN 7
OUT PIN 8
LED PIN 9
pRED CON 12 'Red reading period.
pGREEN CON 10 'Green reading period.
pBLUE CON 8 'Blue reading period.
RED VAR Word 'Red color reading.
GREEN VAR Word 'Green color reading.
BLUE VAR Word 'Blue color reading.
CurrentColor VAR Byte
'----------------无线模块变量---------------------------------------- TX PIN 10 '发送引脚
RX PIN 11 '接收引脚
Baud CON 84 '波特率是9600 9600-0-1
send VAR Byte
recive VAR Byte
'-----------QTI传感器变量---------------------------------------- qtis VAR Nib ' qti black/white states
'---------------次数变量-----------------------------------------
i VAR Word
j VAR Byte
'-----------角度调用中间变量-------------------------------------
l VAR Byte
'------------------主程序------------------------------------------ GOSUB a1
FOR i= 1 TO 10
PULSOUT 12,700
PULSOUT 13,800
PAUSE 20
NEXT
GOSUB a7
GOSUB achufa
FOR j= 1 TO 5
l=j
GOSUB qti190
GOSUB ajiaodu
GOSUB azhuace
IF CurrentColor=j THEN '搬运的颜色和对应路径相同
GOSUB xiangtong
ELSEIF CurrentColor<j THEN '搬运的颜色小于对应的路径
GOSUB xiaoyu
ELSEIF CurrentColor>j THEN '搬运的颜色大于对应的路径
GOSUB dayu
ENDIF
NEXT
'-----------------------搬运完成，回到起点--------------------------- send= 14 '发出14的信号后回起点
FOR i =1 TO 10
SEROUT TX,Baud,[DEC send,CR]
PAUSE 10
NEXT
GOSUB qti190
GOSUB a7
GOSUB ahuiqidian
FOR i = 1 TO 40
PULSOUT 12, 650
PULSOUT 13, 850
PAUSE 20
NEXT
GOSUB a1
GOSUB a1
'-----------程序结束--------------------------------------
END
'------------------------颜色和路径相同------------------------------ xiangtong:
GOSUB azhizou8
IF CurrentColor=1 THEN
GOSUB ahoutui180
ELSE
l=j+4
GOSUB ahoutui180hui
GOSUB ajiaodu
GOSUB ahui180
ENDIF
RETURN
'-------------颜色小于路径---------------------------------
xiaoyu:
l=j+5-CurrentColor
GOSUB a180comeback
GOSUB ajiaodu
GOSUB asong
IF CurrentColor=1 THEN
GOSUB ahoutui180
ELSE
GOSUB alduiying
GOSUB ahoutui180hui
GOSUB ajiaodu
GOSUB ahui180
ENDIF
RETURN
'------------------颜色大于路径--------------------------------- dayu:
send=CurrentColor+5
FOR i =1 TO 10 '循环发送路径给B车
SEROUT TX,Baud,[DEC send,CR]
PAUSE 10
NEXT
DO UNTIL recive=11 '接收状态，直到B车完成清理 11表示清理完成
SERIN RX,Baud,[DEC recive]
PAUSE 20
LOOP
l=CurrentColor-j+1
GOSUB a180comeback
GOSUB ajiaodu
GOSUB asong
GOSUB alduiying
GOSUB ahoutui180hui
GOSUB ajiaodu
GOSUB ahui180
send= 12 '发送搬运完成的信号 12表示搬运完成
FOR i= 1 TO 10
SEROUT TX,Baud,[DEC send,CR]
PAUSE 10
NEXT
DO UNTIL recive=13 '直到接收B车回到X的信号才继续搬运
SERIN RX,Baud,[DEC recive]
PAUSE 20
LOOP
RETURN
'----------a车出发点到 O 点----------------------------------- achufa:
FOR i = 1 TO 30
PULSOUT 12, 650
PULSOUT 13, 850
PAUSE 20
NEXT
GOSUB qti190
RETURN
'-----------------a车抓木块和测颜色--------------------------------- azhuace:
GOSUB qti190
GOSUB Check_Color:
RETURN
'-----------------a车转180后去O点--------------------------------- a180comeback:
OUTA = %1111
FOR i=1 TO 40
PULSOUT 12,680
PULSOUT 13,680
PAUSE 20
NEXT
GOSUB qti190
RETURN
'------------a车从O点送木块------------------------------------ asong:
OUTA = %1111
FOR i =1 TO 285
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 850
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 750
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE %0000
PULSOUT 13, 850
PULSOUT 12, 650
CASE %1111
EXIT
CASE %0110
EXIT
CASE ELSE
PAUSE 3
ENDSELECT
NEXT
RETURN
'--------------a车后退、转180、回O点---------------------------------- ahoutui180hui:
OUTA = %1111
FOR i=1 TO 50
PULSOUT 12,810
PULSOUT 13,690
PAUSE 20
NEXT
FOR i=1 TO 40
PULSOUT 12,680
PULSOUT 13,680
PAUSE 20
NEXT
GOSUB qti190
RETURN
'---------------a车回1点后转180------------------------------------ ahui180:
GOSUB qti190
FOR i=1 TO 40
PULSOUT 12,680
PULSOUT 13,680
PAUSE 20
NEXT
RETURN
'-----------a车回起点------------------------------------------- ahuiqidian:
OUTA = %1111
GOSUB qti190
GOSUB qti190
RETURN
'------a车后退转180--------------------------------------------- ahoutui180:
OUTA = %1111
FOR i=1 TO 50
PULSOUT 12,810
PULSOUT 13,690
PAUSE 20
NEXT
FOR i=1 TO 40
PULSOUT 12,680
PULSOUT 13,680
PAUSE 20
NEXT
RETURN
'-----颜色对应的子函数------------------------------------------- alduiying:
IF CurrentColor=2 THEN
l=6
ELSEIF CurrentColor=3 THEN
l=7
ELSEIF CurrentColor=4 THEN
l=8
ELSEIF CurrentColor=5 THEN
l=9
ENDIF
RETURN
'---------调用角度子函数--------------------------------------------- ajiaodu:
IF l=1 THEN
GOSUB a1
ELSEIF l=2 THEN
GOSUB a2
ELSEIF l=3 THEN
GOSUB a3
ELSEIF l=4 THEN
GOSUB a4
ELSEIF l=5 THEN
GOSUB a5
ELSEIF l=6 THEN
GOSUB a6
ELSEIF l=7 THEN
GOSUB a7
ELSEIF l=8 THEN
GOSUB a8
ELSEIF l=9 THEN
GOSUB a9
ENDIF
RETURN
'----------------左转90--------------------------------------------- a1:
FOR i=1 TO 8
PULSOUT 12,680
PULSOUT 13,830
PAUSE 20
NEXT
FOR i=1 TO 33
PULSOUT 12,660
PULSOUT 13,740
PAUSE 20
NEXT
RETURN
'----------------左转135-------------------------------------------- a2:
FOR i= 1 TO 10
PULSOUT 12,680
PULSOUT 13,820
PAUSE 20
NEXT
FOR i=1 TO 28
PULSOUT 12,650
PULSOUT 13,710
PAUSE 20
NEXT
RETURN
'----------------左转90--------------------------------------------- a3:
FOR i=1 TO 8
PULSOUT 12,680
PULSOUT 13,830
PAUSE 20
NEXT
FOR i=1 TO 33
PULSOUT 12,660
PULSOUT 13,740
PAUSE 20
NEXT
RETURN
'----------------左转45--------------------------------------------- a4:
FOR i=1 TO 10
PULSOUT 12,680
PULSOUT 13,830
PAUSE 20
FOR i=1 TO 20
PULSOUT 12,650
PULSOUT 13,760
PAUSE 20
NEXT
RETURN
'----------------直走5cm-------------------------------------------- a5:
FOR i=1 TO 20
PULSOUT 12,650
PULSOUT 13,850
PAUSE 20
NEXT
RETURN
'----------------右转135-------------------------------------------- a6:
FOR i=1 TO 11
PULSOUT 12,650
PULSOUT 13,850
PAUSE 20
NEXT
FOR i=1 TO 45
PULSOUT 12,770
PULSOUT 13,850
PAUSE 20
NEXT
RETURN
'----------------右转90---------------------------------------------
FOR i=1 TO 35
PULSOUT 12,750
PULSOUT 13,850
PAUSE 20
NEXT
RETURN
'----------------右转45-------------------------------------------- a8:
FOR I=1 TO 35
PULSOUT 12,730
PULSOUT 13,850
PAUSE 20
NEXT
RETURN
'----------------直走5cm-------------------------------------------- a9:
FOR i=1 TO 20
PULSOUT 12,650
PULSOUT 13,850
PAUSE 20
NEXT
RETURN
'----------------直走8cm-------------------------------------------- azhizou8:
FOR i=1 TO 27
PULSOUT 12,650
PULSOUT 13,850
PAUSE 20
RETURN
'---------------------QTI-------------------------------------------- qti190:
OUTA = %1111
FOR i =1 TO 190
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 850
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 750
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE %0111
PULSOUT 13, 780
PULSOUT 12, 720
CASE %1110
PULSOUT 13, 780
PULSOUT 12, 720
CASE %1111
EXIT
CASE %0000
PULSOUT 13, 850
PULSOUT 12, 650
CASE ELSE
PAUSE 3
ENDSELECT
NEXT
RETURN
'---------------颜色传感器检测--------------------------------------- Check_Color:
HIGH S0
HIGH S1
HIGH LED
PAUSE 500
LOW S2
LOW S3
COUNT OUT, pRED, RED
HIGH S3
COUNT OUT, pBLUE, BLUE
HIGH S2
COUNT OUT, pGREEN, GREEN
IF RED>180 AND GREEN>120 AND BLUE<80 THEN
CurrentColor=1 'yellow
ELSEIF RED>180 AND GREEN>180 AND BLUE>180 THEN
CurrentColor=2 'white
ELSEIF RED>90 AND GREEN<50 AND BLUE<50 THEN
CurrentColor=3 'RED
ELSEIF RED<20 AND GREEN<20 AND BLUE<20 THEN
CurrentColor=4 'black
ELSEIF RED<30 AND GREEN<30 AND BLUE<60 THEN
CurrentColor=5 'bule
ENDIF
LOW S0
LOW S1
LOW LED
RETURN
'-------------------QTI测试---------------------------------------- Check_Qtis:
DIRA = %1111
PAUSE 0
DIRA = %0000
PAUSE 0
qtis = INA
RETURN
B车程序：
' {$STAMP BS2}
' {$PBASIC 2.5}
a VAR Word
i VAR Word
recieve VAR Byte
send VAR Byte
angle VAR Byte
times VAR Byte
RX PIN 11 '接收引脚
TX PIN 10 '发送引脚
Baud CON 84 '波特率是9600 9600-0-1
qtis VAR Nib 'qti巡线
'-------------------主程序--------------------------------
'---------------------------------------------------------
recieve=0 '初始化发射接收的变量
send=0
GOSUB subsetout '出发
FOR i=1 TO 10
recieve=0
DO UNTIL recieve>6 '收到清理木块的信号
SERIN RX,Baud,[DEC recieve]
LOOP
angle=recieve '将接收到的变量存储在变量angle中
IF recieve=14 THEN EXIT '当收到recieve=14（A车已经完成搬运），
跳出for...next循环
GOSUB subhead
SELECT recieve
CASE 7 '需清理B点坐在路径
GOSUB subheadLT45
CASE 8 '需清理C点坐在路径
GOSUB subheadT0
CASE 9 '需清理D点坐在路径
GOSUB subheadRT45
CASE 10 '需清理E点坐在路径
GOSUB subheadRT90
ENDSELECT
GOSUB subsweepwood '清理木块
send=11 '清理木块后，告诉A车（send=11，代表已清理完成，可进行搬运）
FOR times=1 TO 10
SEROUT TX,Baud,[DEC send,CR]
NEXT
DO UNTIL recieve=12 '接收到A车已搬运完成的信号（recieve=12，代表A车已清理完成）
SERIN RX,Baud,[DEC recieve]
LOOP
GOSUB subreturnwood '将木块归位
SELECT angle '返回O点并摆正
CASE 7
GOSUB subbackLT45
CASE 8
GOSUB subbackT0
CASE 9
GOSUB subbackRT45
CASE 10
GOSUB subbackRT90
ENDSELECT
GOSUB subreturnX '车回X点
send=13
FOR times=1 TO 10
SEROUT TX,Baud,[DEC send,CR]
NEXT
NEXT
GOSUB subreturn
END
'--------------------------------------------------------- '~~~~~~~~~~~~~从起点出发到X点~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ subsetout:
FOR a=1 TO 40 '右转90°
PULSOUT 12, 750
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 25 '直接前进
PULSOUT 12, 655
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 36 '左转90°
PULSOUT 12, 650
PULSOUT 13, 750
PAUSE 20
NEXT
FOR a=1 TO 25 '直接前进 PULSOUT 12, 655
PULSOUT 13, 820
PAUSE 20
NEXT
OUTA = %1111
FOR a=1 TO 150
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 820
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 760
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE ELSE
PAUSE 1
ENDSELECT
NEXT
RETURN
'~~~~~~~~~~~~~从X点出发一段距离到中心点O~~~~~~~~~~~~~~~~~ subhead:
OUTA = %1111
FOR a=1 TO 270
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 820
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 760
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE %1111
EXIT
CASE ELSE
PAUSE 3
ENDSELECT
NEXT
RETURN
'~~~~~~~~~~~~~清理木块~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ subsweepwood:
'QTI传感器巡线一段距离
OUTA = %1111
FOR a=1 TO 70
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 820
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 760
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE ELSE
PAUSE 3
ENDSELECT
NEXT
'携带木块进行操作
FOR a=1 TO 35 '转弯30~45的角度
PULSOUT 12, 735
PULSOUT 13, 850
PAUSE 20
NEXT
FOR a=1 TO 70 '清理木块前进一段距离 PULSOUT 12, 650
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 35 '转动约180°的弯
PULSOUT 12, 650
PULSOUT 13, 650
PAUSE 20
NEXT
RETURN
'~~~~~~~~~~~送回木块~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ subreturnwood:
FOR a=1 TO 52 '前进一段距离，将木块归位
PULSOUT 12, 650
PULSOUT 13, 820
PAUSE 20
NEXT
'不携带物体
FOR a=1 TO 40 '后退一段距离
PULSOUT 12, 840
PULSOUT 13, 650
PAUSE 20
NEXT
FOR a=1 TO 10 '左转小角度
PULSOUT 12, 650
PULSOUT 13, 750
PAUSE 20
NEXT
FOR a=1 TO 85 '前进一段距离，回到o点
PULSOUT 12, 650
PULSOUT 13, 820
PAUSE 20
NEXT
RETURN
'~~~~~~~~~~前进转弯~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
subheadT0:
FOR a=1 TO 18 '直接前进
PULSOUT 12, 650
PULSOUT 13, 820
PAUSE 20
NEXT
RETURN
subheadLT45:
FOR a=1 TO 30
PULSOUT 12, 650 '左转45°
PULSOUT 13, 770
PAUSE 20
NEXT
RETURN
subheadRT45:
FOR a=1 TO 30 '右转45°
PULSOUT 12, 735
PULSOUT 13, 850
PAUSE 20
NEXT
RETURN
subheadRT90:
FOR a=1 TO 37 '右转90°
PULSOUT 12, 750
PULSOUT 13, 820
PAUSE 20
NEXT
RETURN
'~~~~~~~~~~返回转弯~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
subbackLT45: 'B点送回木块后的角度接巡线前
FOR a=1 TO 25
PULSOUT 12, 670
PULSOUT 13, 790
PAUSE 20
NEXT
RETURN
subbackT0:
FOR a=1 TO 36 'C点送回木块后的角度接巡线前 ' PULSOUT 12, 650
PULSOUT 13, 770
PAUSE 20
NEXT
RETURN
subbackRT45: 'D点送回木块后的角度，接巡线前
FOR a=1 TO 33
PULSOUT 12, 650
PULSOUT 13, 760
PAUSE 20
NEXT
RETURN
subbackRT90:
FOR a=1 TO 50 'E点送回木块后的角度，接巡线前
PULSOUT 12, 650
PULSOUT 13, 750
PAUSE 20
NEXT
RETURN
subreturnX:
OUTA = %1111
FOR a=1 TO 240
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650 PULSOUT 12, 650 CASE %1100
PULSOUT 13, 750 PULSOUT 12, 650 CASE %0100
PULSOUT 13, 800 PULSOUT 12, 650 CASE %0110
PULSOUT 13, 820 PULSOUT 12, 650 CASE %0010
PULSOUT 13, 850 PULSOUT 12, 700 CASE %0011
PULSOUT 13, 850 PULSOUT 12, 760 CASE %0001
PULSOUT 13, 850 PULSOUT 12, 850 CASE ELSE
PAUSE 1
ENDSELECT
NEXT
FOR a=1 TO 40 '转动约180°的弯
PULSOUT 12, 650
PULSOUT 13, 650
PAUSE 20
NEXT
RETURN
'~~~~~~~~~~~~~~~~~~~返回起点~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ subreturn:
FOR a=1 TO 40 '转动约180°的弯
PULSOUT 12, 650
PULSOUT 13, 650
PAUSE 20
NEXT
OUTA = %1111
FOR a=1 TO 150
GOSUB Check_Qtis
SELECT qtis
CASE %1000
PULSOUT 13, 650
PULSOUT 12, 650
CASE %1100
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110
PULSOUT 13, 820
PULSOUT 12, 650
CASE %0010
PULSOUT 13, 850
PULSOUT 12, 700
CASE %0011
PULSOUT 13, 850
PULSOUT 12, 760
CASE %0001
PULSOUT 13, 850
PULSOUT 12, 850
CASE %1111
EXIT
CASE ELSE
PAUSE 1
ENDSELECT
NEXT
FOR a=1 TO 50 '直接前进 PULSOUT 12, 655
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 40 '右转90° PULSOUT 12, 750
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 25 '直接前进 PULSOUT 12, 655
PULSOUT 13, 820
PAUSE 20
NEXT
FOR a=1 TO 40 '右转90° PULSOUT 12, 750
PULSOUT 13, 820
PAUSE 20
NEXT
RETURN
Check_Qtis:
DIRA = %1111
PAUSE 0
DIRA = %0000
PAUSE 0
' PULSOUT UnusedPin, 0
qtis = INA
RETURN。