;********************************************************************** ; ; Filename: scout_6.asm ; Date: 09/06/2003 ; ; Author: Scott Martin ; ; ;********************************************************************** ; ; Files required: p16f877.inc,scout_6.asm ; ;********************************************************************** ; ;Notes: ;Scout Pin Ref ;RA5 - unused for now, set as outputs, output-low ;RA4 ;RA3 ;RA2 ;RA1 ;RA0 ; ; ;All pins PortB set to inputs, using PortB pull-ups. ;RB7 - unused ;RB6 - unused ;RB5 - unused ;RB4 - unused ;RB3 - unused ;RB2 - unused ;RB1 - unused ;RB0 - unused ; ; ;RC7 - RX - UART ;RC6 - TX - UART ;RC5 - unused ;RC4 - unused ;RC3 - Peizo Buzzer - use for sounds ;RC2 - HeartBeat1, monitors BC2 Output( front-view,front-right), set as input ;RC1 - HeartBeat2, monitors Output from a second heart beat source. ;RC0 - unused ;future SPI uses pins RC3-RC5, for now, make them outputs and set to LOW ; ; ;RD7 - Compass N ;set to inputs, ;RD6 - Compass E ;signal from compass has 47K pull-ups. ;RD5 - Compass S ;active low ;RD4 - Compass W ;RD3 - Right Sensor, Output - Low=ON High=OFF, as Input - read sensor ;RD2 - Left Sensor, Output - Low=ON High=OFF, as Input - read sensor ;RD1 - HeartBeat1 enable - output bit high=off, output bit low=on ;RD0 - HeartBeat2 enable - output bit high=off, output bit low=on ; ; ;RE2 - unused for now ;RE1 ;RE0 ; ; ;********************************************************************** list p=16f877 ; list directive to define processor #include ; processor specific variable definitions __CONFIG _BODEN_OFF&_CP_OFF&_WRT_ENABLE_ON&_PWRTE_ON&_WDT_OFF&_HS_OSC&_DEBUG_OFF&_CPD_OFF&_LVP_OFF ; '__CONFIG' directive is used to embed configuration data within .asm file. ; The labels following the directive are located in the respective .inc file. ; See respective data sheet for additional information on configuration word. ;*** Scout2 dependent constant declarations **************************** ; ;***Define Default Port Constants. IO_SETA equ B'00000000' ;set porta as outputs IO_SETB equ B'11111111' ;all set to inputs IO_SETC equ B'10000110' ;RC7,RC2,and RC1 are inputs, the rest -outputs IO_SETD equ B'11111100' IO_SETE equ B'00000000' ;Symbols for Port C usage. RX equ 7 ;RC7 - uart Receive TX equ 6 ;RC6 - uart Transmit SOUND equ 3 ;RC3 - piezo (for now), HEARTBT1 equ 2 ;RC2 - output from master bicore HEARTBT2 equ 1 ;RC1 - output from heartbeat2 bicore ;Symbols for Port D usage. NORTH equ 7 ;RD4 - RD7 input from compass EAST equ 6 SOUTH equ 5 WEST equ 4 RSENSOR equ 3 ;RD3 - Right Tactile Sensor (input/output) LSENSOR equ 2 ;RD2 - Left Tactile Sensor (input/output) ENB1 equ 1 ;RD1 - Enable HeartBeat1 ( bc2 output, walk) ENB2 equ 0 ;RD0 - Enable HeartBeat2 ;Symbols for Port B usage. ; ; ; ;Flags for Current Bot_States - cbot_state ;Current States C_FWD equ 0 ;-forward C_REV equ 1 ;-reverse C_TURN equ 2 ;-turning -using rule C_STOP equ 3 ;-stopped ;Flags for Next Bot States - nbot_state N_FWD equ 0 ;-forward N_REV equ 1 ;-reverse N_TURN equ 2 ;-turning -using rule N_STOP equ 3 ;-stop ;Flags for Bot Mode - rule_state X_HAND_R equ 1 ;X-hand rule select - must set LHR, RHR WSLEEP equ 2 ;sleep-wander aimlesslly TRACK equ 3 ;track compass ;Flags for Tactile Sensors - tact_state WNDR equ 0 RHR equ 1 LHR equ 2 RS_ON equ 6 ;right sensor flag, pic controll ON=1 LS_ON equ 7 ;left sensor flag, pic controll ON=1 CPNW equ 0 ;index for compass variables CPW equ 1 CPSW equ 2 CPS equ 3 CPSE equ 4 CPE equ 5 CPNE equ 6 CPN equ 7 ;**************************************************************************** ;Macros to select the register bank ;Used for clarity more than effeciency. Bank0 MACRO ;macro to select data RAM bank 0 bcf STATUS,RP0 bcf STATUS,RP1 ENDM Bank1 MACRO ;macro to select data RAM bank 1 bsf STATUS,RP0 bcf STATUS,RP1 ENDM Bank2 MACRO ;macro to select data RAM bank 2 bcf STATUS,RP0 bsf STATUS,RP1 ENDM Bank3 MACRO ;macro to select data RAM bank 3 bsf STATUS,RP0 bsf STATUS,RP1 ENDM ;************ VARIABLE DEFINITIONS (for interupt handling) ***************** ; CBLOCK 0x20 w_temp ;variables in bank0 used for context saving status_temp pclath_temp ;**** Scout2 dependent variables in bank0 pstate pcount fstop rule_state cbot_state nbot_state tact_state xhand fhand sfreq stime timer stopchk mosc duscount dmscount ms250cnt quarter compass cp_scratch ENDC ;Dummy variables in bank 1 CBLOCK 0xA0 w_temp1 ; GPR place holder for context saving st_temp1 ; if w_temp is reserved at 0x20 then GPR pc_temp1 ; 0xA0 must be reserved as well (interrupts) ENDC ;Dummy variables in bank 2 CBLOCK 0x120 w_temp2 ; GPR place holder for context saving st_temp2 ; if w_temp is reserved at 0x20 then GPR pc_temp2 ; 0x120 must be reserved as well (interrupts) ENDC ; ;********************************************************************** ;********************************************************************** ; program starts at 03h, interupt starts at 04h ; org 3 goto start ; jump over interupt code to begining of program ; interrupt vector location interupt movwf w_temp ; save off current W register contents movf STATUS,W ; move status register into W register Bank0 ; ensure file register bank set to 0 movwf status_temp ; save off contents of STATUS register movf PCLATH,W movwf pclath_temp ; save off current state of PCLATH movlw high isr_handler ; read upper 8 bits of address for ISR_HANDLER movwf PCLATH ; initialize PCLATH register call isr_handler ; call the interrupt service code movf pclath_temp,W ; retrieve copy of pre-interrupt PCLATH register movwf PCLATH ; restore pre-isr PCLATH register Bank0 ; ensure file register bank set to 0 movf status_temp,W ; retrieve copy of STATUS register movwf STATUS ; restore pre-isr STATUS register contents swapf w_temp,F ; swapf w_temp,W ; restore pre-isr W register contents retfie ; return from interrupt ;*************** interrupt service code goes here *************************** isr_handler nop return ; return to INT_VECTOR code section ;*************** Subroutine Code Starts Here ********************************* ;***************************************************************************** ; delay10us , mosc is the modifier for clock cycle values. ;mosc=1 (4Mhz) - will produce a 12 usec delay ;mosc=2 (8Mhz) - gives a 10.5 usec delay ;mosc=4 (16Mhz) - gives a 9.75 usec delay ;mosc=5 (20Mhz) - gives a 9.6 usec delay ;the delay value includes the call-to and the return-to ;***************************************************************************** delay10us movf mosc,W movwf duscount dlp1 nop nop nop nop nop nop decfsz duscount,F goto dlp1 return ;***************************************************************************** ;delay1ms, uses delay10us so 1ms= 1.01ms at 20Mhz, to 1.49ms at 4Mhz. ; ;***************************************************************************** delay1ms movlw H'64' ;loop 100 times movwf dmscount dlp2 call delay10us decfsz dmscount,F goto dlp2 return ;***************************************************************************** ;delay250ms - 250ms times the value user places in W register before call ;***************************************************************************** delay250ms movwf quarter ;grab the multiplier for 250ms from W dlp4 movlw H'FA' movwf ms250cnt dlp3 call delay1ms ;250 * 1ms loop decfsz ms250cnt,F goto dlp3 decfsz quarter goto dlp4 return ;***************************************************************************** ;beep - send a tone to piezo. ; values for stime and sfreq are set for 20Mhz osc clock ; ;***************************************************************************** ; beep movlw H'FA' ;value for tone duration movwf stime movlw H'0E' ;value for freq timer movwf sfreq sloop decfsz stime,F goto cont goto dones cont clrf TMR0 ;zero TMR0 and prescaler movlw H'01' movwf TMR0 bcf STATUS,Z bsf PORTC,SOUND onlp movf TMR0,W ;ON loop subwf sfreq,W btfss STATUS,Z goto onlp clrf TMR0 ;zero TMR0 and prescaler movlw H'01' movwf TMR0 bcf STATUS,Z bcf PORTC,SOUND offlp movf TMR0,W ;OFF loop subwf sfreq,W btfss STATUS,Z goto offlp goto sloop dones return ;************************************************************************************ ;read_cmps - copies portd into cp_scratch, then parses the 4 bits from portd into ; an 8 bit value for compass. ; Remember...active = low from compass to input, otherwise..high(pull-ups) ;************************************************************************************ read_cmps movf PORTD,W movwf cp_scratch nop clrf compass btfsc cp_scratch,NORTH ;N,NW,NE? goto chk_south btfsc cp_scratch,WEST ;NW? goto chk_ne bsf compass,CPNW ;=NW goto cmpfin chk_ne btfsc cp_scratch,EAST ;NE? goto is_north bsf compass,CPNE ;=NE goto cmpfin is_north bsf compass,CPN ;=N goto cmpfin chk_south btfsc cp_scratch,SOUTH ;S,SW,SE? goto eastwest btfsc cp_scratch,WEST ;SW? goto chk_se bsf compass,CPSW ;=SW goto cmpfin chk_se btfsc cp_scratch,EAST ;SE? goto is_south bsf compass,CPSE ;=SE goto cmpfin is_south bsf compass,CPS ;=S goto cmpfin eastwest btfsc cp_scratch,WEST ;E,W? goto is_east bsf compass,CPW ;=W goto cmpfin is_east bsf compass,CPE ;=E cmpfin return ;*********************************************************************************** ;These routines control the enables of the bicores Heartbeat1 and Heartbeat2 ;*********************************************************************************** ; ;all_off - turns all bicore enables to OFF ; all_off bsf PORTD,ENB1 ;turn motor enable off nop bsf PORTD,ENB2 ;turn L/W enable off nop return ;lw_on - turns enable for listen/walk bicore cycle to ON ; lw_on bcf PORTD,ENB2 ;turn L/W enable ON nop nop return lw_off bsf PORTD,ENB2 ;turn L/W enable OFF nop nop return ;m_on - turns motor enable ON for all bc'2 that control the legs. ; should not be enabled when lw_on cycle has been called. ; m_on bcf PORTD,ENB1 ;turn motor enables ON nop nop return m_off bsf PORTD,ENB1 ;turn motor enables OFF nop nop return ;********************************************************************** ;These are the routines used for monitoring and controlling the IMUX ;********************************************************************** ; ;set_right - set pin as output from PIC, SET LEFT channel of IMX low, so bot TURNS RIGHT ; set_right Bank1 ;switch to bank 1 bcf PORTD,LSENSOR ;set tris bit for output to IMX Bank0 ;switch to bank 0 nop bcf PORTD,LSENSOR ;set left channel of IMX low nop bsf tact_state,LS_ON ;set flag - left sensor ON return ;clr_right - sets pin back to input from IMX ; clr_right Bank1 ;switch to bank 1 bsf PORTD,LSENSOR ;set tris bit back to input from IMX nop Bank0 ;switch bank 0 bcf tact_state,LS_ON ;clear flag - left sensor is OFF nop return ;set_tleft - set pin as output from PIC, SET RIGHT channel of IMX low, so bot TURNS LEFT ; set_left Bank1 ;switch to bank 1 bcf PORTD,RSENSOR ;set tris bit for output to IMX Bank0 ;switch to bank 0 nop bcf PORTD,RSENSOR ;set right channel of IMX low nop bsf tact_state,RS_ON ;set flag - right sensor ON return ;clr_left - sets pin back to input from IMX ; clr_left Bank1 ;switch to bank 1 bsf PORTD,RSENSOR ;set tris bit back to input from IMX nop Bank0 ;switch bank 0 bcf tact_state,RS_ON ;clear flag - right sensor is OFF nop return ;************************************************************************************* ;swprule - flips Right Hand rule to Left Hand rule and visa-versa. ;************************************************************************************* ; swprule btfss tact_state,RHR goto flptoR flptoL bcf tact_state,RHR nop bsf tact_state,LHR goto dflip flptoR bcf tact_state,LHR nop bsf tact_state,RHR dflip return ;******************************************************************************************** ;SENSOR CHECK ;senschk - poll the tactile sensors (IMX) and check the bots status. ; ;******************************************************************************************** ; senschk btfsc nbot_state,N_REV ;if N_REV has not been processed then skip senschk goto getout btfss cbot_state,C_TURN ;is bot in a turn? goto norm ;no - then continue normal check btfsc tact_state,LHR ;check which rule is applied goto doLHT btfsc tact_state,RHR goto doRHT ;doRHT and doLHT poll the opposite tactile goto norm ;all other rules go to norm doLHT btfsc PORTD,LSENSOR ;is left sensor low too? goto getout goto dorev ;then we are in reverse doRHT btfsc PORTD,RSENSOR ;is right sensor low too? goto getout goto dorev ;then we are in reverse norm btfsc PORTD,LSENSOR ;check left sensor - is it low? goto getout ;no - then check if done btfsc PORTD,RSENSOR ;check right sensor - is it low? goto getout ;no - then check if done dorev bsf nbot_state,N_REV ;set flags in nbot_state nop bsf nbot_state,N_STOP ;next action movlw 0x06 ;set pcount to six steps back movwf pcount bsf cbot_state,C_REV ;normally C_REV is set in revset but the tactile ;sensors have already put us in reverse. getout return ;************************************************************************************ ;STARTUP and INITIALIZE ;************************************************************************************ start nop Bank1 ;set to bank 1 movlw B'01010111' ;Port B pull-ups enabled movwf OPTION_REG ;interupt edge select rb0/int- don't care ;TMR0 Clock Source - Internal,clockout ;TMR0 Source Edge Select - low-to-high ;prescaler assigned to Timer0 ;setup option for TMR0 -prescale 1:25 movlw B'00000110' ;setup porta as digital IO movwf ADCON1 ;init tris registers ;setup input/output directions ; movlw IO_SETA movwf PORTA movlw IO_SETB movwf PORTB movlw IO_SETC movwf PORTC ;initial state - sensor output set to input movlw IO_SETD movwf PORTD movlw IO_SETE movwf PORTE ;init port registers and variables in bank 0 ;make sure all unused pins set to output-low,except portb pins ;which are inputs with pull-ups ;might have to add nops here Bank0 ;select bank 0 clrf PORTA ;clear all porta outputs,low bcf PORTC,0 ;rc0,rc4,rc5 - output,low bcf PORTC,4 bcf PORTC,5 bcf PORTE,0 ;re0,re1,re2 - output,low bcf PORTE,1 bcf PORTE,2 call all_off ;initial state of motor enables to off until ; dip switches have been polled in setrule clrf pstate ;reset bot-states. clrf cbot_state clrf nbot_state clrf rule_state clrf tact_state movlw H'05' ;setting the delay modifier for 20mhz, mosc=5 movwf mosc movlw 0x08 ;setup 2 second delay for electronics to settle call delay250ms ; call beep ;signal start ;************************************************************************************* ;poll tactile sensors on startup so user can set LHR, RHR, WSLEEP, or TRACK ; the tactile sensors are acting like 'dip switches' for us set_X_hand movlw 0x08 ;setup 2 second delay so user can activate sensors call delay250ms call beep ;let user know sensor start of read movlw 0x08 ;time for tactile debounce call delay250ms movf PORTD,W ;read portd to w register movwf xhand ;make copy of portd to xhand call beep ;let user know end of read,.. double beep movlw 0x02 ;delay half+ of second. call delay250ms call beep ;make beep again. movlw 0x08 ;setup 2 second delay for user to release sensors call delay250ms ;just incase user has not let go yet. btfss xhand,LSENSOR ;is left sensor low=ON, high=OFF goto xRH_or_WNDR xLH_or_T btfss xhand,RSENSOR ;is right sensor low,high goto xdo_LH bsf rule_state,TRACK ;set for TRACK compass = 1,1(OFF,OFF) nop movlw B'11011101' ;Compass defaults South, active low movwf compass ; goto finxh xdo_LH bsf tact_state,LHR ;set Left-Hand Rule = 1,0 nop bsf rule_state,X_HAND_R goto finxh xRH_or_WNDR btfss xhand,RSENSOR goto xdo_WNDR bsf tact_state,RHR ;sets Right-Hand Rule = 0,1 nop bsf rule_state,X_HAND_R goto finxh xdo_WNDR bsf rule_state,WSLEEP ;sets WANDER = 0,0 (ON,ON) nop bsf tact_state,WNDR finxh bsf nbot_state,N_FWD ;setup default start direction for bot call m_on ;done polling - turn motor bicores on. ;********************************************************************************************** ;SET ACTION ;********************************************************************************************** setact nop ;TRACK track_C btfss rule_state,TRACK ;are we using compass tracking? goto stopset ;no? next? ;stop bot, 5 sec delay call m_off ;use m_off for now,...turn off all enables movlw 0x14 ;setup 5 second delay for compass to settle call delay250ms call read_cmps ;sample compass, ;and adjust previous and current movlw 0x14 ;set up count for 20 steps forward movwf pcount ;normal forward state call m_on ;turn enable back on and continue goto heart_b1 ;STOP stopset btfss nbot_state,N_STOP ;look for stop action initiated by going into reverse goto revset ; call m_off ;use m_off for now,...turn off all enables movlw 0x08 ;setup 2 second delay call delay250ms ;wait clrf nbot_state ;reset nbot_state nop ;Clear action C_REV and C_TURN clrf cbot_state ;reset so RHR or LHR dosn't take control and put us ;into reverse - we are turning against the Rule Set. call swprule ;swap the X_Hand Rule set. bsf cbot_state,C_STOP ;flag C_STOP so fwdset can swap back X_hand. nop bsf nbot_state,N_TURN ;setup next action after this call m_on ;use m_on for now, ...turn leg motors back on goto setact ;stop uses a timer delay not heartbeat, so go back to setact ;REVERSE revset btfss nbot_state,N_REV ;check for reverse action goto fwdset ;skip to forward set clrf nbot_state ;reset nbot_state btfsc cbot_state,C_REV ;was bot just in rev? Yes,then get out goto setnxt ;setup next action, then back to setact movlw 0x06 ;set six steps back movwf pcount ;set pcount for reverse action nop bsf cbot_state,C_REV ;set current action nop bsf nbot_state,N_STOP ;set next action call set_right ;bring both IMX channels low to start reverse call set_left goto heart_b1 setnxt bsf nbot_state,N_STOP ;set next action goto setact ;go back to setact and execute N_STOP ;FORWARD fwdset btfss nbot_state,N_FWD ;check for forward action goto trnset clrf nbot_state ;reset nbot_state nop btfss cbot_state,C_STOP ;check for a recent past STOP goto wsleep call swprule ;restore proper X_HAND. clrf cbot_state ;clear C_STOP and C_TURN wsleep btfss rule_state,WSLEEP ;are we sleep-wandering? goto nforwd ;normal forward movlw 0xFF ;load max value for wander movwf pcount bsf cbot_state,C_FWD nop bsf nbot_state,N_FWD call swprule goto heart_b1 nforwd movlw 0x0C ;set up count for 12 steps forward movwf pcount ;normal forward state bsf cbot_state,C_FWD ;set current action nop bsf nbot_state,N_TURN ;next action goto heart_b1 ;TURN trnset btfss nbot_state,N_TURN ;check for turn action goto setact ;if it got here something was missed - go back and check clrf nbot_state ;reset nbot_state nop bcf cbot_state,C_FWD movlw 0x05 ;set up count for turn steps movwf pcount bsf cbot_state,C_TURN ;set current action nop bsf nbot_state,N_FWD ;next action btfsc tact_state,RHR ;skip if not righthand rule call set_right btfsc tact_state,LHR ;skip if not lefthand rule call set_left ;fall thru to heart_b1 ;**************************************************************************************** ;HEARTBEAT ; pcount is the state duartion counter, and is sync'd ; to each walk cycle, one tick of a bicore heart beat. ;**************************************************************************************** ;heart_b1 - Count pulses from Bicore Output. heart_b1 incf pcount,F ;pre inc value for loop because pcount just ploop decfsz pcount,F ;might be equal to zero. make sure. goto bc_on goto donep bc_on call senschk ;check sensor activity. btfss PORTC,HEARTBT1 ;wait for a pulse to sync, then skipover. goto bc_on ;loop until you find one bsf pstate,0 ;set pstate=1, pulse is on btfss cbot_state,C_REV ;is the current state a REV action goto rd_bc ; no? skip to rd_bc call beep ; for each reverse step, make beep rd_bc call senschk ;check sensor activity btfss PORTC,HEARTBT1 bcf pstate,0 ;set pstate=0, pulse is off btfss pstate,0 ;if pstate=1 then skip to rd_bc goto ploop ;else goto rd_bc donep btfsc tact_state,LS_ON ;are tactile sensors on? call clr_right ;then turn them off btfsc tact_state,RS_ON call clr_left goto setact ;next action END ; of program