; File: sonar.asm ; 6 channel sonar and beacon code for vacuum robot ; Developed on Mandrake Linux 7.2 ; Written for 16C73A PIC microcontroller with 4.0 MHZ clock ; Version 1.11 - fixed some wrong comments ; ; This file can be assembled with gpasm avaliable at: ; http://gpasm.sourceforge.net/ ; ; The hardware schematic is in sonar-2.sch ; ; Copyright Dale A. Heatherington Feb-Apr 2001 ; ;;---------------------------------------------------------------------- ;;This program is free software; you can redistribute it and/or modify ;;it under the terms of the GNU General Public License as published by ;;the Free Software Foundation; either version 2 of the License, or ;;(at your option) any later version. ;; ;;This program is distributed in the hope that it will be useful, ;;but WITHOUT ANY WARRANTY; without even the implied warranty of ;;MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ;;GNU General Public License for more details. ;;You should have received a copy of the GNU General Public License ;;along with this program; if not, write to the Free Software ;;Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA ;;------------------------------------------------------------------------ ; ; ; PIC16C73B 28 pin DIP pin assignments ; ; 2 RA0 Not used ; 3 RA1 Not used ; 4 RA2 Raw Echo input from comparators, LOW TRUE. ; 5 RA3 Not used ; 6 RA4 Receiver blanking pulse output, HIGH TRUE. ; 7 RA5 Not used ; ; 21 RB0 40 khz Piezo sonar transmitter common ; 22 RB1 Left Front sonar transmitter ; 23 RB2 Right Front sonar transmitter ; 24 RB3 Right Side sonar transmitter ; 25 RB4 Left Side sonar transmitter ; 26 RB5 Right Rear sonar transmitter ; 27 RB6 Left Rear sonar transmitter ; 28 RB7 IR LED driver output ; ; 11 RC0 Analog mux address line 0 (A) ; 12 RC1 Analog mux address line 1 (B) ; 13 RC2 Analog mux address line 2 (C) ; 14 RC3 SPI clock output ; 15 RC4 SPI chip select output, LOW TRUE ; 16 RC5 SPI data output ; 17 RC6 Left 40 khz IR receiver input ; 18 RC7 Right 40 khz IR receiver input ; ;----------------------------------------------------------------------- ; SPI Data format ; ; The SPI port is "output only". 9 bytes are transmitted in each frame. ; ; SPI Port byte sequence. ; ; 1 Flag, 7E hex, to verify framing at receiver ; 2 Left front sonar echo time ; 3 Right front ; 4 Right side ; 5 Left side ; 6 Right Rear ; 7 Left Rear ; 8 Beacon distance ; 9 IR receiver status bits (see below) ; ; IR Status bits ; 0 x ; 1 x ; 2 3rd Left ; 3 3rd Right ; 4 2nd Left ; 5 2nd Right ; 6 1st Left ; 7 1st Right ; ; ;All echo distances are 48 counts per foot or 4 per inch. ;The minimum distance is 6.5 inches and the max is 5.8 feet. ;A ZERO reading means 0 to 6.5 inches. ;A 255 reading means 5.8 feet or more. ; ;The Beacon distance is 24 counts per foot with no offset. ;ie: 48 = 24 inches distance from beacon. ;------------------------------------------------------------- RADIX dec processor 16C73A __CONFIG h'3ff2' ;external 4 RC mhz osc include "p16c73a.inc" ;------------------------------------------------------------ ;PORT A bits ECHO_IN equ 2 ;raw echo input , low true input OUT_BLANK equ 4 ;Sonar receiver blanking pulse, high true output ;PORT B output bits IR_LED equ 7 ;IR LED driver, high true output ;SPI port bits (bit bang) ;PORTC: SPI_CS equ 4 SPI_CLK equ 3 SPI_DOUT equ 5 IR_RX_LEFT equ 6 ;IR receiver input, left low true IR_RX_RIGHT equ 7 ;IR receiver input, right low true ;---------------------------------- ;40 khz microphone addressing bits ;PORTC: MIC_0 equ 0 MIC_1 equ 1 MIC_2 equ 2 ;Left front is 000 ;Right front is 011 ;left side is 010 ;right side is 001 ;right rear is 100 ;left rear is 101 ;------------------------------- ;Define how many cycles of 40khz are in one pulse N_cycles equ 7 ; ;define our variables ; ; j_main equ 20h k_main equ 21h i_main equ 22h t_echo_1a equ 23h t_echo_1b equ 24h t_echo_2a equ 25h t_echo_2b equ 26h t_echo_3a equ 27h t_echo_3b equ 28h t_echo_4a equ 29h t_echo_4b equ 2ah t_avg_1 equ 2bh t_avg_2 equ 2ch t_avg_3 equ 2dh t_avg_4 equ 2eh ir_bits equ 2fh ;Left and right IR receiver state bits spi_data equ 30h t_echo_x1a equ 31h ;echo delay from beacon transponder #1 t_echo_x1b equ 32h t_echo_x2a equ 33h ;echo delay from beacon transponder #2 [NOT USED] t_echo_x2b equ 34h t_avg_x1 equ 35h t_avg_x2 equ 36h t_echo_5a equ 37h t_echo_5b equ 38h t_echo_6a equ 39h t_echo_6b equ 3ah t_avg_5 equ 3bh t_avg_6 equ 3ch ctr equ 3dh ;increments each time we go through the loop workb equ 3eh ;-------------------------------------------------- ; MACROS ;33 khz IR LED driver macro. High true output for driver transistor. ;one cycle of 33 khz (less 3uS for overhead in other code) cycle_led MACRO bit nop nop nop nop nop nop nop nop nop nop nop nop nop bsf PORTB,bit ;set output bit (LED on) nop nop nop nop nop nop nop nop nop nop nop nop bcf PORTB,bit ;clear output bit (LED off) endm ;--------------------------------------------------------------------------- ;Piezo ultrasonic transducer differencal driver macro. ; ;The common lead is toggled 180 degrees out of phase with the signal lead ;to make a 10 volt difference across the transducer. ; ;Creates one cycle of 40 khz (less 3uS for overhead in other code) ; cycle_pz MACRO nop nop nop nop nop nop nop nop movlw 7fh xorwf workb,W ;Flip bits 0..6 result to W movwf PORTB ;put in PORTB and diddle the piezo transducers nop nop nop nop nop nop nop nop nop xorlw 7fh ;flip them back movwf PORTB endm ;------------------------------------------------------------------ ;Average two echo times avg_echo MACRO t1,t2,avg movf t1,W ;get 1st t value addwf t2,W ;add the 2nd to it movwf avg ;move low 8 bits to avg rrf avg,f ;divide by 2 including carry bit ENDM ;--------------------------------------------------------------------- ;Actual code starts here... ORG 0 main_code clrf STATUS movlw 0ffh movwf PORTB movwf PORTA movwf PORTC bsf STATUS,RP0 ;bank 1 movlw 0efh ;Bit 4, port A is an output (Receiver blanking) movwf TRISA ; movlw 00h ;Port B all outputs movwf TRISB movlw 0c0h ;Bit 6,7 port c are inputs movwf TRISC movlw 80h ;PORTB pullups disabled movwf OPTION_REG movlw 4 ;RA 2,4,5 are digital movwf ADCON1 ;Other PORTA inputs are analog but not used bcf STATUS,RP0 ;bank 0 clrf INTCON ;no interrupts clrf ADCON0 ;A to D converter OFF bsf PORTB,SPI_CLK ;SPI clock high movlw 255 movwf t_avg_x2 ;Sonar Pulsing sequence: LF = Left front RF = right front RS = Right side ;RR = Right Rear LR = Left Rear ; 1 LF ; 2 RF ; 3 RS ; 4 RF ; 5 LF ; 6 RS ; 7 RR ; 8 LR label_0001 ;Top of main loop ; ;----------- Emit 1st pulse on LEFT FRONT 40khz emitter ------- pulse_1 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bcf PORTC,MIC_0 bcf PORTC,MIC_1 ;address left front MIC bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,1 ;Enable transducer #1 pulse_loop_1 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_1 ;loop N_cycles bcf workb,1 ;short the piezo tx element call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo movwf t_echo_1a ;-------- Emit 1st pulse on RIGHT FRONT 40khz emitter --------- pulse_2 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bsf PORTC,MIC_0 bsf PORTC,MIC_1 ;address Right front microphone 011 bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,2 ;Enable transducer #2 pulse_loop_2 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_2 ;loop N_cycles bcf workb,2 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo movwf t_echo_2a ;-------- Emit pulse on RIGHT SIDE 40khz emitter --------- ; pulse_3 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bsf PORTC,MIC_0 bcf PORTC,MIC_1 ;address the right side mic 001 bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,3 ;Enable transducer #3 pulse_loop_3 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_3 ;loop N_cycles bcf workb,3 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo btfss ctr,0 movwf t_echo_3a ;even loops put result here btfsc ctr,0 movwf t_echo_3b ;odd loops put result here ;-------- Emit 2nd pulse on RIGHT FRONT 40khz emitter --------- pulse_4 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bsf PORTC,MIC_0 bsf PORTC,MIC_1 ;address right front mic 011 bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,2 ;Enable transducer #2 pulse_loop_4 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_4 ;loop N_cycles bcf workb,2 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo movwf t_echo_2b ;----------- Emit 2nd pulse on LEFT FRONT 40khz emitter ------- pulse_5 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bcf PORTC,MIC_0 bcf PORTC,MIC_1 ;address left front mic 000 bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,1 ;Enable transducer #1 pulse_loop_5 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_5 ;loop N_cycles bcf workb,1 ;short the piezo tx element call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo movwf t_echo_1b ; ; ;-------- Emit pulse on LEFT SIDE 40khz emitter --------- ; pulse_6 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bcf PORTC,MIC_0 bsf PORTC,MIC_1 ;address left side mic 010 bcf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,4 ;Enable transducer #3 pulse_loop_6 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_6 ;loop N_cycles bcf workb,4 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo btfss ctr,0 movwf t_echo_4a ;even loops put result here btfsc ctr,0 movwf t_echo_4b ;odd loops put result here ; ; ;-------- Emit pulse on RIGHT REAR 40khz emitter --------- ; pulse_7 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bcf PORTC,MIC_0 bcf PORTC,MIC_1 ;address right rear mic 100 bsf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,6 ;Enable transducer #6 pulse_loop_7 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_7 ;loop N_cycles bcf workb,6 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo btfss ctr,0 movwf t_echo_5a ;even loops put result here btfsc ctr,0 movwf t_echo_5b ;odd loops put result here ; ; ;-------- Emit pulse on LEFT REAR 40khz emitter --------- ; pulse_8 movlw N_cycles ; movwf i_main ; Loop N_cycles times for N_cycles cycles of 40 khz ; bsf PORTC,MIC_0 bcf PORTC,MIC_1 ;address left rear mic 101 bsf PORTC,MIC_2 bsf PORTA,OUT_BLANK ;Blank receiver clrf workb bsf workb,5 ;Enable transducer #5 pulse_loop_8 cycle_pz ;Do a 40khz cycle on piezo transducer decfsz i_main,f goto pulse_loop_8 ;loop N_cycles bcf workb,5 call delay_900 bcf PORTA,OUT_BLANK ;unblank receiver call get_echo btfss ctr,0 movwf t_echo_6a ;even loops put result here btfsc ctr,0 movwf t_echo_6b ;odd loops put result here ;--------------------------------------------------------------- ;The following code polls the beacon and measures the ;returning IR and ultrasonic signals. ; ; ; First, send a short 600 microsecond IR pulse to trigger Beacon. ; The beacon will resond with 4 IR pulses. The first is ; sent on all three beacon LEDs to condition the receiver AGC. ; The next three are sent on each of the 3 IR LEDs in sequence ; spaced at 1000 uS. The 3 beacon LEDs are pointed in different ; directions so we can determine which "zone" the robot is in based ; on which of the 3 pulses we see. ; ; After the IR pulses the beacon sends a 40khz ultrasonic pulse. ; We measure the travel time to determine our distance from the beacon. ; It's calibrated at 0.5 inch per count. Max distance is 10.625 feet = 255. ; Only the LEFT FRONT sonar receiver is used to receive the beacons pulse. ; ;------------------------------------------------------------------------ pulse_led movlw 20 movwf i_main Pulse_loop_led cycle_led IR_LED ;Send IR pulse to poll the beacon decfsz i_main,f goto pulse_loop_led ;loop for 20 cycles ; call delay_1000 ;wait for initial 1ms IR AGC leveling pulse ; call delay_1000 ;wait until the end of 1st 1000us IR data pulse. ;Trailing edge delays require longer delay by about 300us ; ;so we sample near the end of the rx pulse. call delay_200 bsf PORTA,OUT_BLANK ;Blank sonar receiver clrf ir_bits movlw 0c0h ;put 0xc0 bit mask in k_main movwf k_main ;now sample the 1st IR return pulse comf PORTC,W ;get IR receivers to W and invert the bits andwf k_main,W ;keep only bits 6 and 7 iorwf ir_bits,f ;or them into ir_bits 6,7 call delay_1000 ;wait for 2nd IR return pulse from beacon ; comf PORTC,W ;get IR receivers to W and invert the bits andwf k_main,W ;keep only bits 6 an 7 movwf j_main bcf STATUS,C rrf j_main,f rrf j_main,W ;move bits to positions 4,5 iorwf ir_bits,f ;OR them into ir_bits 4,5 call delay_1000 ;Wait for 3rd IR return pulse from beacon ; comf PORTC,W ;get IR receivers to W with bits inverted andwf k_main,W ;keep only bits 6 and 7 movwf j_main bcf STATUS,C rrf j_main,f rrf j_main,f rrf j_main,f rrf j_main,W ;move bits to positions 2,3 iorwf ir_bits,f ;OR them into ir_bits bcf PORTC,MIC_0 bcf PORTC,MIC_1 ;address left front mic 000 bcf PORTC,MIC_2 call delay_100 ;wait 300us to align with the start of beacon sonar pulse call delay_200 bcf PORTA,OUT_BLANK ;unblank sonar receiver call get_echo ;get the "echo" time value from the beacon transponder ;Save delay time from transponder (0.5 inch resolution, it's not round trip!) btfss ctr,0 movwf t_echo_x1a ;even loops put result here btfsc ctr,0 movwf t_echo_x1b ;odd loops put result here ;average the samples avg_echo t_echo_1a, t_echo_1b, t_avg_1 avg_echo t_echo_2a, t_echo_2b, t_avg_2 avg_echo t_echo_3a, t_echo_3b, t_avg_3 avg_echo t_echo_4a, t_echo_4b, t_avg_4 avg_echo t_echo_5a, t_echo_5b, t_avg_5 avg_echo t_echo_6a, t_echo_6b, t_avg_6 avg_echo t_echo_x1a, t_echo_x1b, t_avg_x1 ;Output 9 bytes to SPI port bcf PORTC,SPI_CS ;Lower SPI CS line nop ;wait 7 uS nop nop nop nop nop nop movlw 7eh call spi_out ;Send out a flag byte movf t_avg_1,W ;Left front echo average to W call spi_out ;send movf t_avg_2,W ;right front echo average to W call spi_out ;send movf t_avg_3,W ;Right Side echo time to W call spi_out ;send movf t_avg_4,W ;Left Side echo time to W call spi_out ;send movf t_avg_5,W ;Right Rear echo time to W call spi_out movf t_avg_6,W ;Left Rear echo to W call spi_out movf t_avg_x1,W ;Beacon #1 transponder echo time call spi_out ;send movf ir_bits,W ;IR receiver status bits call spi_out ;send bsf PORTC,SPI_CS ;raise SPI CS line incf ctr,f ;count this loop goto label_0001 ;Infinite main loop ;------------------------------ ; ;Bit bang the SPI port... ; spi_out movwf spi_data movlw 8 movwf i_main spi_dat bcf PORTC,SPI_CLK ;clock low bcf PORTC,SPI_DOUT ;clear output bit btfsc spi_data,7 bsf PORTC,SPI_DOUT ;set output bit if data is ONE rlf spi_data,f ;Left shift data bsf PORTC,SPI_CLK ;clock high DATA VALID NOW decfsz i_main,f goto spi_dat ;loop 8 times bcf PORTC,SPI_DOUT ;data low return ;------------------------------------------- ; ;This is the ECHO TIME measuring routine. ;Sample every 38us for 1/4 inch resolution. W register returns number of 1/4 inch counts. ;We wait 9.7 ms for all returning echos. Only the first echo is measured. ;A return value of 0 means the distance is less than 6.5 inches (approx). ;A return value of 255 means the distance is greater than 5.8 feet. ;There are 48 counts per foot, 4 counts per inch. get_echo clrf k_main movlw 255 movwf j_main ;main loop counter = 255 _loop movlw 9 movwf i_main _loop2 decfsz i_main,f ;27us delay loop goto _loop2 nop ;trim loop out to 38us nop nop nop btfss PORTA,ECHO_IN ;check for echo presense goto _got_echo incf k_main,f ;count one 38us interval decfsz j_main,f goto _loop goto _done _got_echo movlw 9 movwf i_main _loop3 decfsz i_main,f goto _loop3 nop nop ;trim loop to 38 us nop nop nop nop nop decfsz j_main,f goto _got_echo _done movf k_main,W return ;---------------------------------------- ; ;Some time wasting delay routines. Not all are used. ; ;Delay 1000us delay_1000 movlw 248 movwf i_main _D1000L nop decfsz i_main,f goto _D1000L nop nop return ;Delay 900us delay_900 movlw 223 movwf i_main _D900L nop decfsz i_main,f goto _D900L nop nop return ;Delay 500us delay_500 movlw 123 movwf i_main _D500L nop decfsz i_main,f goto _D500L nop nop return ;Delay 200 us delay_200 movlw 48 movwf i_main _D200L nop decfsz i_main,f goto _D200L nop nop return ;Delay 100 us delay_100 movlw 23 movwf i_main _D100L nop decfsz i_main,f goto _D100L nop nop return END