双龙SL积木式智能机器人的寻迹程序说明_积木式机器人_寻迹

工控网首页

应用设计

双龙SL积木式智能机器人的寻迹程序说明

nielin001

2013/9/5 10:44:03

;程序说明：程序演示了双龙积木式机器人的寻迹功能

; 通电后，运行 PA 口跑马灯程序

; 声控起动后，运行寻迹车程序，白底黑线，探测黑线，沿黑线运动

;DIY02-06 组成寻迹机器人探测信号的说明

;探测白底黑线： 0 表示有反光，探测到白底；1 表示无反光，探测到黑线

;PC7 PC6 PC5 编码(十进制) 机器人的相应动作

; 0 0 0 0x00 (0) 直行

; 0 0 1 0x20 (32) 往右转弯

; 0 1 0 0x40 (64) 直行

; 0 1 1 0x60 (96) 往右转弯

; 1 0 0 0x80 (128) 往左转弯

; 1 0 1 0xA0 (160) 直行

; 1 1 0 0xC0 (192) 往左转弯

; 1 1 1 0xE0 (224) 前行，再探测，连续长时间后，机器人停止待命

;若器件配置文件选"8535def.inc" 编译后适用于 SLDIY02-03 + SLDIY02-06 组成的积木式机器人

;若器件配置文件选"8515def.inc" 编译后适用于 SLDIY02-01 + SLDIY02-06 组成的积木式机器人

;更新日期：2003-07-15

.include "e:\program\include\8535def.inc" ;器件配置文件

;.include "e:\program\include\8515def.inc" ;器件配置文件

.equ t0_times = $72

.equ mic_in = $75

.equ cny_in = $77

.equ cny_times = $78

.equ cny_delayms = $84

.equ timers = $88

;******************************************************************

.cseg

.org $000

rjmp cny_car ;跳过中断区,跳转到寻迹机器人运行程序

.org OVF0addr

rjmp timer0_ovf_isr ;定时器0溢出中断处理程序

;******************************************************************

.org $020

cny_car: ;寻迹机器人运行程序

ldi r16,low(ramend) ;栈指针初始化，SP 指向片内SRAM的末地址

ldi r17,high(ramend)

out spl,r16

out sph,r17

ldi r29,$02 ;设置 Y=R29+R28 -> $0200

ldi r28,$00

rcall port_init ;调用芯片端口初始化子程序

clr r16 ;参数初始化为0

sts t0_times,r16 ;t0_times = 0

sts timers,r16 ;timers = 0

sts timers+1,r16

rcall timer0_init ;调用定时器0初始化子程序

clr R2

out MCUCR,R2 ;

out GIMSK,R2 ;

in R24,TIMSK ;

ori R24,(1<<TOIE0) ;

out TIMSK,R24 ;允许定时器0溢出中断

cnycar00:

sei ;允许中断

rcall mic_startup ;调用声控起动子程序，等待声控起动

cli

cnycar01:

ldi R24,50 ;设置延时时间常数

sts cny_delayms,R24 ;对于不同的路线可改变时间常数，使机器人寻迹效果更好

cnycar02:

in R24,PINC ;读取PC口管脚的状态

andi R24,224 ;保留PC7,PC6,PC5的状态

sts cny_in,R24 ;作为探测器检测到的信号（cny_in）

tst R24

breq cnycar03 ;cny_in = 0 ，跳转到 cnycar03

cpi R24,64

breq cnycar03 ;cny_in = 64 ，跳转到 cnycar03

cpi R24,160

brne cnycar04 ;cny_in != 160 ，跳转到 cnycar04

cnycar03: ;0,64,160 ---- 直行

rcall forward ;调用机器人向前行进的子程序

clr R2

sts cny_times,R2 ;cny_times = 0

cnycar04:

lds R24,cny_in

cpi R24,32

breq cnycar05 ;cny_in = 32 ，跳转到 cnycar05

cpi R24,96

brne cnycar06 ;cny_in != 96 ，跳转到 cnycar06

cnycar05: ;32,96 ---- 往右转弯

rcall turn_right_s ;调用机器人慢速右转的子程序

lds R16,cny_delayms ;装入延时时间

