;************************************************************************** ; FILE: buglet.asm * ; CONTENTS: Buglet robot * ; COPYRIGHT: MadLab Ltd. 2007 * ; AUTHOR: James Hutchby * ; UPDATED: 29/03/07 * ;************************************************************************** list p=16F628A include "p16f628a.inc" __config _INTOSC_OSC_NOCLKOUT & _WDT_ON & _PWRTE_ON & _BOREN_OFF & _MCLRE_OFF & _LVP_OFF & _CP_ON __idlocs h'A110' errorlevel -205,-207,-302,-305,-306 ;************************************************************************** ; * ; Specification * ; * ;************************************************************************** ; power-up self-test - RGB LEDs flash once in turn and beeps twice, ; echoes left and right IR sensors to red and green LEDs, first with low ; power then with high power, beeps twice again ; if preset moved left to right or right to left during self-test, echoes LDR ; sensor to LEDs, otherwise tests both motors briefly ; male = preset hard right, female = preset hard left ; male/female indicated by blue/pink flash ; H(ue) = 0° to 360° (corresponding to 360 degrees around hexcone) (= 0 to 239 binary) ; red 0° - yellow 60° - green 120° - cyan 180° - blue 240° - magenta 300° - red 360° ; S(aturation) = 0.0 (shade of grey) to 1.0 (pure colour) (= 0 to 255 binary) ; V(alue) = 0.0 (black) to 1.0 (white) (= 0 to 255 binary) ; IR message format: ; command/flags ; bit 7 - male (1) or female (0) ; bit 6 - left (1) or right (0) ; bit 5 - high (1) or low (0) power ; bits 0 to 4 - command (bit 4 set if data present) ; source ID ; destination ID (0 if broadcast) ; data (if command bit 4 set) ; checksum/timestamp ; periodically will spin in a random direction then zig zag for a short distance ; also moves when light level changes ; male-male 'rutting': ; if two males directly face each other, hiss and flash red for a period then ; ram each other, submissive shows green, dominant flashes red/blue and beeps ; submissive remembers ID and shows green and submits when next encountered ; female-female: ; call and response chirruping with synchronised random colour bursts ; male-female: ; female chirrups and displays pink ; male clicks and displays blue, inches towards female ; beeping followed by synchronised colour fading when meet ; if isolated and no communication with others, after a while goes crazy ; then eventually sleeps ;************************************************************************** ; * ; Port assignments * ; * ;************************************************************************** PORTA_IO equ b'10111011' ; port A I/O status PORTB_IO equ b'00110001' ; port B I/O status #define LED_R PORTB,7 ; red LED #define LED_G PORTA,6 ; green LED #define LED_B PORTB,6 ; blue LED #define SPEAKER1 PORTB,1 ; piezo speaker #define SPEAKER2 PORTB,2 #define LDR PORTB,0 ; LDR #define PRESET PORTA,3 ; preset #define IR_RX PORTB,5 ; IR receiver #define IR_TX1_LO PORTA,7 ; left IR emitter - low power #define IR_TX1_HI PORTA,1 ; left IR emitter - high power #define IR_TX2_LO PORTA,0 ; right IR emitter - low power #define IR_TX2_HI PORTB,4 ; right IR emitter - high power #define MOTOR1 PORTA,2 ; left motor #define MOTOR2 PORTB,3 ; right motor SPEAKER_PORT equ PORTB SPEAKER_MASK equ b'00000110' PORTA_HI equ PORTA_IO&~(1<<1)|(1<<7)|(1<<0) PORTA_LO equ PORTA_IO&~(1<<7)&~(1<<0)|(1<<1) PORTB_HI equ PORTB_IO&~(1<<4) PORTB_LO equ PORTB_IO|(1<<4) LEDS_off macro bcf LED_R bcf LED_G bcf LED_B endm motors_off macro bcf MOTOR1 bcf MOTOR2 endm charge_ldr macro movlw PORTB_IO|(1<<0) tris_B endm discharge_ldr macro movlw PORTB_IO&~(1<<0) tris_B bcf LDR endm charge_preset macro movlw PORTA_IO|(1<<3) tris_A endm discharge_preset macro movlw PORTA_IO&~(1<<3) tris_A bcf PRESET endm ;************************************************************************** ; * ; Constants and timings * ; * ;************************************************************************** CLOCK equ d'4000000' ; processor clock frequency in Hz DUTY_CYCLE equ d'100' ; LED duty cycle frequency in Hz DUTY_STEPS equ d'128' ; number of duty cycle steps MAX_RGB equ d'127' ; maximum RGB HUE_STEPS equ d'6'*d'40' ; number of hue steps MALE_R equ d'0' ; blue MALE_G equ d'0' MALE_B equ d'127' FEMALE_R equ d'127' ; pink FEMALE_G equ d'30' FEMALE_B equ d'50' MAX_SPEED equ d'64' ; maximum motor speed ACCELERATION equ d'2' ; motor acceleration IR_CARRIER equ d'38' ; IR carrier frequency in kHz IR_BURST equ d'1000' ; IR carrier burst period in us IR_RECOVERY equ d'1' ; IR receiver recovery time in ms IR_START_ON equ d'2400' ; IR start bit on period in us IR_START_OFF equ d'600' ; IR start bit off period in us IR_DATA0_ON equ d'600' ; IR data 0 bit on period in us IR_DATA1_ON equ d'1200' ; IR data 1 bit on period in us IR_DATA_OFF equ d'600' ; IR data bit off period in us IR_GAP equ d'1200' ; IR inter-byte gap in us IR_TIMEOUT equ d'25' ; IR wait timeout in ms MOTION_MASK equ ~b'111' ; LDR motion detector mask DISCHARGE equ d'1' ; capacitor discharge period in ms MAX_NOTE equ d'6'*d'12' ; maximum speaker note DURATION equ h'c000' ; note duration MOD1_FRQ equ d'15' ; modulation frequency #1 MOD2_FRQ equ d'3' ; modulation frequency #2 CNTL_FRQ equ d'100' ; control frequency in Hertz CHIRRUP_PITCH equ d'27' ; chirrup pitch CHIRRUP_TIME equ d'35' ; chirrup duration in 1/100 s MSG_LEN equ d'5' ; message length (with data) MSG_NUM equ d'10' ; number of stacked messages SUB_NUM equ d'5' ; submission list size ; states STATE_NULL equ d'0' ; null STATE_M_M equ d'1' ; male-male STATE_M_F equ d'2' ; male-female STATE_F_M equ d'3' ; female-male STATE_F_F equ d'4' ; female-female ; message flags MSG_SEX equ 7 ; male/female MSG_SIDE equ 6 ; left/right MSG_POWER equ 5 ; high/low power ; message commands MSG_NULL equ h'00' ; null MSG_HELLO equ h'01' ; hello MSG_ACK equ h'02' ; acknowledge MSG_RAM equ h'03' ; ram MSG_SUBMIT equ h'04' ; submit MSG_HUE equ h'15' ; hue MSG_MATE equ h'16' ; mate MSG_MASK equ h'1f' ;************************************************************************** ; * ; File register usage * ; * ;************************************************************************** cblock h'20' ; bank 0 context:3 ; saved context ticks:2 ; clock ticks rtc ; real-time clock (counts 1/2 s) rtc_ ; previous value timer ; timer timeout ; timeout timer, -1 if disabled hue, saturation, value ; current colour in HSV space red, green, blue ; current colour in RGB space red_, green_, blue_ ; PWM counters sector, fraction ; colour wheel sector and fraction temp_R, temp_G, temp_B ; temporary RGB components duty ; duty cycle counter offset ; hue offset speed1, speed2 ; motor speeds (pwm duty cycles), 0 (off) to 64 (fully on) accel1, accel2 ; instantaneous motor speeds pitch:2 ; note pitch duration:2 ; note/chirrup duration mod1_cnt, mod2_cnt ; chirrup modulation counters message:MSG_LEN ; current message flags ; various flags ID ; unique random ID target ; target ID light ; light level state ; state bits ; bits count shift ; shift register command ; saved command/flags isolated ; isolated timer addr ; RAM/EEPROM address rand:2 ; random number arg1:2, arg2:2 ; arithmetic routine arguments count:2, loop, repeat ; scratch counters work1, work2, work3 ; work registers temp1, temp2 ; isr registers RAM0 endc if RAM0 > h'80' error "File register usage overflow" endif cblock h'a0' ; bank 1 context_:3 ; saved context messages:MSG_NUM*MSG_LEN ; message stack submission:SUB_NUM ; submission list RAM1 endc if RAM1 > h'f0' error "File register usage overflow" endif ; flags MALE equ 0 ; set if male TX_LFT equ 1 ; set if left IR transmitter TX_RGT equ 2 ; set if right IR transmitter TX_HI equ 3 ; set if high power transmitters FUND equ 4 ; chirrup fundamental flag MOD1 equ 5 ; chirrup modulation #1 flag MOD2 equ 6 ; chirrup modulation #2 flag STATIC equ 7 ; set if static LEDS macro r_,g_,b_ if r_ == 0 clrf red else movlf r_,red endif if g_ == 0 clrf green else movlf g_,green endif if b_ == 0 clrf blue else movlf b_,blue endif endm MOTORS macro l_,r_ if l_ == 0 clrf speed1 else movlf l_,speed1 endif if r_ == 0 clrf speed2 else movlf r_,speed2 endif endm ;************************************************************************** ; * ; Macros * ; * ;************************************************************************** routine macro label ; routine label endm table macro label ; define lookup table label addwf PCL endm entry macro value ; define table entry retlw value endm index macro label ; index lookup table call label endm jump macro label ; jump through table goto label endm tstw macro ; test w register iorlw 0 endm movff macro f1,f2 ; move file to file movfw f1 movwf f2 endm movlf macro n,f ; move literal to file movlw n movwf f endm tris_A macro ; tristate port A bsf STATUS,RP0 movwf TRISA bcf STATUS,RP0 endm tris_B macro ; tristate port B bsf STATUS,RP0 movwf TRISB bcf STATUS,RP0 endm option_ macro ; set options bsf STATUS,RP0 movwf OPTION_REG bcf STATUS,RP0 endm ;-------------------------------------------------------------------------- ; reset and interrupt service routine vectors ;-------------------------------------------------------------------------- org 0 goto main_entry org 4 goto isr ;************************************************************************** ; * ; Lookup tables * ; * ;************************************************************************** lookup movwf PCL ; look up table table fraction_tbl entry d'0' entry d'7' entry d'13' entry d'20' entry d'26' entry d'33' entry d'39' entry d'46' entry d'52' entry d'59' entry d'65' entry d'72' entry d'78' entry d'85' entry d'92' entry d'98' entry d'105' entry d'111' entry d'118' entry d'124' entry d'131' entry d'137' entry d'144' entry d'150' entry d'157' entry d'163' entry d'170' entry d'177' entry d'183' entry d'190' entry d'196' entry d'203' entry d'209' entry d'216' entry d'222' entry d'229' entry d'235' entry d'242' entry d'248' entry d'255' if 0 table note_tbl note_ macro freq period set (((CLOCK<<1)/freq)-NOTE_OVERHEAD+NOTE_LOOP/2)/NOTE_LOOP entry high period entry low period endm note_ d'435' ; 2 octaves below international A (435Hz) note_ d'461' ; A# note_ d'488' ; B note_ d'517' ; C note_ d'548' ; C# note_ d'581' ; D note_ d'615' ; D# note_ d'652' ; E note_ d'691' ; F note_ d'732' ; F# note_ d'775' ; G note_ d'821' ; G# endif ;************************************************************************** ; * ; Procedures * ; * ;************************************************************************** ;-------------------------------------------------------------------------- ; LED pwm control ;-------------------------------------------------------------------------- routine do_leds decfsz duty ; start of duty cycle ? goto dol1 ; branch if not movlf DUTY_STEPS,duty ; recharge counter movff red,red_ ; initialise PWM counters skpz bsf LED_R movff green,green_ skpz bsf LED_G movff blue,blue_ skpz bsf LED_B dol1 decf red_ ; red PWM skpnz bcf LED_R decf green_ ; green PWM skpnz bcf LED_G decf blue_ ; blue PWM skpnz bcf LED_B return ;-------------------------------------------------------------------------- ; dc motor pwm control ;-------------------------------------------------------------------------- do_motors macro swapf ticks+0,w andlw h'f0' movwf temp1 swapf ticks+1,w andlw h'0f' iorwf temp1 movfw temp1 ; left motor pwm andlw h'3f' subwf accel1,w skpnz clrc skpc bcf MOTOR1 skpnc bsf MOTOR1 movfw accel1 ; accelerate subwf speed1,w movlw ACCELERATION skpz addwf accel1 movfw speed1 subwf accel1,w skpnc subwf accel1 comf temp1,w ; right motor pwm andlw h'3f' subwf accel2,w skpnz clrc skpc bcf MOTOR2 skpnc bsf MOTOR2 movfw accel2 ; accelerate subwf speed2,w movlw ACCELERATION skpz addwf accel2 movfw speed2 subwf accel2,w skpnc subwf accel2 endm ;-------------------------------------------------------------------------- ; interrupt service routine ;-------------------------------------------------------------------------- routine isr movwf context+0 ; save context swapf STATUS,w clrf STATUS ; bank 0 movwf context+1 movff PCLATH,context+2 clrf PCLATH ; page 0 btfss PIR1,TMR1IF goto isr1 bcf PIR1,TMR1IF ; clear interrupt incf rtc ; increment real-time clock decf timeout ; decrement timeout timer if btfsc timeout,7 ; not disabled incf timeout goto isr2 isr1 bcf PIR1,TMR2IF ; clear interrupt incf ticks+1 ; increment clock ticks skpnz incf ticks+0 call do_leds ; LED pwm control movfw ticks+1 andlw h'0f' bnz isr2 do_motors ; dc motor pwm control isr2 movff context+2,PCLATH ; restore context swapf context+1,w movwf STATUS swapf context+0 swapf context+0,w retfie ;-------------------------------------------------------------------------- ; locks the LEDs ;-------------------------------------------------------------------------- routine lock_LEDS LEDS_off btfsc red,6 bsf LED_R btfsc green,6 bsf LED_G btfsc blue,6 bsf LED_B return ;-------------------------------------------------------------------------- ; generates a pseudo random number, returns the number in w reg ;-------------------------------------------------------------------------- routine get_random movfw rand+0 iorwf rand+1,w bnz getr1 movfw TMR0 ; seed generator movwf rand+1 xorlw h'ff' movwf rand+0 getr1 rlf rand+0,w ; calculate next in sequence xorwf rand+0,w movwf work1 ; msb <= Q15 ^ Q14 swapf rand+1,w btfsc rand+0,4 xorlw h'80' ; msb <= Q12 ^ Q3 xorwf work1 rlf work1 rlf rand+1 rlf rand+0 ; << 1 + (Q15 ^ Q14 ^ Q12 ^ Q3) movfw rand+1 ; w <= random number return ;-------------------------------------------------------------------------- ; short beep ;-------------------------------------------------------------------------- BEEP_PITCH equ d'35' ; beep pitch BEEP_PERIOD equ d'250' ; beep period routine beep_2 call beep movlw d'15' call wait routine beep bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS bcf SPEAKER1 bsf SPEAKER2 movlf BEEP_PERIOD,work1 beep1 movlf BEEP_PITCH,work2 ; half-cycle delay beep2 clrwdt decfsz work2 goto beep2 movlw SPEAKER_MASK ; toggle speaker output xorwf SPEAKER_PORT decfsz work1 goto beep1 bcf SPEAKER1 bcf SPEAKER2 bsf INTCON,GIE ; interrupts on return ;-------------------------------------------------------------------------- ; reads a byte from data memory, fed with the address in addr, returns ; the byte in w reg ;-------------------------------------------------------------------------- if 0 routine read_EEPROM movfw addr bsf STATUS,RP0 movwf EEADR bsf EECON1,RD ; read byte movfw EEDATA bcf STATUS,RP0 return endif ;-------------------------------------------------------------------------- ; writes a byte to data memory, fed with the address in addr and the ; byte in w reg ;-------------------------------------------------------------------------- if 0 routine write_EEPROM bsf STATUS,RP0 movwf EEDATA bcf STATUS,RP0 movfw addr bsf STATUS,RP0 movwf EEADR bcf INTCON,GIE ; write byte bsf EECON1,WREN movlf h'55',EECON2 movlf h'aa',EECON2 bsf EECON1,WR wr1 nop btfsc EECON1,WR goto wr1 bcf EECON1,WREN bcf STATUS,RP0 bsf INTCON,GIE return endif ;-------------------------------------------------------------------------- ; delay ;-------------------------------------------------------------------------- delay64 goto $+1 ; [8] goto $+1 ; [8] goto $+1 ; [8] goto $+1 ; [8] goto $+1 ; [8] goto $+1 ; [8] delay16 return ; [8] delay macro cycles ; delay for a number of cycles if (cycles) < 0 error "Delay cycles negative" endif variable i = cycles while i > 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 (at least) the number of ms in w reg ;-------------------------------------------------------------------------- routine wait_ms movwf work1 WAIT set CLOCK/(d'1000'*d'80') wait1 movlf WAIT,work2 wait2 clrwdt ; [4] delay d'64' ; [64] decfsz work2 ; [4] goto wait2 ; [8] decfsz work1 goto wait1 return ;-------------------------------------------------------------------------- ; waits for the number of 1/100 s in w reg ;-------------------------------------------------------------------------- routine short_wait movlw 1 goto wait routine wait_tenth movlw d'10' goto wait routine wait_half movlw d'50' goto wait routine wait_one movlw d'100' goto wait routine long_wait clrw routine wait movwf work1 wait3 movfw ticks+1 addlw (DUTY_CYCLE*DUTY_STEPS)/d'100' movwf work2 wait4 clrwdt movfw ticks+1 subwf work2,w bz wait5 decf ticks+1,w subwf work2,w bnz wait4 wait5 decfsz work1 goto wait3 return ;************************************************************************** ; I/O routines * ;************************************************************************** ;-------------------------------------------------------------------------- ; multiplication (8-bit arg1 * 8-bit arg2 >> 8 -> 8-bit arg1) ;-------------------------------------------------------------------------- routine mult movlf d'8',count movff arg1,shift movff arg2,arg2+1 clrf arg1+0 clrf arg1+1 clrf arg2+0 mult1 rrf shift bnc mult2 movfw arg2+1 addwf arg1+1 skpnc incf arg1+0 movfw arg2+0 addwf arg1+0 mult2 clrc rlf arg2+1 rlf arg2+0 decfsz count goto mult1 return ;-------------------------------------------------------------------------- ; converts HSV colour space to RGB colour space ;-------------------------------------------------------------------------- routine conv_RGB movfw value movwf temp_R movwf temp_G movwf temp_B tstf saturation ; special case bz conv8 movff hue,fraction ; determine the colour wheel sector (0 to 5) movlf -1,sector ; and fraction (0 to 39) movlw d'40' conv0 incf sector subwf fraction bc conv0 addwf fraction movfw fraction ; scale to 8-bit range index fraction_tbl movwf fraction incf saturation,w ; special case bnz conv1 incf value,w bnz conv1 clrf work1 comf fraction,w movwf work2 movff fraction,work3 goto conv2 conv1 comf saturation,w ; (value * (255 - saturation)) / 256 movwf arg1 movff value,arg2 call mult movff arg1,work1 movff saturation,arg1 ; (value * (255 - (saturation * fraction) / 256)) / 256 movff fraction,arg2 call mult comf arg1 movff value,arg2 call mult movff arg1,work2 comf fraction,w ; (value * (255 - (saturation * (255 - fraction)) / 256)) / 256 movwf arg1 movff saturation,arg2 call mult comf arg1 movff value,arg2 call mult movff arg1,work3 conv2 tstf sector bnz conv3 movff work3,temp_G ; sector 0 movff work1,temp_B goto conv8 conv3 decf sector bnz conv4 movff work2,temp_R ; sector 1 movff work1,temp_B goto conv8 conv4 decf sector bnz conv5 movff work1,temp_R ; sector 2 movff work3,temp_B goto conv8 conv5 decf sector bnz conv6 movff work1,temp_R ; sector 3 movff work2,temp_G goto conv8 conv6 decf sector bnz conv7 movff work3,temp_R ; sector 4 movff work1,temp_G goto conv8 conv7 movff work1,temp_G ; sector 5 movff work2,temp_B conv8 bcf INTCON,GIE clrc rrf temp_R,w movwf red clrc rrf temp_G,w movwf green clrc rrf temp_B,w movwf blue bsf INTCON,GIE return ;-------------------------------------------------------------------------- ; samples the LDR sensor, returns the result in w reg (0 to 255) ;-------------------------------------------------------------------------- routine get_ldr discharge_ldr ; discharge the capacitor movlw DISCHARGE call wait_ms clrwdt clrf count bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS charge_ldr ; charge the capacitor getl1 incf count ; [4] bz getl2 ; branch if timeout [8] delay d'8' ; [8] btfss LDR ; charged ? [4] goto getl1 ; loop if not [8] getl2 bsf INTCON,GIE ; interrupts on decf count,w return ;-------------------------------------------------------------------------- ; samples the preset, returns the result in w reg (0 to 255) ;-------------------------------------------------------------------------- routine get_preset discharge_preset ; discharge the capacitor movlw DISCHARGE call wait_ms clrwdt clrf count bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS charge_preset ; charge the capacitor getp1 variable n = d'7' while n > 0 incf count ; [4] btfsc PRESET ; charged ? [8] goto getp2 ; branch if yes n -= 1 endw incf count ; [4] bz getp2 ; branch if timeout [8] btfss PRESET ; charged ? [4] goto getp1 ; loop if not [8] getp2 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 = ((2*IR_BURST)/d'1000')+IR_RECOVERY routine test_obstacle bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS movlw PORTA_HI ; high or low power emitters btfss flags,TX_HI movlw PORTA_LO tris_A movlw PORTB_HI btfss flags,TX_HI movlw PORTB_LO tris_B 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'44' DELAY set (DELAY+2)&~3 testo1 btfsc flags,TX_LFT ; IR emitter on [8/4] bsf IR_TX1_HI ; [0/4] btfsc flags,TX_RGT ; [8/4] bsf IR_TX2_HI ; [0/4] btfsc flags,TX_LFT ; [8/4] bsf IR_TX1_LO ; [0/4] btfsc flags,TX_RGT ; [8/4] bsf IR_TX2_LO ; [0/4] delay DELAY ; half-cycle delay clrwdt ; [4] btfss IR_RX ; [8/4] incf count ; [0/4] btfsc flags,TX_LFT ; IR emitter off [8/4] bcf IR_TX1_HI ; [0/4] btfsc flags,TX_RGT ; [8/4] bcf IR_TX2_HI ; [0/4] btfsc flags,TX_LFT ; [8/4] bcf IR_TX1_LO ; [0/4] btfsc flags,TX_RGT ; [8/4] bcf IR_TX2_LO ; [0/4] delay DELAY ; half-cycle delay decfsz loop ; [4] goto testo1 ; [8] movlw ITERS/2 subwf count,w bnc testo3 clrf count ITERS set ((CLOCK/d'1000000')*IR_BURST)/d'56' movlf ITERS,loop testo2 clrwdt ; [4] btfss IR_RX ; [8/4] incf count ; [0/4] delay d'32' ; [32] decfsz loop ; [4] goto testo2 ; [8] movlw ITERS/2 subwf count,w bc testo3 bsf INTCON,GIE ; interrupts on movlw IR_RECOVERY ; receiver recovery time call wait_ms setc ; signal obstacle return testo3 bsf INTCON,GIE ; interrupts on clrc ; signal no obstacle return ;-------------------------------------------------------------------------- ; IR bit routines ;-------------------------------------------------------------------------- routine tx_on movwf work1 HALF set CLOCK/(2*IR_CARRIER*d'1000') HALF set (HALF+2)&~3 btfss flags,TX_HI goto ton2 ton1 btfsc flags,TX_LFT ; IR emitters on [8/4] bsf IR_TX1_HI ; [0/4] btfsc flags,TX_RGT ; [8/4] bsf IR_TX2_HI ; [0/4] clrwdt ; [4] delay HALF-d'20' ; half-cycle delay nop ; IR emitters off [4] bcf IR_TX1_HI ; [4] nop ; [4] bcf IR_TX2_HI ; [4] delay HALF-d'28' ; half-cycle delay decfsz work1 ; [4] goto ton1 ; [8] return ton2 btfsc flags,TX_LFT ; IR emitters on [8/4] bsf IR_TX1_LO ; [0/4] btfsc flags,TX_RGT ; [8/4] bsf IR_TX2_LO ; [0/4] clrwdt ; [4] delay HALF-d'20' ; half-cycle delay nop ; IR emitters off [4] bcf IR_TX1_LO ; [4] nop ; [4] bcf IR_TX2_LO ; [4] delay HALF-d'28' ; half-cycle delay decfsz work1 ; [4] goto ton2 ; [8] return ir_on macro us movlw us/((2*HALF)/(CLOCK/d'1000000')) call tx_on endm routine tx_off movwf work1 bcf IR_TX1_HI bcf IR_TX2_HI bcf IR_TX1_LO bcf IR_TX2_LO toff1 clrwdt ; [4] delay d'32' ; [32] decfsz work1 ; [4] goto toff1 ; [8] CYCLES set d'48' return ir_off macro us movlw us/(CYCLES/(CLOCK/d'1000000')) call tx_off endm ;-------------------------------------------------------------------------- ; transmits an IR byte, fed with the byte in w reg ;-------------------------------------------------------------------------- routine tx_byte movwf shift clrf work2 ir_on IR_START_ON ; start bit ir_off IR_START_OFF movlf d'8',bits txb1 rlf shift ; data bits bc txb2 ir_on IR_DATA0_ON ir_off IR_DATA_OFF goto txb3 txb2 ir_on IR_DATA1_ON ir_off IR_DATA_OFF incf work2 txb3 decfsz bits goto txb1 btfsc work2,0 ; parity bit goto txb4 ir_on IR_DATA0_ON goto txb5 txb4 ir_on IR_DATA1_ON txb5 ir_off IR_DATA_OFF ir_off IR_GAP return ;-------------------------------------------------------------------------- ; receives an IR byte, returns the byte in w reg with the C flag set ; if error ;-------------------------------------------------------------------------- routine rx_byte clrf count ; wait for start bit rxb1 clrwdt ; [4] incf count ; [4] bz rxb6 ; branch if timeout [8] delay d'40' ; [40] btfsc IR_RX ; [4] goto rxb1 ; [8] clrf count ; wait for end of start bit rxb2 clrwdt ; [4] incf count ; [4] bz rxb6 ; branch if timeout [8] delay d'24' ; [24] btfss IR_RX ; [4] goto rxb2 ; [8] clrf shift clrf work2 movlf d'8'+1,bits rxb3 clrf count ; wait for high clrwdt rxb4 incf count ; [4] bz rxb6 ; branch if timeout [8] btfsc IR_RX ; [4] goto rxb4 ; [8] clrf count ; wait for low clrwdt rxb5 incf count ; [4] bz rxb6 ; branch if timeout [8] btfss IR_RX ; [4] goto rxb5 ; [8] CYCLES set d'24' MID set ((IR_DATA0_ON+IR_DATA1_ON)/2)/((CYCLES*d'1000000')/CLOCK) movlw MID subwf count,w rlf shift rrf shift,w skpnc incf work2 decfsz bits goto rxb3 btfsc work2,0 ; test parity bit goto rxb6 clrc ; signal no error return rxb6 setc ; signal error return ;************************************************************************** ; message routines * ;************************************************************************** ;-------------------------------------------------------------------------- ; calculates the checksum for the current message, returns the checksum ; in w reg ;-------------------------------------------------------------------------- routine get_checksum movfw message+0 variable n = 1 while n < MSG_LEN-2 xorwf message+n,w n += 1 endw btfsc message+0,4 xorwf message+n,w return ;-------------------------------------------------------------------------- ; transmits a message ;-------------------------------------------------------------------------- routine tx_message bcf message+0,MSG_SEX btfsc flags,MALE bsf message+0,MSG_SEX bcf message+0,MSG_POWER btfsc flags,TX_HI bsf message+0,MSG_POWER call get_checksum ; calculate checksum btfss message+0,4 movwf message+3 btfsc message+0,4 movwf message+4 movlw PORTA_HI ; high or low power emitters btfss flags,TX_HI movlw PORTA_LO tris_A movlw PORTB_HI btfss flags,TX_HI movlw PORTB_LO tris_B bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS movlf message,FSR ; transmit bytes movlf MSG_LEN,repeat btfss message+0,4 decf repeat txm1 movfw 0 call tx_byte incf FSR decfsz repeat goto txm1 bsf INTCON,GIE ; interrupts on return routine tx_left bsf flags,TX_LFT ; transmit on left side bcf flags,TX_RGT bsf message+0,MSG_SIDE goto tx_message routine tx_right bsf flags,TX_RGT ; transmit on right side bcf flags,TX_LFT bcf message+0,MSG_SIDE goto tx_message routine tx_both bsf flags,TX_RGT ; transmit on left and right sides bsf flags,TX_LFT call get_random andlw 1<>8 movlf ITERS,count+0 ; wait for start bit clrf count+1 rxm1 clrwdt ; [4] tstf count+1 ; [4] skpnz ; [8] decf count+0 decf count+1 ; [4] movfw count+0 ; [4] iorwf count+1,w ; [4] bz rxm5 ; branch if timeout [8] btfsc IR_RX ; [4] goto rxm1 ; [8] bcf INTCON,GIE ; interrupts off motors_off call lock_LEDS movlf message,FSR ; receive bytes movlf MSG_LEN-1,repeat rxm2 call rx_byte bc rxm4 ; branch if error movwf 0 incf FSR decfsz repeat goto rxm2 btfss message+0,4 goto rxm3 call rx_byte bc rxm4 movwf 0 incf FSR rxm3 call get_checksum ; test checksum decf FSR xorwf 0,w bnz rxm4 clrc ; signal no error clrz bsf INTCON,GIE ; interrupts on return rxm4 setc ; signal error clrz bsf INTCON,GIE ; interrupts on return rxm5 setz ; signal timeout clrc return ;-------------------------------------------------------------------------- ; returns the message command in w reg ;-------------------------------------------------------------------------- routine get_command movfw message+2 ; correct destination ? skpz subwf ID,w movlw MSG_NULL bnz getc1 ; branch if not movfw message+0 ; message command andlw MSG_MASK getc1 return ;-------------------------------------------------------------------------- ; copies the message stack to the current message, fed with FSR pointing ; to the stack, returns FSR advanced ;-------------------------------------------------------------------------- routine get_message variable n = 0 while n < MSG_LEN movff 0,message+n incf FSR n += 1 endw return ;-------------------------------------------------------------------------- ; copies the current message to the message stack, fed with FSR pointing ; to the stack, returns FSR advanced ;-------------------------------------------------------------------------- routine put_message variable n = 0 while n < MSG_LEN movff message+n,0 incf FSR n += 1 endw return ;-------------------------------------------------------------------------- ; clears the message stack ;-------------------------------------------------------------------------- routine clear_messages movlf messages,FSR movlf MSG_NUM*MSG_LEN,count clrm1 clrf 0 incf FSR decfsz count goto clrm1 return ;-------------------------------------------------------------------------- ; pushes the current message onto the stack ;-------------------------------------------------------------------------- routine push_message movlf messages+MSG_NUM*MSG_LEN,FSR movlf (MSG_NUM-1)*MSG_LEN,count pushm1 movlw MSG_LEN+1 subwf FSR movff 0,work1 movlw MSG_LEN addwf FSR movff work1,0 decfsz count goto pushm1 movff rtc,message+4 ; set timestamp movlf messages,FSR goto put_message ;-------------------------------------------------------------------------- ; expires old messages on the stack ;-------------------------------------------------------------------------- routine expire_messages ; message lifetime in seconds MSG_LIFETIME equ d'6' movlf messages+4,FSR movlf MSG_NUM,count expir1 movfw 0 subwf rtc,w sublw d'2'*MSG_LIFETIME ; message expired ? bc expir2 ; branch if not movlw 4 subwf FSR clrf 0 ; discard message addwf FSR expir2 movlw MSG_LEN addwf FSR decfsz count goto expir1 return ;-------------------------------------------------------------------------- ; acknowledges a message ;-------------------------------------------------------------------------- routine ack_message movlw d'2' call wait_ms movlf MSG_ACK,message+0 ; command movff message+1,message+2 ; destination ID movff ID,message+1 ; source ID goto tx_both ; acknowledge ;-------------------------------------------------------------------------- ; waits for acknowledge, returns the NZ flag set if received ;-------------------------------------------------------------------------- routine wait_ack call rx_message ; wait for message bz wack1 bc wack1 ; branch if timeout or error call get_command ; acknowledge ? sublw MSG_ACK bnz wack1 ; branch if not clrz ; signal acknowledged return wack1 setz ; signal not acknowledged return ;-------------------------------------------------------------------------- ; tests for the presence of a male or female, returns the NZ flag set ; if detected ;-------------------------------------------------------------------------- routine test_male movlw 1 goto test0 routine test_female clrw test0 movwf work1 movlf messages,FSR movlf MSG_NUM,count test1 call get_message tstf message+0 ; null message ? bz test5 ; branch if yes movfw message+0 ; correct sex ? tstf work1 skpnz comf message+0,w andlw 1< 0 call ack_message call wait_tenth n -= 1 endw movfw target ; add to submission list call push_submission if 0 call get_random ; veer to the right or left andlw h'80' movlw 0 skpz movwf speed1 skpnz movwf speed2 VEER equ d'150' movlw VEER call wait endif goto submit ; submissive ram4 MOTORS 0,0 call wait_half movlf d'20',repeat ; dominant - flash and beep ram5 call beep LEDS 0,0,MAX_RGB call wait_tenth call beep LEDS MAX_RGB,0,0 call wait_tenth decfsz repeat goto ram5 ram6 MOTORS 0,0 LEDS 0,0,0 call long_wait call long_wait goto wander ;-------------------------------------------------------------------------- ; male-male submit ;-------------------------------------------------------------------------- routine submit MOTORS 0,0 LEDS 0,MAX_RGB/4,0 call long_wait call long_wait goto main_loop ;-------------------------------------------------------------------------- ; male-female mate ;-------------------------------------------------------------------------- routine mate MATE_REPEAT equ d'6' MOTORS 0,0 LEDS 0,0,0 movlf MATE_REPEAT*2,repeat ; beep mate1 call beep call wait_tenth decfsz repeat goto mate1 clrf hue movlf MATE_REPEAT,repeat mate2 call wait_tenth clrf saturation movlf d'255',value mate3 decf saturation ; fade to white call conv_RGB call short_wait decfsz saturation goto mate3 movfw offset addwf hue movlw HUE_STEPS subwf hue,w skpnc movwf hue mate4 incf saturation call conv_RGB call short_wait incfsz saturation goto mate4 movlf d'255',saturation clrf value mate5 decf value ; fade to black call conv_RGB call short_wait decfsz value goto mate5 call wait_tenth movfw offset addwf hue movlw HUE_STEPS subwf hue,w skpnc movwf hue mate6 incf value call conv_RGB call short_wait incfsz value goto mate6 decfsz repeat goto mate2 LEDS 0,0,0 call long_wait call long_wait goto wander ;************************************************************************** ; EEPROM data * ;************************************************************************** org h'2100' end