;************************************************************************** ; FILE: madbot.asm * ; CONTENTS: MadBot robot * ; COPYRIGHT: MadLab Ltd. 2006 * ; AUTHOR: James Hutchby * ; UPDATED: 27/06/06 * ;************************************************************************** list p=16F628A ; list p=16F648A ifdef __16F628A include "p16f628a.inc" endif ifdef __16F648A include "p16f648a.inc" endif __config _INTOSC_OSC_NOCLKOUT & _WDT_ON & _PWRTE_ON & _BOREN_OFF & _MCLRE_OFF & _LVP_OFF & _CP_ON __idlocs h'FB10' errorlevel -205,-207,-302,-305,-306 ;************************************************************************** ; * ; Specification * ; * ;************************************************************************** ; power-up self-test - LEDs flash and beeps twice ; if keyboard not detected on power-up then previous mode, otherwise: ; F1 -> roam mode (obstacle avoidance) ; F2 -> maze mode (wall following) ; F3 -> push mode (searches for objects then pushes) ; F4 -> avoid mode (triggered by changes in light level) ; F5 -> light mode (searches at random for light regions) ; F6 -> dark mode (searches at random for dark regions) ; F7 -> dance mode (plays recorded sequence of moves) ; shift F7 -> edit mode (records sequence of moves) ; shift F10 -> adjust steering (corrects for left/right motor bias) ; F11 -> echo IR sensors to LEDs (left & right) ; F12 -> echo LDR sensor to LEDs (left & right + keyboard) ; shift PAGE UP -> increase speed ; shift PAGE DOWN -> decrease speed ; shift HOME -> IR sensors detuning off ; shift END -> IR sensors detuning on (3 settings) ; arrow keys -> move robot (release to stop) ; ESC -> restart ; 2 second delay after selecting mode F1 to F7 if keyboard connected ; edit mode: ; UP ARROW -> move forward, followed by duration in 1/8s (hex digit 1 to 15) ; DOWN ARROW -> move backward, followed by duration in 1/8s (hex digit 1 to 15) ; LEFT ARROW -> turn left, followed by duration in 1/8s (hex digit 1 to 15) ; RIGHT ARROW -> turn right, followed by duration in 1/8s (hex digit 1 to 15) ; S -> stop movement, followed by duration in 1/8s (hex digit 1 to 15) ; B -> beep ; F -> flash LEDs ; L -> loop to start ; M -> macro move, followed by # (1 to 6) ; BACKSPACE, DELETE -> delete last move ; ENTER, ESC -> finish recording ; adjust steering: ; robot moves forward ; LEFT ARROW -> adjust movement to the left ; RIGHT ARROW -> adjust movement to the right ; ENTER, ESC -> finish adjusting ; steering, speed and detuning stored in EEPROM ;************************************************************************** ; * ; Port assignments * ; * ;************************************************************************** PORTA_IO equ b'01100100' ; port A I/O status PORTB_IO equ b'01000001' ; port B I/O status #define LED1 PORTA,0 ; left LED #define LED2 PORTA,3 ; right LED #define SPEAKER PORTA,4 ; piezo speaker #define LDR PORTA,2 ; LDR #define IR_RX PORTB,0 ; IR receiver #define IR_TX1 PORTA,7 ; left IR emitter #define IR_TX2 PORTA,1 ; right IR emitter #define KBD_CLK PORTB,7 ; keyboard clock #define KBD_DATA PORTB,6 ; keyboard data #define MOTORS PORTB ; motors port #define MOTORS_EN 3 ; PWM #define MOTOR1_CON1 2 ; left control #define MOTOR1_CON2 1 #define MOTOR2_CON1 5 ; right control #define MOTOR2_CON2 4 MOTORS_MASK equ (1< MAX error "File register usage overflow" endif ; flags KEYBOARD equ 0 ; set if keyboard found RESET equ 1 ; set if keyboard reset DETECT equ 2 ; set if obstacles to be detected PIEZO equ 3 ; piezo state ; keyboard flags LEFT equ 0 ; set if left arrow key pressed RIGHT equ 1 ; set if right arrow key pressed FORWARD equ 2 ; set if up arrow key pressed BACKWARD equ 3 ; set if down arrow key pressed EXTENDED equ 4 ; set if extended key code received BREAK equ 5 ; set if key break code received SHIFT1 equ 6 ; set if left shift key pressed SHIFT2 equ 7 ; set if right shift key pressed SHIFT_MASK equ (1< d'64' call delay64 ; [64] i -= d'64' endw if i >= d'16' variable n = (i-d'16')/d'8' call delay16-n ; [8+8n+8] i -= (n*d'8')+d'16' endif while i >= d'4' nop ; [4] i -= d'4' endw if i != 0 error "Delay cycles not multiple of 4" endif endm ;-------------------------------------------------------------------------- ; waits for the number of ms in w reg ;-------------------------------------------------------------------------- routine wait_ms movwf count WAIT equ CLOCK/(d'1000'*d'80') wait1 movlf WAIT,loop wait2 clrwdt ; [4] delay d'64' ; [64] decfsz loop ; [4] goto wait2 ; [8] decfsz count goto wait1 return ;-------------------------------------------------------------------------- ; waits for the number of seconds in w reg ;-------------------------------------------------------------------------- routine wait_s movwf repeat wait3 movlw d'250' call wait_ms movlw d'250' call wait_ms movlw d'250' call wait_ms movlw d'250' call wait_ms decfsz repeat goto wait3 return ;************************************************************************** ; PS/2 keyboard routines * ;************************************************************************** ;-------------------------------------------------------------------------- ; waits for a clock transition from the keyboard, returns the Z flag set ; if timeout ;-------------------------------------------------------------------------- routine wait_kbd ; timeout period in ms TIMEOUT equ (d'4'*d'256'*d'20')/(CLOCK/d'1000') movlw d'4' ; wait for clock to go high waitk1 clrwdt clrf timeout waitk2 btfsc KBD_CLK ; [8] goto waitk3 decfsz timeout ; [4] goto waitk2 ; [8] addlw -1 bnz waitk1 goto waitk7 ; timeout waitk3 movlw d'4' ; wait for clock to go low waitk4 clrwdt clrf timeout waitk5 btfss KBD_CLK ; [8] goto waitk6 decfsz timeout ; [4] goto waitk5 ; [8] addlw -1 bnz waitk4 goto waitk7 ; timeout waitk6 delay d'10'*(CLOCK/d'1000000') clrz ; signal ok waitk7 return ;-------------------------------------------------------------------------- ; tests if the keyboard is ready to transmit a byte, returns the NZ flag ; set if yes ;-------------------------------------------------------------------------- routine test_kbd ; test period in ms TEST equ (d'2'*d'256'*d'24')/(CLOCK/d'1000') movlw d'2' ; wait for clock to go low clrf timeout test1 clrwdt ; [4] btfss KBD_CLK ; [8] goto test2 decfsz timeout ; [4] goto test1 ; [8] addlw -1 bnz test1 goto test3 ; timeout test2 delay d'10'*(CLOCK/d'1000000') clrz ; signal ready test3 return ;-------------------------------------------------------------------------- ; transmits the byte in w reg to the keyboard, returns the Z flag set if ; timeout ;-------------------------------------------------------------------------- routine tx_kbd movwf kbd_code bcf INTCON,GIE ; interrupts off kbd 0,1 ; request to send movlw 1 call wait_ms kbd 0,0 ; start bit nop kbd 1,0 call wait_kbd bz tx6 ; branch if timeout movlf d'8',count clrf bits tx1 btfsc kbd_code,0 incf bits rrf kbd_code ; transmit data bit skpnc bsf KBD_DATA skpc bcf KBD_DATA call wait_kbd bz tx6 ; branch if timeout decfsz count goto tx1 btfss bits,0 ; parity bit (odd parity) bsf KBD_DATA btfsc bits,0 bcf KBD_DATA call wait_kbd bz tx6 ; branch if timeout kbd 1,1 ; stop bit clrf timeout ; wait for acknowledge bit tx2 clrwdt decf timeout bz tx6 ; branch if timeout btfsc KBD_DATA goto tx2 clrf timeout tx3 clrwdt decf timeout bz tx6 ; branch if timeout btfsc KBD_CLK goto tx3 clrf timeout tx4 clrwdt decf timeout bz tx6 ; branch if timeout btfss KBD_DATA goto tx4 clrf timeout tx5 clrwdt decf timeout bz tx6 ; branch if timeout btfss KBD_CLK goto tx5 clrz ; signal ok goto tx7 tx6 kbd 1,1 setz ; signal timeout tx7 bsf INTCON,GIE ; interrupts on return ;-------------------------------------------------------------------------- ; receives a byte from the keyboard into w reg (and kbd_code), returns ; the Z flag set if timeout and the C flag set if error ;-------------------------------------------------------------------------- routine get_kbd bcf INTCON,GIE ; interrupts off kbd 1,1 call test_kbd ; keyboard ready to transmit ? bz rx4 ; branch if not routine rx_kbd bcf INTCON,GIE ; interrupts off btfsc KBD_DATA ; test start bit goto rx5 ; branch if error movlf d'8',count clrf bits rx1 call wait_kbd bz rx4 ; branch if timeout clrc ; receive data bit btfsc KBD_DATA setc rrf kbd_code btfsc kbd_code,7 incf bits decfsz count goto rx1 call wait_kbd bz rx4 ; branch if timeout btfsc KBD_DATA ; test parity bit incf bits btfss bits,0 goto rx5 ; branch if error call wait_kbd bz rx4 ; branch if timeout btfss KBD_DATA ; test stop bit goto rx5 ; branch if error clrf timeout ; wait for clock to go high rx2 clrwdt btfsc KBD_CLK goto rx3 decfsz timeout goto rx2 goto rx4 ; timeout rx3 clrz ; signal ok clrc goto rx6 rx4 clrf kbd_code setz ; signal timeout clrc goto rx6 rx5 clrf kbd_code clrz ; signal error setc rx6 kbd 0,1 swapf kbd_code swapf kbd_code,w swapf kbd_code ; w <= byte bsf INTCON,GIE ; interrupts on return ;-------------------------------------------------------------------------- ; polls the keyboard ;-------------------------------------------------------------------------- routine poll_kbd clrf kbd_key call get_kbd ; get keyboard byte bz pollk8 ; branch if timeout bc pollk7 ; branch if error tstf kbd_ignore ; ignore scan codes ? bz pollk1 ; branch if not decf kbd_ignore goto pollk7 pollk1 movlw KBD_PAUSE ; pause key ? subwf kbd_code,w bnz pollk2 ; branch if not movlf d'7',kbd_ignore ; ignore the next scan codes goto pollk7 pollk2 movlw KBD_EXTENDED ; extended key code ? subwf kbd_code,w bnz pollk3 ; branch if not bsf kbd_flags,EXTENDED ; signal received goto pollk8 pollk3 movlw KBD_BREAK ; key break code ? subwf kbd_code,w bnz pollk4 ; branch if not bsf kbd_flags,BREAK ; signal received goto pollk8 pollk4 btfsc kbd_flags,EXTENDED ; extended key ? goto pollk5 ; branch if yes key_ macro scan,bit local key1 movlw (scan)&h'7f' ; key ? subwf kbd_code,w bnz key1 ; branch if not bsf kbd_flags,bit ; set flag if pressed btfsc kbd_flags,BREAK bcf kbd_flags,bit ; clear flag if released goto pollk6 key1 endm key_ KBD_LEFT_SHFT,SHIFT1 ; test shift keys key_ KBD_RIGHT_SHFT,SHIFT2 goto pollk6 pollk5 key_ KBD_LEFT_ARROW,LEFT ; test arrow keys key_ KBD_RIGHT_ARROW,RIGHT key_ KBD_UP_ARROW,FORWARD key_ KBD_DOWN_ARROW,BACKWARD pollk6 btfsc kbd_flags,BREAK ; key break ? goto pollk7 ; branch if yes movff kbd_code,kbd_key ; store key pressed btfsc kbd_flags,EXTENDED bsf kbd_key,7 ; extended scan code pollk7 bcf kbd_flags,EXTENDED bcf kbd_flags,BREAK pollk8 return ;-------------------------------------------------------------------------- ; transmits the command in w reg to the keyboard, returns the Z flag set ; if error ;-------------------------------------------------------------------------- routine tx_command movwf kbd_command txc1 movfw kbd_command ; transmit command call tx_kbd bz txc4 ; branch if timeout call get_kbd ; wait for acknowledge bz txc2 ; branch if timeout bc txc2 ; branch if error xorlw KBD_RESEND ; resend ? bz txc1 ; branch if yes xorlw KBD_RESEND ; acknowledge ? xorlw KBD_ACK bnz txc4 ; branch if not goto txc3 txc2 movlw KBD_RESEND ; transmit command call tx_kbd bz txc4 ; branch if timeout call get_kbd ; wait for acknowledge bz txc4 ; branch if timeout bc txc4 ; branch if error xorlw KBD_RESEND ; resend ? bz txc1 ; branch if yes xorlw KBD_RESEND ; acknowledge ? xorlw KBD_ACK bnz txc4 ; branch if not txc3 clrz ; signal ok return txc4 setz ; signal error return ;-------------------------------------------------------------------------- ; initialises the keyboard, returns the Z flag set if error ;-------------------------------------------------------------------------- routine initialise_keyboard bcf flags,RESET REPEAT equ d'1000'/TIMEOUT initk1 movlf REPEAT,repeat ; wait for keyboard BAT initk2 call get_kbd bz initk3 ; branch if timeout bc initk4 ; branch if error xorlw KBD_BAT bnz initk4 goto initk5 ; branch if ok initk3 decfsz repeat goto initk2 initk4 btfsc flags,RESET ; not received or error goto initk6 bsf flags,RESET movlw d'250' call wait_ms movlw d'250' call wait_ms movlw d'250' call wait_ms movlw d'250' call wait_ms movlw KBD_RESET ; reset keyboard call tx_command bz initk6 ; branch if error goto initk1 initk5 movlw KBD_TYPEMATIC ; set typematic to slowest call tx_command bz initk6 movlw h'7f' call tx_command bz initk6 ; branch if error clrz ; signal ok return initk6 setz ; signal error return ;-------------------------------------------------------------------------- ; inhibits communication from the keyboard ;-------------------------------------------------------------------------- routine inhibit_keyboard kbd 0,1 movlw 1 call wait_ms return ;-------------------------------------------------------------------------- ; releases communication from the keyboard ;-------------------------------------------------------------------------- routine release_keyboard kbd 1,1 ; delay d'75'*(CLOCK/d'1000000') return ;-------------------------------------------------------------------------- ; flashes the keyboard LEDs ;-------------------------------------------------------------------------- routine flash_keyboard movlf d'4',repeat clrf kbd_leds flash1 movlw KBD_LEDS ; set/reset LEDs call tx_command bz flash2 ; branch if error comf kbd_leds ; toggle LEDS movfw kbd_leds andlw 7 call tx_command bz flash2 ; branch if error movlw d'200' call wait_ms decfsz repeat goto flash1 flash2 return ;-------------------------------------------------------------------------- ; gets a hex digit (1 to 9, A to F) from the keyboard, returns the digit ; in w reg, or the Z flag set if no digit pressed ;-------------------------------------------------------------------------- routine get_digit getd1 movlw 1 ; wait for key press call wait_ms call poll_kbd tstf kbd_key bz getd1 clrf ndx getd2 movfw ndx index digit_tbl incf ndx tstw ; end of table ? bz getd3 ; branch if yes subwf kbd_key,w ; scan code found ? bnz getd2 ; branch if not incf ndx clrc rrf ndx,w ; w <= digit getd3 return ;-------------------------------------------------------------------------- ; polls for and processes a key press ;-------------------------------------------------------------------------- routine do_key movlw 1 call wait_ms call poll_kbd ; poll keyboard tstf kbd_key bz dok6 clrf ndx dok1 movfw ndx index key_tbl incf ndx tstw ; end of table ? bz dok6 ; branch if yes subwf kbd_key,w ; scan code found ? bnz dok4 ; branch if not movfw ndx ; get shift status index key_tbl incf ndx tstw bnz dok2 movfw kbd_flags ; shift key must not be pressed andlw SHIFT_MASK bnz dok5 goto dok3 dok2 andlw h'80' bnz dok3 movfw kbd_flags ; shift key must be pressed andlw SHIFT_MASK bz dok5 dok3 bcf LED1 ; LEDs off bcf LED2 movlw KBD_LEDS call tx_command movlw 0 skpz call tx_command call stop ; motors off call beep_2 ; double beep clrf kbd_key ; clear key movfw ndx ; execute associated routine call key_tbl goto dok6 dok4 incf ndx dok5 incf ndx goto dok1 dok6 return ;************************************************************************** ; I/O routines * ;************************************************************************** ;-------------------------------------------------------------------------- ; samples the LDR sensor, returns the result in w reg (0 to 255) ;-------------------------------------------------------------------------- routine get_ldr discharge ; discharge the capacitor movlw DISCHARGE call wait_ms clrwdt clrf count bcf INTCON,GIE ; interrupts off charge ; charge the capacitor getl1 incf count ; [4] bz getl2 ; branch if timeout [8] delay d'100' ; [100] btfss LDR ; charged ? [4] goto getl1 ; loop if not [8] getl2 bsf INTCON,GIE ; interrupts on decf count,w return ;-------------------------------------------------------------------------- ; tests for an obstacle, returns the C flag set if obstacle detected ;-------------------------------------------------------------------------- ; total time in ms IR_PERIOD equ ((2*IR_BURST)/d'1000')+IR_RECOVERY routine obstacle_left bcf work1,0 goto obst1 routine obstacle_right bsf work1,0 obst1 bcf work1,7 bcf INTCON,GIE ; interrupts off movff CCPR1L,work2 ; motors off clrf CCPR1L clrf count ITERS set ((IR_BURST*IR_CARRIER)/d'1000')+1 movlf ITERS,loop DELAY set (((CLOCK/d'1000')+IR_CARRIER)/(IR_CARRIER*2))-d'52' DELAY set (DELAY+2)&(~3) obst2 btfss work1,0 ; IR emitter on [8/4] bsf IR_TX1 ; [0/4] btfsc work1,0 ; [8/4] bsf IR_TX2 ; [0/4] btfsc detune,0 ; detune carrier [8/4] goto $+1 ; [0/8] btfsc detune,1 ; [8/4] goto $+1 ; [0/8] btfsc detune,2 ; [8/4] goto $+1 ; [0/8] delay DELAY ; half-cycle delay clrwdt ; [4] btfss IR_RX ; [8/4] incf count ; [0/4] btfss work1,0 ; IR emitter off [8/4] bcf IR_TX1 ; [0/4] btfsc work1,0 ; [8/4] bcf IR_TX2 ; [0/4] btfsc detune,0 ; detune carrier [8/4] goto $+1 ; [0/8] btfsc detune,1 ; [8/4] goto $+1 ; [0/8] btfsc detune,2 ; [8/4] goto $+1 ; [0/8] delay DELAY ; half-cycle delay decfsz loop ; [4] goto obst2 ; [8] movlw ITERS/2 subwf count,w skpnc bsf work1,7 clrf count ITERS set ((CLOCK/d'1000000')*IR_BURST)/d'56' movlf ITERS,loop obst3 clrwdt ; [4] btfss IR_RX ; [8/4] incf count ; [0/4] delay d'32' ; [32] decfsz loop ; [4] goto obst3 ; [8] movlw ITERS/2 subwf count,w skpnc bcf work1,7 movff work2,CCPR1L bsf INTCON,GIE ; interrupts on movlw IR_RECOVERY ; receiver recovery time call wait_ms rlf work1 ; C set if carrier present return ;-------------------------------------------------------------------------- ; plays a note, fed with the note in w reg (1 to 72 = 6 octaves A to G), ; or 0 for note off ;-------------------------------------------------------------------------- routine play_note tstw ; note off ? bz play6 ; branch if yes movwf work1 sublw MAX_NOTE movlw MAX_NOTE skpc movwf work1 clrf work2 ; determine octave and offset decf work1 play1 incf work2 movlw d'12' subwf work1 bc play1 addwf work1 movfw work1 ; look up period addwf work1,w index note_tbl movwf pitch+0 incf work1,w addwf work1,w index note_tbl movwf pitch+1 play2 decf work2 ; adjust for octave bz play3 clrc rrf pitch+0 rrf pitch+1 bnc play2 incf pitch+1 skpnz incf pitch+0 goto play2 play3 movlf high DURATION,duration+0 movlf low DURATION,duration+1 bcf INTCON,GIE ; interrupts off bcf flags,PIEZO play4 clrwdt ; [4] spk_set ; toggle speaker output [20] movlw 1<