clr R17

rcall delay_ms ;调用延时子程

clr R2

sts cny_times,R2 ;cny_times = 0

cnycar06:

lds R24,cny_in

cpi R24,128

breq cnycar07 ;cny_in = 128 ，跳转到 cnycar07

cpi R24,192

brne cnycar08 ;cny_in != 192 ，跳转到 cnycar08

cnycar07: ;128,192 ---- 往左转弯

rcall turn_left_s ;调用机器人慢速左转的子程序

lds R16,cny_delayms ;装入延时时间

clr R17

rcall delay_ms ;调用延时子程

clr R2

sts cny_times,R2 ;cny_times = 0

cnycar08:

lds R24,cny_in

cpi R24,224

brne cnycar02 ;cny_in != 224 ，跳转到 cnycar02

rcall forward ;cny_in = 224，调用前行子程序

lds R24,cny_times

subi R24,255 ; cny_times = cny_times + 1

sts cny_times,R24

ldi R16,10 ;设置延时时间

ldi R17,0

rcall delay_ms ;调用延时子程序

lds R24,cny_times

cpi R24,100

brne cnycar02 ;cny_times != 100 ，跳转到 cnycar02

rcall stop ;cny_times = 100 ，机器人停止待命

rjmp cnycar00 ;等待声控重新起动寻迹机器人程序

;*******************************************************************************

; TIMER0 1024 分频 25ms 定时

timer0_init: ;定时器0初始化子程序

clr R2

out TCCR0,R2 ;

ldi R24,61

out TCNT0,R24 ;设置计数初值

ldi R24,5

out TCCR0,R24 ;1024分频

ret

;*******************************************************************************

timer0_ovf_isr: ;定时器0溢出中断执行程序

push r31 ;对寄存器做必要的保护,数据入堆栈保护

push r30

push r25

push r24

push r21

push r20

push r17

push r16

in r2,SREG

push r2 ;保护状态寄存器

ldi R24,61 ;重装定时器0的计数初值

out TCNT0,R24

clr r17

lds r16,t0_times

inc r16 ;t0_times = t0_times + 1

sts t0_times,r16

cpi r16,10 ;t0_times 的值是否等于 10

brne ovf0_end ;t0_times != 10 不等，不做其它处理，中断返回

sts t0_times,r17 ;t0_times = 10，令t0_times = 0

lds r24,timers

lds r25,timers+1

inc r24 ;timers = timers + 1

adc r25,r17

sts timers+1,r25

sts timers,r24

ldi r30,low(led_data*2) ;装入led_data（LED广告灯数据表）的首址到 Z

ldi r31,high(led_data*2)

add r30,r24 ;首址加上偏移量

adc r31,r25

lpm ;查表得到数据

mov r16,r0

out porta,r16 ;数据由 PORTA 输出

cpi r16,$0a ;看是否到表的末尾

brne ovf0_end ;没有到末尾，不做处理，中断返回

clr r24 ;到了末尾，清除 timers

clr r25

sts timers+1,r25

sts timers,r24

ovf0_end:

pop r2

out SREG,R2 ;恢复状态寄存器

pop r16

pop r17

pop r20

pop r21

pop r24

pop r25

pop r30

pop r31

reti ;中断返回

;******************************************************************

port_init: ;芯片端口初始化程序

ldi R24,255

out DDRA,R24

out PORTA,R24 ;PA口输出，输出初值为全1

clr R2

out DDRB,R2

out PORTB,R24 ;PB口输入，内部上拉

ldi R24,1

out DDRC,R24

ldi R24,255

out PORTC,R24 ;PC0输出，PC1~PC7带内部上拉输入

ldi R24,240

out DDRD,R24

ldi R24,255

out PORTD,R24 ;PD0~PD3带内部上拉输入，PD4~PD7输出驱动电机

ret

;******************************************************************

mic_startup: ;检测声音输入启动程序

in R24,PINB ;读入PB口管脚的状态

andi R24,4 ;分离出PB2脚（声音输入管脚）的状态 -> mic_in

sts mic_in,R24

tst R24 ;

brne mic_startup ;mic_in = 1 没有声音信号输入，循环检测

