;------------------------------------------------------------------------------ ; EXPLOR1.ASM - Ultimo aggrn. 25-8-2002 ; ; (c) 2002, Claudio Fin ;------------------------------------------------------------------------------ ;------------------------------------------------------------------------------ ; Utilizzo porte ;------------------------------------------------------------------------------ ; RA0 <---- (Ain) Fotoresistenza ; RA1 <---- (Ain) Fotoresistenza ; RA2 <---- Sensori suono (PLL NE567) ; RA3 <---- (Ain) Sensore gas ; RA4 <---- Pulsante ; RA5 <---- Pulsante ; ; RB0 ----> Bit0 DAC ; RB1 ----> Bit1 DAC ; RB2 ----> Bit2 DAC ; RB3 ----> Bit3 DAC ; RB4 ----> Bit4 DAC ; RB5 ----> debug led (0=acceso 1=spento) / serial out ; RB6 ----> direzione motore sinistro (1=avanti 0=indietro) ; RB7 ----> direzione motore destro (1=avanti 0=indietro) ; ; RC0 ----> ; RC1 ----> CCP2 PWM motore destro (duty 100%=motore fermo) ; RC2 ----> CCP1 PWM motore sinistro (duty 100%=motore fermo) ; RC3 ----> ; RC4 ----> ; RC5 ----> ; RC6 ----> ; RC7 ----> ; ; |------------------------| ; RD0 <------------| 3x | ; RD1 <---- 2x 4x | Sensori ; RD2 <---- 1x 5x | IR ; RD3 <---- | ; RD4 <---- | ; RD5 <---- 6x 0x--------| ; RD6 <------------------| ; ; RD7 <---- Pulsante ; ; RE0 ----> Clock verso bussola ; RE1 ----> Dati verso bussola ; RE2 <---- Dati da bussola ;------------------------------------------------------------------------------ ; FLAGMOVE 0 1=Stop motori per evitare collisione ; FLAGMOVE 1 1=Ostacoli sulla destra ; FLAGMOVE 2 1=Ostacoli frontali ; FLAGMOVE 3 1=Ostacoli sulla sinistra ; FLAGMOVE 4 1=Ruotare a destra ; FLAGMOVE 5 1=Ruotare a sinistra ; FLAGMOVE 6 ; FLAGMOVE 7 ;------------------------------------------------------------------------------ ;------------------------------------------------------------------------------ ; Definizioni ;------------------------------------------------------------------------------ PROCESSOR 16F877 ; CLOCK 20Mhz RADIX DEC INCLUDE "P16F877.INC" INCLUDE "pwrasm.inc" __CONFIG 11111100111010B #define serout PORTB,5 #define oggdx flagmove,1 #define oggfron flagmove,2 #define oggsx flagmove,3 #define ruotadx flagmove,4 #define ruotasx flagmove,5 #define cmps_outck PORTE,0 #define cmps_outda PORTE,1 #define cmps_inda PORTE,2 ;valori comando pwm motori sxavanti EQU 37 dxavanti EQU 25 sxindietro EQU 31 dxindietro EQU 37 ;------------------------------------------------------------------------------ ; Variabili del programma (bytes usati: 27) ;------------------------------------------------------------------------------ ORG 20H contmov RES 1 ;conteggio cicli programma flagmove RES 1 flagmoveb RES 1 cmdmov RES 1 ;comando di movimento 0000=fermo 0001=avanti ;0010=indietro 0100=rot sx. 1000=rot dx ; Variabili per lettura bussola (4 bytes) cmps_ang RES 1 ;angolo misurato 0..255 cmps_cont RES 1 ;contatore bit cmps_error RES 1 ;0=ok 1=nok cmps_byte RES 1 ;byte trasm.o ricevuto ; Variabili per IR ranging (20 bytes) cont RES 1 ;usato in ranging flags2 RES 1 ;flag misurazione dac RES 1 ;valore da scrivere sul dac tmp_rang RES 1 ;variabile di lavoro task ranging trang RES 7 ;flags fasce di percorribilita' trang2 RES 7 ;tabella misure racont RES 2 ;conteggio per delay ; Variabili per trasmissione seriale (3 bytes) txdelay RES 1 txbyte RES 1 txcontabit RES 1 ;------------------------------------------------------------------------------ ; Inizio programma all'indirizzo 0. ;------------------------------------------------------------------------------ ORG 0 goto inizio ;------------------------------------------------------------------------------ ; Tabelle normalizzazione sensori IR in fasce di percorribilita' ;------------------------------------------------------------------------------ tabsensfro addwf PCL,f retlw 1 ;troppo vicino retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 ;ok retlw 2 retlw 2 retlw 8 ;nei paraggi retlw 8 retlw 8 retlw 8 tabsenssin addwf PCL,f retlw 1 ;troppo vicino retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 2 retlw 2 retlw 2 retlw 2 ;ok retlw 2 retlw 4 ;troppo lontano retlw 8 ;nei paraggi retlw 8 retlw 8 tabsensdes addwf PCL,f retlw 1 ;troppo vicino retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 1 retlw 2 retlw 2 retlw 2 ;ok retlw 2 retlw 4 ;troppo lontano retlw 8 ;nei paraggi retlw 8 ;------------------------------------------------------------------------------ ; Inizializzazione hardware ;------------------------------------------------------------------------------ inizio ;impostazione porte I/O Ldf PORTB,11100000B Ldf PORTC,00000110B Ldf PORTE,00000011B Bank1 clrf TRISB Ldf TRISC, 00000000B Ldf ADCON1,00000100B Ldf TRISE, 00000100B Bank0 Ldf PORTB,11100000B Ldf PORTC,00000110B Ldf PORTE,00000011B ;impostazione PWM Bank1 Ldf PR2,127 ;Imposta frequenza PWM Bank0 Ldf CCPR1L,128 ;Imposta PWM 100% Ldf CCPR2L,128 ;Imposta PWM 100% Ldf T2CON,00000110B movlw 00111100B iorwf CCP1CON,f iorwf CCP2CON,f ;impostazione clock TMR0 a 19531,25Hz Bank1 movlw 11010111B andwf OPTION_REG,f ;clock interno e prescaler assegnato a TMR0 movlw 00000111B iorwf OPTION_REG,f ;setta prescaler TMR0 a :256 Bank0 ;------------------------------------------------------------------------------ ; Inizializzazione variabili ;------------------------------------------------------------------------------ bsf flags2,7 ;azzera stato task ranging (1=first level) clrf flagmove ;------------------------------------------------------------------------------ ; Programma principale ; Avanza fino a segnale di stop (flagmove<0> settato), breve retromarcia, ; ruota nella direzione decisa dalla rete sensoriale fino a trovare campo ; libero davanti (flag oggfron=0). La rotazione avviene verso il lato ; piu' libero, se entrambi i lati sono occupati o liberi si ruota random ; leggendo il bit0 del TMR0 ;------------------------------------------------------------------------------ main bcf PORTB,5 ;accende LED Ldf contmov,40 ;attesa 2 secondi facendo il ranging main1 call ranging call normir call sensdir Djnz contmov,main1 bsf PORTB,5 ;spegne il LED main2 Ldf cmdmov,1 ;comando di avanti call ranging call normir call sensdir call taskmove Jnb flagmove,0,main2 ;se non ostacolo continua call retro ;altrimenti piccola retromarcia call ranging call normir call sensdir Skb ruotadx ;ruota nella direzione scelta call rotsx ;dalla rete di riconoscimento Skb ruotasx ;spazi percorribili call rotdx main3 call ranging call normir call sensdir Jb oggfron,main3 ;attende visuale libera call stopall goto main2 ; call readbussola ; Jb cmps_error,0,main ; call trasmetti ; goto main ;------------------------------------------------------------------------------ ; Lettura angolo dalla bussola in I2C simulato a software da PORTE ;------------------------------------------------------------------------------ readbussola clrf cmps_error call cmps_start ;invia start su bus i2c Ldf cmps_byte,11000000B call cmps_tx ;trasmette address bussola Jb cmps_error,0,cmps_fine ;se non ack goto fine call cmps_del2 Ldf cmps_byte,1 call cmps_tx ;trasmette nr.registro da leggere Jb cmps_error,0,cmps_fine ;se non ack goto fine call cmps_del2 call cmps_rstart ;invia repeated start su bus i2c Ldf cmps_byte,11000001B call cmps_tx ;trasmette address bussola OR 01H Jb cmps_error,0,cmps_fine ;se non ack goto fine call cmps_del2 call cmps_rx ;riceve lettura 1 Movff cmps_byte,cmps_ang call cmps_rx ;riceve e scarta lettura 2 call cmps_rx ;riceve e scarta lettura 3 cmps_fine call cmps_stop ;invia stop su bus i2c return cmps_rstart bsf cmps_outda call cmps_del bsf cmps_outck call cmps_del cmps_start bcf cmps_outda call cmps_del bcf cmps_outck call cmps_del return cmps_stop bsf cmps_outck call cmps_del bsf cmps_outda call cmps_del return cmps_tx Ldf cmps_cont,8 cmps_tx1 Movb cmps_byte,7,cmps_outda rlf cmps_byte,f call cmps_del bsf cmps_outck call cmps_del bcf cmps_outck call cmps_del Djnz cmps_cont,cmps_tx1 bsf cmps_outda call cmps_del Movb cmps_inda,cmps_error,0 bsf cmps_outck call cmps_del bcf cmps_outck call cmps_del return cmps_rx bsf cmps_outda call cmps_del Ldf cmps_cont,8 cmps_rx1 rlf cmps_byte,f Movb cmps_inda,cmps_byte,0 call cmps_del bsf cmps_outck call cmps_del bcf cmps_outck call cmps_del Djnz cmps_cont,cmps_rx1 bcf cmps_outda call cmps_del bsf cmps_outck call cmps_del bcf cmps_outck call cmps_del return cmps_del goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 return cmps_del2 Ldf cmps_cont,15 cmps_del22 call cmps_del Djnz cmps_cont,cmps_del22 return ;------------------------------------------------------------------------------ ; Task di ranging Per una misura completa va chiamato 21 volte. ; I livelli misurati vanno da 5 a 25. I valori vengono posti nei 7 bytes ; di trang2. ; Se non si riceve il segnale riflesso il valore e' 32. ; Una chiamata dura circa 2.5mS (la misura completa avviene in 52.5mS) ;------------------------------------------------------------------------------ ranging Ldf cont,21 pippo call rang Djnz cont,pippo ret rang Jnb flags2,7,ranging1 ranging0 Ldf dac,5 ;valore di partenza dac clrf flags2 ;imposta over range flags ranging1 movf dac,w iorwf PORTB,f ;scrive il valore dac Delay16 racont,1,94 ;630.6µS movf PORTD,w xorlw 11111111B movwf tmp_rang movlw 11100000B ;azzera DAC andwf PORTB,f Delay16 racont,1,94 ;630.6µS movf PORTD,w andwf tmp_rang,f movf dac,w iorwf PORTB,f ;riscrive valore DAC Delay16 racont,1,94 ;630.6µS movf PORTD,w xorlw 11111111B andwf tmp_rang,f movlw 11100000B andwf PORTB,f ;riazzera DAC Delay16 racont,1,94 ;630.6µS movf PORTD,w andwf tmp_rang,f Jb flags2,0,ranging4_3 Jnb tmp_rang,0,ranging4_3 bsf flags2,0 Movff dac,trang2 ranging4_3 Jb flags2,1,ranging4_4 Jnb tmp_rang,1,ranging4_4 bsf flags2,1 Movff dac,trang2+1 ranging4_4 Jb flags2,2,ranging4_5 Jnb tmp_rang,2,ranging4_5 bsf flags2,2 Movff dac,trang2+2 ranging4_5 Jb flags2,3,ranging4_6 Jnb tmp_rang,3,ranging4_6 bsf flags2,3 Movff dac,trang2+3 ranging4_6 Jb flags2,4,ranging4_7 Jnb tmp_rang,4,ranging4_7 bsf flags2,4 Movff dac,trang2+4 ranging4_7 Jb flags2,5,ranging4_8 Jnb tmp_rang,5,ranging4_8 bsf flags2,5 Movff dac,trang2+5 ranging4_8 Jb flags2,6,ranging4_9 Jnb tmp_rang,6,ranging4_9 bsf flags2,6 Movff dac,trang2+6 ranging4_9 incf dac,f Cpje dac,26,ranging4_2 return ranging4_2 bsf flags2,7 ;setta stato task first level Sknb flags2,0 ;imposta overflow se non ricevuto segnale Skip 2 Ldf trang2,32 Sknb flags2,1 Skip 2 Ldf trang2+1,32 Sknb flags2,2 Skip 2 Ldf trang2+2,32 Sknb flags2,3 Skip 2 Ldf trang2+3,32 Sknb flags2,4 Skip 2 Ldf trang2+4,32 Sknb flags2,5 Skip 2 Ldf trang2+5,32 Sknb flags2,6 Skip 2 Ldf trang2+6,32 return ;------------------------------------------------------------------------------ ; Normalizzazione letture IR in fasce di percorribilita', il bot per la ; navigazione si basera' su queste fasce. ; ; I sensori laterali servono per la navigazione, wall following, corner detection, ; object seeking, e' necessario poter correggere la distanza in modo abbastanza ; fine entro una fascia di pochi cm. ; ; Sensori laterali desta ; Lettura IR codifica flag descrizione ; < 11 00000001 troppo vicino ; 11..12 00000010 ok ; 13 00000100 troppo lontano ; 14..15 00001000 nei paraggi ; >15 00010000 overrange ; ; Sensori laterali sinistra ; Lettura IR codifica flag descrizione ; < 10 00000001 troppo vicino ; 10..11 00000010 ok ; 12 00000100 troppo lontano ; 13..15 00001000 nei paraggi ; >15 00010000 overrange ; ; I sensori frontali e diagonali servono solo come antibump ; per cui e' sufficiente individuare il range di sicurezza. ; ; Lettura IR codifica flag descrizione ; < 8 00000001 troppo vicino ; 8..11 00000010 ok stop ; 12..15 00001000 nei paraggi ; >15 00010000 overrange ;------------------------------------------------------------------------------ normir clrf PCLATH movlw 16 ;normalizza later.destro posteriore subwf trang2,w Jc normir6 movf trang2,w call tabsensdes movwf trang Skip 2 normir6 Ldf trang,00010000B Movff trang,trang2;debug movlw 16 ;normalizza later.sinistro anteriore subwf trang2+1,w Jc normir5 movf trang2+1,w call tabsenssin movwf trang+1 Skip 2 normir5 Ldf trang+1,00010000B Movff trang+1,trang2+1;debug movlw 16 ;normalizza diagonale sinistro subwf trang2+2,w Jc normir1 movf trang2+2,w call tabsensfro movwf trang+2 Skip 2 normir1 Ldf trang+2,00010000B Movff trang+2,trang2+2;debug movlw 16 ;normalizza frontale subwf trang2+3,w Jc normir2 movf trang2+3,w call tabsensfro movwf trang+3 Skip 2 normir2 Ldf trang+3,00010000B Movff trang+3,trang2+3;debug movlw 16 ;normalizza diagonale destro subwf trang2+4,w Jc normir3 movf trang2+4,w call tabsensfro movwf trang+4 Skip 2 normir3 Ldf trang+4,00010000B Movff trang+4,trang2+4;debug movlw 16 ;normalizza later.destro anteriore subwf trang2+5,w Jc normir7 movf trang2+5,w call tabsensdes movwf trang+5 Skip 2 normir7 Ldf trang+5,00010000B Movff trang+5,trang2+5;debug movlw 16 ;normalizza later.sinistro posteriore subwf trang2+6,w Jc normir4 movf trang2+6,w call tabsenssin movwf trang+6 Skip 2 normir4 Ldf trang+6,00010000B Movff trang+6,trang2+6;debug return ;------------------------------------------------------------------------------ ; Raggruppa le fasce misurate per determinare i flags di riconoscimento ; oggetti e direzioni possibili ;------------------------------------------------------------------------------ sensdir bcf oggsx bcf oggfron bcf oggdx bcf ruotadx bcf ruotasx Sknb trang+5,0 bsf oggdx Sknb trang+5,1 bsf oggdx Sknb trang+5,2 bsf oggdx Sknb trang+4,0 bsf oggdx Sknb trang+4,1 bsf oggdx Sknb trang+2,0 bsf oggfron Sknb trang+2,1 bsf oggfron Sknb trang+3,0 bsf oggfron Sknb trang+3,1 bsf oggfron Sknb trang+4,0 bsf oggfron Sknb trang+4,1 bsf oggfron Sknb trang+1,0 bsf oggsx Sknb trang+1,1 bsf oggsx Sknb trang+1,2 bsf oggsx Sknb trang+2,0 bsf oggsx Sknb trang+2,1 bsf oggsx Jb oggdx,sensdir1 Jnb oggsx,sensrand bsf ruotadx return sensdir1 Jb oggsx,sensrand bsf ruotasx return sensrand Movb TMR0,0,ruotasx Movnb ruotasx,ruotadx return ;------------------------------------------------------------------------------ ;------------------------------------------------------------------------------ taskmove Jnb cmdmov,0,taskmoveok movf trang+2,w iorwf trang+3,w iorwf trang+4,w iorwf trang+1,w iorwf trang+5,w andlw 00000001B Jz taskmoveok call stopall bsf flagmove,0 return taskmoveok bcf flagmove,0 Jb cmdmov,0,taskmovav Jb cmdmov,2,taskmovsx call rotdx return taskmovsx call rotsx return taskmovav call avall return ;------------------------------------------------------------------------------ ; Retromarcia antibump ;------------------------------------------------------------------------------ retro call stopall call ranging call indall Ldf contmov,2 retro1 call ranging Djnz contmov,retro1 call stopall return ;------------------------------------------------------------------------------ ; Subroutines controllo motori ;------------------------------------------------------------------------------ stopall Ldf CCPR1L,128 Ldf CCPR2L,128 return avall bsf PORTB,6 bsf PORTB,7 Ldf CCPR1L,sxavanti Ldf CCPR2L,dxavanti return indall bcf PORTB,6 bcf PORTB,7 Ldf CCPR1L,sxindietro Ldf CCPR2L,dxindietro return stopsx Ldf CCPR1L,128 return stopdx Ldf CCPR2L,128 return avsx bsf PORTB,6 Ldf CCPR1L,sxavanti return avdx bsf PORTB,7 Ldf CCPR2L,dxavanti return indsx bcf PORTB,6 Ldf CCPR1L,sxindietro return inddx bcf PORTB,7 Ldf CCPR2L,dxindietro return rotsx bsf PORTB,7 Ldf CCPR2L,dxavanti bcf PORTB,6 Ldf CCPR1L,sxindietro return rotdx bcf PORTB,7 Ldf CCPR2L,dxindietro bsf PORTB,6 Ldf CCPR1L,sxavanti return ;------------------------------------------------------------------------------ ; Trasmette i dati di ranging al pc per debug ;------------------------------------------------------------------------------ trasmetti movlw 65 call sertx movf trang2,w call sertx movf trang2+1,w call sertx movf trang2+2,w call sertx movf trang2+3,w call sertx movf trang2+4,w call sertx movf trang2+5,w call sertx movf trang2+6,w call sertx movf cmps_ang,w call sertx return ;------------------------------------------------------------------------------ ; Subroutine per tx seriale per debug ;------------------------------------------------------------------------------ sertx movwf txbyte ;mette dato in txbyte bcf serout ;inizio start bit a 0 movlw 171 ;imposta la sua durata movwf txdelay decfsz txdelay,f goto $-1 ;ciclo di ritardo movlw 8 ;8 bit da trasmettere movwf txcontabit btfsc txbyte,0 ;testa bit 0 di txbyte goto $+4 nop bcf serout ;se 0 abbassa il bit serout goto $+4 bsf serout ;altrimenti lo alza nop nop rrf txbyte,f ;poi scorre a destra txbyte movlw 170 ;imposta durata del bit movwf txdelay decfsz txdelay,f goto $-1 ;ciclo di ritardo decfsz txcontabit,f goto $-14 ;ciclo di 8 bit goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 goto $+1 bsf serout ;inizio stop bits a 1 movlw 255 ;imposta la loro durata movwf txdelay decfsz txdelay,f goto $-1 ;ciclo di ritardo movlw 255 ;imposta la loro durata movwf txdelay decfsz txdelay,f goto $-1 ;ciclo di ritardo return ;------------------------------------------------------------------------------ END