; очень сырой вариант программы! ; использовать только в учебных целях! ; robo.com.ua 2004 (C) .include "2313def.inc" ; установки для мотора, плата управления ; двигателями на основе 3-х реле .EQU go_f=0b0000001 ; оба вперед .EQU go_b=0b0000111 ; оба назад .EQU go_r=0b0000101 ; правый вперед .EQU go_l=0b0000011 ; левый вперед .EQU MOTORDDR=DDRB .EQU MOTORPORT=PORTB ; устанавливаем константы ; 4 MHz ;.EQU wait_motor=2 ;.EQU motor_bcount=30 ; 8 MHz .EQU wait_motor=4 .EQU motor_bcount=60 ; используем регистры для хранения информации .DEF reg_motor=R24 .DEF reg_count=R25 ; сенсоры пола .EQU botsensor1=0 ; номер ножки .EQU botsensor2=1 ; .EQU botsensorreg=PIND ; регистр ВВ .EQU wait_bs=$10 ; константа для отъезда при срабатывании ; сенсора пола ; ИК-бампер .EQU dsimpulses=10 ; ипмульсов ; 4 MHz ;.EQU dswait1=19 ; константы задержки ;.EQU dswait2=23 ; ;.EQU dswait3=100 ; задержка между пачками ; 8 MHz .EQU dswait1=38 .EQU dswait2=46 .EQU dswait3=200 .EQU dsport=PORTD ; порт для чтения .EQU dsddr=DDRD ; настроить порт .EQU dspin=PIND ; чтение данных .EQU dspinin=5 ; вход .EQU dspinout=4 ; выход .EQU wait_ds=$10 ; константа для отъезда от препятствия ; (сработка ИК-бампера) ; сенсор одометрии .EQU odport=PORTD .EQU odddr=DDRD .EQU odpin=PIND .EQU odpinin=0 .EQU odconst1=5 .EQU odconst2=1 .DSEG od_wait1: .BYTE 1 ; ячейки используются для od_wait2: .BYTE 1 ; подсчета задержки od_tip: .BYTE 1 ; в каком положении сенсор (1/0) .CSEG ; временные регистры .DEF temp=R30 .DEF temp1=R29 .EQU wait_init=$2A ;==================================== ; I N I T ;==================================== ; готовим порт В для вывода ldi r30,$FF out DDRB,r30 ; готовим порт D ввода-вывода - ldi r30,$00 out DDRD,r30 ; включаем резисторы- на входе будет 1 ; ldi r30,$7F ; out PORTD,r30 ; инициализация стека ldi R30,RAMEND out SPL,R30 ; Инициализация ИК-бампера cbi dsddr, dspinin ;TSOP sbi dsport, dspinin ;подтяжка sbi dsddr, dspinout ; Инициализация датчика одометрии cbi odddr, odpinin ;odometry init ldi temp, odconst1 sts od_wait1, temp ldi temp, odconst2 sts od_wait2, temp ;==================================== start: ; небольшая задержка ldi r28,2 ; wait rcall s_wait rjmp motor_end; dec reg_count brne l_odometry ; перейти к опросу датчика края ; 9 cpi reg_motor,9 ; fight brne l0 ldi temp,go_f out MOTORPORT,temp rjmp motor_end l0: ; find cpi reg_motor,2 ; 2,1- find brne l01 dec reg_motor ldi temp,go_l out MOTORPORT,temp ldi reg_count, motor_bcount rjmp motor_end l01: ; было 1 стало 2, движение вперед cpi reg_motor,1 ; 1- find brne l1 ldi temp,go_f out MOTORPORT,temp ldi reg_count, motor_bcount ldi reg_motor, 2 rjmp motor_end l1: cpi reg_motor,5 brne l2 ldi reg_motor, 2 ldi temp,go_f out MOTORPORT,temp ldi reg_count, motor_bcount rjmp motor_end l2: cpi reg_motor,6 brne l3 ldi reg_motor, 2 ldi temp,go_r out MOTORPORT,temp ldi reg_count, motor_bcount rjmp motor_end l3: cpi reg_motor,7 brne motor_end ldi reg_motor, 2 ldi temp,go_l out MOTORPORT,temp ldi reg_count, motor_bcount motor_end: ; end motor ;odometry sensor l_odometry: lds temp1, od_tip in temp, odpin ; прочитать порт Д sbrs temp, odpinin ; если установлен сенсор rjmp l_ssensor2 ; 0 ; 1 cpi temp1,1 breq l_ssensor3 ldi temp,1 sts od_tip, temp ; save 1 rjmp l_ssensor4 l_ssensor2: cpi temp1,0 breq l_ssensor3 ;no ; 0 clr temp sts od_tip, temp ; save 0 rjmp l_ssensor4 l_ssensor3: lds temp, od_wait1 ; младшая часть cpi temp,0 brne l_ssensor31 ldi temp, odconst1 ; частота - mладшая часть sts od_wait1, temp ; сохранить младший байт lds temp, od_wait2 ; старшая часть cpi temp,0 brne l_ssensor32 rjmp l_back ; both 0 l_ssensor31: dec temp sts od_wait1, temp ; сохранить младший байт rjmp l_ssensor5 l_ssensor32: dec temp sts od_wait2, temp ; сохранить st байт rjmp l_ssensor5 l_ssensor4: ; init ldi temp, odconst1 sts od_wait1, temp ldi temp, odconst2 sts od_wait2, temp l_ssensor5: ;----- odometry sensor end ; bottom sensor ; проверять датчик ; не сработал- перейти к проверке bs_test: ; Датчик пола отключен- робот бегает вне ринга rjmp bs_end sbic botsensorreg, botsensor1 rjmp bs1 sbic botsensorreg, botsensor2 rjmp bs2 rjmp bs_end ; сработал- сделать отъезд bs1: ldi temp,go_b ; back out MOTORPORT,temp ldi r28,wait_bs ; wait rcall s_wait ldi r30,go_r ; right out MOTORPORT,r30 ldi r28,wait_bs*2 ; wait rcall s_wait ldi r30,0 ; stop out MOTORPORT,r30 ldi r28,wait_bs ; wait rcall s_wait ldi temp,go_f out MOTORPORT,temp ldi reg_motor, 2 ldi reg_count, motor_bcount rjmp bs_end bs2: ldi temp,go_b out MOTORPORT,temp ldi r28,wait_bs ; wait rcall s_wait ldi r30,go_l out MOTORPORT,r30 ldi r28,wait_bs*2 ; wait rcall s_wait ldi r30,0 ; stop out MOTORPORT,r30 ldi r28,wait_bs ; wait rcall s_wait ldi temp,go_f out MOTORPORT,temp ldi reg_motor, 2 ldi reg_count, motor_bcount rjmp bs_end bs_end: ; distance sensor ; сформировать пачку из N импульсов ; если да- установить регистр борьбы reg_motor ds_start: ldi temp1,dsimpulses ds00: cbi dsport, dspinout ldi temp, dswait1 dsDelay12: dec temp brne dsDelay12 sbi dsport, dspinout ldi temp, dswait1 dsDelay22: dec temp brne dsDelay22 dec temp1 brne ds00 ds05: ; проверить приход sbic dspin, dspinin rjmp ds99 rjmp l_back ldi reg_motor,9 ; fight ldi reg_count,motor_bcount ; motor count rjmp ds99 ds02: cpi reg_motor, 9 brne ds99 ldi reg_motor,2 ; find ldi reg_count,motor_bcount ; motor count rjmp ds99 ; небольшая задержка, чтобы успел ИК-датчик перевести состояние в 1 ds99: ldi r26,$AF dsloop1: dec r26 nop nop nop cpi r26,0 brne dsloop1 rjmp start ; участок отъезда l_back: ldi temp,go_b out MOTORPORT,temp ldi r28,wait_ds ; wait rcall s_wait ldi r30,go_l out MOTORPORT,r30 ldi r28,wait_ds ; wait rcall s_wait ldi r30,0 ; stop out MOTORPORT,r30 ldi r28,wait_ds ; wait rcall s_wait ldi temp,go_f out MOTORPORT,temp ldi reg_motor, 2 ldi reg_count, motor_bcount ; odometry ldi temp, odconst1 sts od_wait1, temp ldi temp, odconst2 sts od_wait2, temp rjmp start ; конец основной программы ;===================================================================== ;===================================================================== ; подпрограмма задержки s_wait: nop ;ret ; отладка- лучше отключить s_wait1: ldi r27,$20 s_wait2: ldi r26,$ff s_wait3: dec r26 nop nop nop cpi r26,0 brne s_wait3 dec r27 cpi r27,0 brne s_wait2 dec r28 cpi r28,0 brne s_wait1 ret .exit ; все, дальше ничего не будет