* Filename : FOLLOW.ASM * Programmer : Michael Hattermann * Date : March 31, 2002 * Version : 1.0 * Description : This file contains the code for * object following. The following * functions are available: * * * *#define __DEBUGFOLLOW_ 1 *#define __PRINTFOLLOW_ 1 #include "hc12.asm" * ************************************************************ * Object Following Equates ************************************************************ * MINFOLLOW EQU $55 ; minimum reading on sensor to execute following OBJAVOIDSPD EQU HALFSPEED ; speed to perform object avoidance at MINFOLLOWSPD EQU HALFSPEED ; minimum speed to follow at MAXFOLLOWSPD EQU FULLSPEED ; maximum speed to follow at FOLLOWTURNSPD EQU _5_8_SPEED ; max speed to perform a hard turn at NUMAVGCNT EQU 20 ; number of sensor values to average SPDACCEL EQU 1 ; desired amount of acceleration SPDDOWNACCEL EQU -2 ; slow down acceleration SPDUPACCEL EQU 2 ; speed up acceleration * ************************************************************ * Object Following Debug Code ************************************************************ * #ifdef __DEBUGFOLLOW_ ORG USERPROG_PVECT JMP TEST ORG $B000 TEST LDAA #$00 ; turn off COP watchdog timer STAA COPCTL LDS #$0bff ; init the stack pointer HERE BRA HERE ; end of test program CFVALUE DC.B $00 ; center follow sensor value LFVALUE DC.B $00 ; left follow sensor value RFVALUE DC.B $00 ; right follow sensor value FTBLIDX DC.B $00 ; index into reaction table LASTFOLLOW DC.B $00 ; last follow direction OLDCFAVG DC.W $0000 ; old average of center follow sensor values NEWCFAVG DC.W $0000 ; new average of center follow sensor values AVGCNT DC.B $0000 ; number of items in new average #include "atd.asm" #include "pwm.asm" #endif * Reaction table for following (Center|Left|Right) ************************************************************ FOLLOWTBL DC.W FOBJAVOID ; 000 - Object avoidance DC.W FHARDR ; 001 - Hard right DC.W FHARDL ; 010 - Hard left DC.W FCONT ; 011 - ERROR - do what we did last DC.W FFOWARD ; 100 - Foward DC.W FSOFTR ; 101 - Soft right DC.W FSOFTL ; 110 - Soft left DC.W FCONT ; 111 - ERROR - do what we did last * ******************************************************************************* * SUBROUTINE - FOLLOW * Description: Performs obstacle following behavior by reading the values * from the IR sensors and moving the robot accordingly. * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * FOLLOW PSHX ; save register X PSHD ; save register D * JSR BUMPED ; check to see if we bumped something * TBEQ A,FOLLOW1 ; no bump, continue following behavior * JMP MAINOUT ; we bumped something, quit program FOLLOW1 JSR FGETDATA ; get data from following sensors #ifdef __PRINTFOLLOW_ PSHD ; save reg D PSHX ; save reg X LDAA LFVALUE ; print left value JSR OUTNUM LDAA #$20 ; print space JSR OUTCHAR LDAA CFVALUE ; print center value JSR OUTNUM LDAA #$20 ; print space JSR OUTCHAR LDAA RFVALUE ; print right value JSR OUTNUM LDX #NEWLINE ; print new line JSR OUTSTR PULX ; restore reg X PULD ; restore reg D #endif FOLLOW2 LDX #FOLLOWTBL ; load address of reaction table LDAA FTBLIDX ; get the reaction table index LSLA ; convert to 16-bit index LDX A,X ; get address of handling routine JMP 0,X ; jump to appropriate routine FOLLOWX PULD ; restore register D PULX ; restore register X RTS ; return to caller * ******************************************************************************* * SUBROUTINE - FGETDATA * Description: Reads the values from the following IR detectors. * Input : None. * Output : LFVALUE,RFVALUE. * Destroys : LFVALUE,RFVALUE. * Calls : None. ******************************************************************************* * FGETDATA PSHA ; save register A MOVB #$00,FTBLIDX ; clear reaction table index LDAA #CENTERFOLLOW ; get data from center sensor JSR ANALOG ; STAA CFVALUE ; save center sensor value CMPA #MINFOLLOW ; did center see speeding car? BLO FGETDATA1 ; no, so continue BSET FTBLIDX,BIT2 ; yes, so set bit in index FGETDATA1 LDAA #LEFTFOLLOW ; get data from left sensor JSR ANALOG ; STAA LFVALUE ; save left sensor value CMPA #MINFOLLOW ; did left see speeding car? BLO FGETDATA2 ; no, so continue BSET FTBLIDX,BIT1 ; yes, so set bit in index FGETDATA2 LDAA #RIGHTFOLLOW ; get data from right sensor JSR ANALOG ; STAA RFVALUE ; save right sensor value CMPA #MINFOLLOW ; did right see speeding car? BLO FGETDATAX ; no, so get out BSET FTBLIDX,BIT0 ; yes, so set bit in index FGETDATAX PULA ; restore register A RTS ; return to caller * ******************************************************************************* * SUBROUTINES - FHARDR,FHARDL,FFOWARD,FSOFTR,FSOFTL,FCONT * Description: Handle motor control for following behavior * Input : None. * Output : None. * Destroys : Reg X, Reg D. * Calls : STEER. ******************************************************************************* * FHARDR MOVB FTBLIDX,LASTFOLLOW ; save off last table index LDD #FOLLOWTURNSPD ; set speed to turning speed JSR CHNGSPEED ; LDX #HARDRIGHT ; turn hard right JSR STEER ; JMP FOLLOWX ; get out FHARDL MOVB FTBLIDX,LASTFOLLOW ; save off last table index LDD #FOLLOWTURNSPD ; set speed to turning speed JSR CHNGSPEED ; LDX #HARDLEFT ; turn hard left JSR STEER ; JMP FOLLOWX ; get out FFOWARD MOVB FTBLIDX,LASTFOLLOW ; save off last table index JSR SPEEDCALC ; go set new speed LDX #GOFOWARD ; go foward JSR STEER ; JMP FOLLOWX ; get out FSOFTR MOVB FTBLIDX,LASTFOLLOW ; save off last table index JSR SPEEDCALC ; go set new speed LDX #SOFTRIGHT ; turn soft right JSR STEER ; JMP FOLLOWX ; get out FSOFTL MOVB FTBLIDX,LASTFOLLOW ; save off last table index JSR SPEEDCALC ; go set new speed LDX #SOFTLEFT ; turn soft left JSR STEER ; JMP FOLLOWX ; get out FCONT LDX #FOLLOWTBL ; load address of reaction table LDAA LASTFOLLOW ; get the last reaction index STAA FTBLIDX ; save as current reaction LSLA ; convert to 16-bit index LDX A,X ; get address of handling routine JMP 0,X ; jump to appropriate routine * ******************************************************************************* * SUBROUTINE - FOBJAVOID * Description: Handles object avoidance for following behavior * Input : None. * Output : None. * Destroys : Reg X, Reg D. * Calls : SIRENON,SIRENOFF,OBJAVOID,WAIT. ******************************************************************************* * FOBJAVOID JSR SIRENOFF ; turn off siren to indicate we lost speeder LDD #OBJAVOIDSPD ; set motor speed to obj avoid speed JSR CHNGSPEED ; FOBJAVOID1 JSR OBJAVOID ; avoid obstacles LDX #OAPROCRATE ; wait designated amount of time JSR WAIT ; JSR FGETDATA ; check following sensors TST FTBLIDX ; did we find the speeder? BEQ FOBJAVOID1 ; no, continue object avoidance JSR SIRENON ; turn siren on to indicate active chase LDX #FOLLOWTBL ; load address of reaction table LDAA FTBLIDX ; get the reaction table index LSLA ; convert to 16-bit index LDX A,X ; get address of handling routine JMP 0,X ; jump to appropriate routine * ******************************************************************************* * SUBROUTINE - SPEEDCALC * Description: Calculates and sets the next max motor speed so that the robot * has a constant acceleration * Input : None. * Output : None. * Destroys : Reg X, Reg D. * Calls : . ******************************************************************************* * SPEEDCALC LDD NEWCFAVG ; load the current working average ADDB CFVALUE ; add the newest center sensor ADCA #$00 ; to the current average LDX AVGCNT ; get the count of # items in working average INX ; increment the count STX AVGCNT ; save the count CPX #NUMAVGCNT ; do we have the correct sum yet? BLT SPEEDCALCX ; no, get out IDIVS ; calculate average XGDX ; get average STD NEWCFAVG ; save average #ifdef __PRINTFOLLOW_ PSHX ; save reg X LDX OLDCFAVG ; print old average JSR OUTADDR ; LDAA #$20 ; print a space JSR OUTCHAR ; LDX NEWCFAVG ; print new average JSR OUTADDR ; LDX #NEWLINE ; print newline JSR OUTSTR ; PULX ; restore reg X #endif SUBD OLDCFAVG ; calc difference between old,new average CPD #SPDACCEL ; compare to the desired acceleration BEQ SPEEDCALC4 ; they are equal, do nothing BLT SPEEDCALC2 ; need to speed up * SLOW DOWN LDD MAXFOWARDSPD ; get the current max speed ADDD #SPDDOWNACCEL ; slow it down CPD #MINFOLLOWSPD ; have we slowed down too far? BGE SPEEDCALC1 ; no, skip adjustment LDD #MINFOLLOWSPD ; make speed = minimum speed SPEEDCALC1 JSR CHNGSPEED ; set the new speed BRA SPEEDCALC4 ; get out * SPEED UP SPEEDCALC2 LDD MAXFOWARDSPD ; get the current max speed ADDD #SPDUPACCEL ; speed it up CPD #MAXFOLLOWSPD ; have we speed up too far? BLE SPEEDCALC3 ; no, skip adjustment LDD #MAXFOLLOWSPD ; make speed = maximum speed SPEEDCALC3 JSR CHNGSPEED ; set the new speed SPEEDCALC4 MOVW NEWCFAVG,OLDCFAVG ; make new average the old average MOVW #$0000, NEWCFAVG ; clear new average to start over MOVW #$0000,AVGCNT ; clear average count for next time SPEEDCALCX RTS ; return to caller * *******************************************************************************