ret ;mic_in = 0 有声音信号输入，启动执行后面的程序

;******************************************************************

; time -> R16,R17

delay_us: ;微秒级延时程序，延时时间为 (R17:R16)

subi R16,1

sbci R17,0 ;(R17:R16) = (R17:R16) - 1

ldi R24,1

ldi R25,0

cp R24,R16

cpc R25,R17

brlt delay_us

ret

;******************************************************************

; time -> R20,R21

delay_ms: ;毫秒级延时程序，延时时间为 (R21:R20)

push r21

push r20

push r17

push r16

mov R20,R16 ;装入延时时间常数 (R21:R20) = (R17:R16)

mov R21,R17

rjmp delay_ms2

delay_ms1:

ldi R16,1000

ldi R17,3

rcall delay_us ;1ms延时

subi R20,1

sbci R21,0 ;(R21:R20) = (R21:R20) - 1

delay_ms2:

cpi R20,0 ;(R21:R20) = 0 延时时间到，延时子程返回

cpc R20,R21

brne delay_ms1 ;(R21:R20) != 0 延时时间未到，继续延时

pop r16

pop r17

pop r20

pop r21

ret ;子程序返回

;******************************************************************

turn_right: ;机器人右转子程

ldi R24,144

out PORTD,R24 ;输出电机控制信号，使机器人右转

ldi R24,204

out PORTA,R24 ;设置机器人右转时对应的指示灯状态

ret

;******************************************************************

turn_left: ;机器人左转子程

ldi R24,96

out PORTD,R24 ;输出电机控制信号，使机器人左转

ldi R24,51

out PORTA,R24 ;设置机器人左转时对应的指示灯状态

ret

;******************************************************************

turn_right_s: ;机器人慢速右转子程

ldi R24,128

out PORTD,R24 ;输出电机控制信号，使机器人慢速右转

ldi R24,207

out PORTA,R24 ;设置机器人慢速右转时对应的指示灯状态

ret

;******************************************************************

turn_left_s: ;机器人慢速左转子程

ldi R24,32

out PORTD,R24 ;输出电机控制信号，使机器人慢速左转

ldi R24,243

out PORTA,R24 ;设置机器人慢速左转时对应的指示灯状态

ret

;******************************************************************

backward: ;机器人向后倒退子程

ldi R24,80

out PORTD,R24 ;输出电机控制信号，使机器人向后倒退

ldi R24,60

out PORTA,R24 ;设置机器人向后倒退时对应的指示灯状态

ret

;******************************************************************

forward: ;机器人向前行进子程

ldi R24,160

out PORTD,R24 ;输出电机控制信号，使机器人向前行进

ldi R24,195

out PORTA,R24 ;设置机器人向前行进时对应的指示灯状态

ret

;******************************************************************

stop: ;机器人停止待命子程

ldi R24,255

out PORTD,R24 ;输出电机控制信号，使机器人停止待命

out PORTA,R24 ;设置机器人停止待命时对应的指示灯状态

ret

;**************************************************************************

;设置LED广告灯数据表

led_data:

.db 0xfe,0xfd,0xfb,0xf7,0xef,0xdf,0xbf,0x7f,0xbf,0xdf,0xef,0xf7,0xfb,0xfd

.db 0x00,0x18,0x3c,0x7e,0xff,0x7e,0x3c,0x18

.db 0xf8,0xf1,0xe3,0xc7,0x8f,0x1f,0x8f,0xc7,0xe3,0xf1

.db 0xfe,0xfc,0xf8,0xf0,0xe0,0xc0,0x80,0x00,0x80,0xc0,0xe0,0xf0,0xf8,0xfc

.db 0xff,0xe7,0xdb,0xbd,0x7e,0xbd,0xdb,0xe7

.db 0xff,0x00

.db 0x0a,0x0a

;***************************************************************************

投诉建议

提交

查看更多评论

其他资讯

自动化机床的故障排除技术浅析

安川焊接机器人编程

ABB机器人控制软件RobotWare应用手册SafeMove（英文）

ABB IRB7600 机器人维护信息

ABB IRC5P机器人培训教材