* Filename      : OBJAVOID.ASM
* Programmer    : Michael Hattermann
* Date          : February 4, 2002
* Version       : 1.0
* Description   : This file contains the code for
*                 object avoidance. The following
*                 functions are available:
*
*                   OBJAVOID - reads IR and avoids obstacles
*                   GETVALUES - reads IR values from analog port
*                   CONVREACT - converts IR value to a reaction
*                   LEFTRIGHT - decided to turn left,right,or random
*                   BACKUP - backs robot up and turns it
*                   HARD - turns robot hard in a direction
*                   SOFT - turns robot soft in a direction
*
*
*#define __DEBUGOBJAVOID2_ 1
*#define __PRINTOBJAVOID2_ 1
#include "hc12.asm"


*
************************************************************
* Object Avoidance Equates
************************************************************
*
LREQUAL         EQU         $10         ; left,right IR values equality threshold
BACKUPTIME      EQU         150         ; # ms to backup

*
************************************************************
* OBJAVOID2 Debug Code
************************************************************
*
#ifdef __DEBUGOBJAVOID2_
            ORG     USERPROG_PVECT
            JMP     TEST

            ORG     $B000
TEST        JSR     INITATD         ; init A/D system
TEST1       LDAA    #HALFSPEED      ; go at half speed
            JSR     OBJAVOID        ; go avoid objects
            BRA     TEST1
            SWI

LVALUE      DC.B        $00         ; value of left IR
LREACT      DC.B        $00         ; reaction to left IR value
RVALUE      DC.B        $00         ; value of right IR
RREACT      DC.B        $00         ; reaction to right IR value
CVALUE      DC.B        $00         ; value of center IR
CREACT      DC.B        $00         ; reaction to center IR value
PREVRAND    DC.B        $00         ; previous turn direction
PREVREACT   DC.B        $00         ; previous reaction value

#include "atd.asm"
#include "time.asm"
#include "pwm.asm"
#include "sci.asm"
#include "wait.asm"

#endif

#ifdef __PRINTOBJAVOID2_
LEFT        DC.B    'Left IR='
            DC.B    EOS
CENTER      DC.B    ', Center IR='
            DC.B    EOS
RIGHT       DC.B    ', Right IR='
            DC.B    EOS
LEFTR       DC.B    'Left Reaction='
            DC.B    EOS
CENTERR     DC.B    ', Center Reaction='
            DC.B    EOS
RIGHTR      DC.B    ', Right Reaction='
            DC.B    EOS
BACKSTR     DC.B    'BACKUP'
            DC.B    CR,LF,CR,LF,EOS
LHARDSTR    DC.B    'HARD LEFT'
            DC.B    CR,LF,CR,LF,EOS
LSOFTSTR    DC.B    'SOFT LEFT'
            DC.B    CR,LF,CR,LF,EOS
RHARDSTR    DC.B    'HARD RIGHT'
            DC.B    CR,LF,CR,LF,EOS
RSOFTSTR    DC.B    'SOFT RIGHT'
            DC.B    CR,LF,CR,LF,EOS
FOWARDSTR   DC.B    'FOWARD'
            DC.B    CR,LF,CR,LF,EOS
#endif

* Reaction table for values for center IR
************************************************************
CNTRTBL     DC.B        NOACTION        ; $00-$07
            DC.B        NOACTION        ; $08-$0F
            DC.B        NOACTION        ; $10-$17
            DC.B        NOACTION        ; $18-$1F
            DC.B        REACTSOFT       ; $20-$27
            DC.B        REACTSOFT       ; $28-$2F
            DC.B        REACTSOFT       ; $30-$37
            DC.B        REACTSOFT       ; $38-$3F
            DC.B        REACTSOFT       ; $40-$47
            DC.B        REACTHARD       ; $48-$4F
            DC.B        REACTHARD       ; $50-$57
            DC.B        REACTHARD       ; $58-$5F
            DC.B        REACTHARD       ; $60-$67
            DC.B        REACTHARD       ; $68-$6F
            DC.B        REACTBACK       ; $70-$77
            DC.B        REACTBACK       ; $78-$7F
            DC.B        REACTBACK       ; $80-$87
            DC.B        REACTBACK       ; $88-$8F
            DC.B        REACTBACK       ; $90-$97
            DC.B        REACTBACK       ; $98-$9F

* Reaction table for values for left,right IR
************************************************************
LEFTTBL     DC.B        NOACTION        ; $00-$07
            DC.B        NOACTION        ; $08-$0F
            DC.B        NOACTION        ; $10-$17
            DC.B        NOACTION        ; $18-$1F
            DC.B        REACTSOFT       ; $20-$27
            DC.B        REACTSOFT       ; $28-$2F
            DC.B        REACTSOFT       ; $30-$37
            DC.B        REACTSOFT       ; $38-$3F
            DC.B        REACTHARD       ; $40-$47
            DC.B        REACTHARD       ; $48-$4F
            DC.B        REACTHARD       ; $50-$57
            DC.B        REACTHARD       ; $58-$5F
            DC.B        REACTHARD       ; $60-$67
            DC.B        REACTHARD       ; $68-$6F
            DC.B        REACTBACK       ; $70-$77
            DC.B        REACTBACK       ; $78-$7F
            DC.B        REACTBACK       ; $80-$87
            DC.B        REACTBACK       ; $88-$8F
            DC.B        REACTBACK       ; $90-$97
            DC.B        REACTBACK       ; $98-$9F

* Reaction table for values for left,right IR
************************************************************
RIGHTTBL    DC.B        NOACTION        ; $00-$07
            DC.B        NOACTION        ; $08-$0F
            DC.B        NOACTION        ; $10-$17
            DC.B        NOACTION        ; $18-$1F
            DC.B        REACTSOFT       ; $20-$27
            DC.B        REACTSOFT       ; $28-$2F
            DC.B        REACTSOFT       ; $30-$37
            DC.B        REACTSOFT       ; $38-$3F
            DC.B        REACTHARD       ; $40-$47
            DC.B        REACTHARD       ; $48-$4F
            DC.B        REACTHARD       ; $50-$57
            DC.B        REACTHARD       ; $58-$5F
            DC.B        REACTHARD       ; $60-$67
            DC.B        REACTHARD       ; $68-$6F
            DC.B        REACTBACK       ; $70-$77
            DC.B        REACTBACK       ; $78-$7F
            DC.B        REACTBACK       ; $80-$87
            DC.B        REACTBACK       ; $88-$8F
            DC.B        REACTBACK       ; $90-$97
            DC.B        REACTBACK       ; $98-$9F

*
*******************************************************************************
*                       SUBROUTINE -  OBJAVOID
* Description: Performs an obstacle avoidance behavior by reading the values
*               from the IR and bump sensors and moving the robot accordingly.
* Input         : None.
* Output        : None.
* Destroys      : None.
* Calls         : GETVALUES,CONVREACT.
*******************************************************************************
*
OBJAVOID    PSHD                    ; save reg D
            PSHX                    ; save reg X

            JSR     BUMPED          ; check to see if we bumped something
            CMPA    #FRONTREAR      ; did we bump something in the front?
            BLO     OBJAVOIDB       ; no bump, check the IR
            JMP     BACKUP          ; we bumped something, backup

OBJAVOIDB   JSR     GETVALUES       ; get the IR readings
            JSR     CONVREACT       ; convert readings to reactions

            LDAB    #REACTBACK      ; get code for back up reaction
            CMPB    CREACT          ; does center say backup
            BNE     OBJAVOID1       ; no, continue checking
            JMP     BACKUP          ; yes, backup

OBJAVOID1   CMPB    LREACT          ; does left say backup
            BNE     OBJAVOID2       ; no, continue checking
            JMP     BACKUP          ; yes, backup

OBJAVOID2   CMPB    RREACT          ; does right say backup
            BNE     OBJAVOID3       ; no, continue checking
            JMP     BACKUP          ; yes, backup

OBJAVOID3   LDAB    #REACTHARD      ; get code for hard turn reaction
            CMPB    CREACT          ; does center say backup
            BNE     OBJAVOID4       ; no, continue checking
            JMP     HARD            ; yes, turn hard

OBJAVOID4   CMPB    LREACT          ; does left say turn hard
            BNE     OBJAVOID5       ; no, continue checking
            JMP     HARD            ; yes, turn hard

OBJAVOID5   CMPB    RREACT          ; does right say turn hard
            BNE     OBJAVOID6       ; no, continue checking
            JMP     HARD            ; yes, turn hard

OBJAVOID6   LDAB    #REACTSOFT      ; get code for hard turn reaction
            CMPB    CREACT          ; does center say backup
            BNE     OBJAVOID7       ; no, continue checking
            JMP     SOFT            ; yes, turn soft

OBJAVOID7   CMPB    LREACT          ; does left say turn hard
            BNE     OBJAVOID8       ; no, continue checking
            JMP     SOFT            ; yes, turn soft

OBJAVOID8   CMPB    RREACT          ; does right say turn hard
            BNE     OBJAVOID9       ; no, continue checking
            JMP     SOFT            ; yes, turn hard

OBJAVOID9
#ifdef __PRINTOBJAVOID2_
            PSHX                    ; save reg X
            LDX     #FOWARDSTR      ; print foward string
            JSR     OUTSTR          ;
            PULX                    ; restore reg X
#endif

OBJAVOID10  LDX     #GOFOWARD               ; no obstacles, go foward at set speed
            JSR     STEER                   ;
            CLR     PREVRAND                ; clear previous random direction
            MOVB    #NOACTION,PREVREACT     ; save this reaction

OBJAVOIDX   PULX                    ; restore reg X
            PULD                    ; restore reg D
            RTS                     ; return to caller
*
*******************************************************************************
*                       SUBROUTINE -  GETVALUES
* Description: Gets the IR values for the left,center,and right channels
* Input         : None.
* Output        : RVALUE,LVALUE,CVALUE.
* Destroys      : RVALUE,LVALUE,CVALUE.
* Calls         : ANALOG.
*******************************************************************************
*
GETVALUES   PSHA                    ; save reg A
            LDAA    #RIGHTIR        ; read right IR value
            JSR     ANALOG          ;
            STAA    RVALUE          ; save right IR value
            LDAA    #LEFTIR         ; read left IR value
            JSR     ANALOG          ;
            STAA    LVALUE          ; save left IR value
            LDAA    #CENTERIR       ; read center IR value
            JSR     ANALOG          ;
            STAA    CVALUE          ; save center IR value

#ifdef __PRINTOBJAVOID2_
            PSHX                    ; save reg X
            LDX     #LEFT           ; print left IR header
            JSR     OUTSTR          ;
            LDAA    LVALUE          ; print left IR value
            JSR     OUTNUM          ;
            LDX     #CENTER         ; print center IR header
            JSR     OUTSTR          ;
            LDAA    CVALUE          ; print center IR value
            JSR     OUTNUM          ;
            LDX     #RIGHT          ; print right IR header
            JSR     OUTSTR          ;
            LDAA    RVALUE          ; print right IR value
            JSR     OUTNUM          ;
            LDX     #NEWLINE        ; print new line
            JSR     OUTSTR          ;
            PULX                    ; restore reg X
#endif

            PULA                    ; restore reg A
            RTS                     ; return to caller
*
*******************************************************************************
*                       SUBROUTINE -  CONVREACT
* Description: Converts IR readings to reaction values using lookup tables
* Input         : RVALUE,LVALUE,CVALUE.
* Output        : CREACT,LREACT,RREACT.
* Destroys      : CREACT,LREACT,RREACT.
* Calls         : None.
*******************************************************************************
*
CONVREACT   PSHX                            ; save register X
            LDX     #CNTRTBL                ; load address of lookup table
            LDAB    CVALUE                  ; get center value
            LSRB                            ; convert center value
            LSRB                            ;  to table lookup
            LSRB                            ;   value
            MOVB    B,X,CREACT              ; lookup reaction for center

            LDX     #LEFTTBL                ; load address of lookup table
            LDAB    LVALUE                  ; get left value
            LSRB                            ; convert left value
            LSRB                            ;  to table lookup
            LSRB                            ;   value
            MOVB    B,X,LREACT              ; lookup reaction for left channel

            LDX     #RIGHTTBL               ; load address of lookup table
            LDAB    RVALUE                  ; get right value
            LSRB                            ; convert right value
            LSRB                            ;  to table lookup
            LSRB                            ;   value
            MOVB    B,X,RREACT              ; lookup reaction for right channel

#ifdef __PRINTOBJAVOID2_
            PSHX                            ; save reg X
            LDX     #LEFTR                  ; print left IR header
            JSR     OUTSTR                  ;
            LDAA    LREACT                  ; print left IR reaction
            JSR     OUTNUM                  ;
            LDX     #CENTERR                ; print center IR header
            JSR     OUTSTR                  ;
            LDAA    CREACT                  ; print center IR reaction
            JSR     OUTNUM                  ;
            LDX     #RIGHTR                 ; print right IR header
            JSR     OUTSTR                  ;
            LDAA    RREACT                  ; print right IR reaction
            JSR     OUTNUM                  ;
            LDX     #NEWLINE                ; print new line
            JSR     OUTSTR                  ;
            PULX                            ; restore reg X
#endif
            PULX                            ; restore register X
            RTS                             ; return to caller
*
*******************************************************************************
*                       SUBROUTINE -  LEFTRIGHT
* Description: Decides if we should turn left or right based on IR sensor
*               readings. It will compare left and right values, and if they
*               differ by more than some threshold, this decides the turn
*               direction.  Otherwise, turn direction is random.  Return values:
*                   REACTLEFT - Turn left
*                   REACTRIGHT - Turn right
* Input         : LVALUE,RVALUE.
* Output        : Reg B has direction.
* Destroys      : Reg B, PREVRAND.
* Calls         : None.
*******************************************************************************
*
LEFTRIGHT   PSHA                        ; save reg A

            LDAA    LVALUE              ; get the left value
            LDAB    RVALUE              ; get the right value
            SBA                         ; compare the values
            CMPA    #LREQUAL            ; if left > right by the threshold
            BGE     LEFTRIGHT1          ; turn right
            CMPA    #(-LREQUAL)         ; if left < right by the threshold
            BLE     LEFTRIGHT2          ; turn left

            TST     PREVRAND            ; do we have a previous direction
            BEQ     LEFTRIGHTR          ; no, so go generate a direction
            LDAB    PREVRAND            ; yes, so use previous direction
            BRA     LEFTRIGHTX          ; get out

LEFTRIGHTR  LDAB    TCNTL               ; otherwise, get lower half of timer
            LSRB                        ; check lowest bit
            BCS     LEFTRIGHT2          ; go left if set, right if clear
LEFTRIGHT1  LDAB    #REACTRIGHT         ; turn right
            STAB    PREVRAND            ; save previous direction
            BRA     LEFTRIGHTX          ; get out
LEFTRIGHT2  LDAB    #REACTLEFT          ; turn left
            STAB    PREVRAND            ; save previous direction
LEFTRIGHTX  PULA                        ; restore reg A
            RTS                         ; return to caller
*
*******************************************************************************
*                       SUBROUTINE -  BACKUP
* Description: Backs up and turns a random direction (left or right).
* Input         : None.
* Output        : None.
* Destroys      : Reg B,Reg X,PREVRAND.
* Calls         : STEER,WAIT.
*******************************************************************************
*
BACKUP
            MOVB    #REACTBACK,PREVREACT    ; save this reaction
            CLR     PREVRAND                ; clear previous random direction

#ifdef __PRINTOBJAVOID2_
            PSHX
            LDX     #BACKSTR
            JSR     OUTSTR
            PULX
#endif

BACKUP1     JSR     LEFTRIGHT       ; do we have a preference left or right
            CMPB    #REACTLEFT      ; if we need to go left, go left
            BEQ     BACKUPL         ; go left

            LDX     #BACKRIGHT      ; go hard right
            JSR     STEER           ;
            BRA     BACKUPX         ; get out

BACKUPL     LDX     #BACKLEFT       ; go hard left
            JSR     STEER           ;

BACKUPX     LDX     #GOBACK         ; go backward
            JSR     STEER           ;
            LDX     #BACKUPTIME     ; go back for set amount of time
            JSR     WAIT            ;
            JMP     OBJAVOIDX       ; get out
*
*******************************************************************************
*                       SUBROUTINE -  HARD
* Description: Turns hard in a random direction (left or right).
* Input         : None.
* Output        : None.
* Destroys      : Reg B,Reg X.
* Calls         : STEER.
*******************************************************************************
*
HARD
            LDAB    PREVREACT               ; get previous reaction
            CMPB    #REACTHARD              ; was it react hard?
            BEQ     HARD1                   ; yes, so continue
            CLR     PREVRAND                ; no, clear previous turn direction
            MOVB    #REACTHARD,PREVREACT    ; save this reaction
HARD1
            JSR     LEFTRIGHT               ; do we have a preference left or right
            CMPB    #REACTLEFT              ; if we need to go left, go left
            BEQ     HARDL                   ; go left

#ifdef __PRINTOBJAVOID2_
            PSHX                            ; save reg X
            LDX     #RHARDSTR               ; print hard right string
            JSR     OUTSTR                  ;
            PULX                            ; restore reg X
#endif

HARDR       LDX     #HARDRIGHT              ; go hard right
            JSR     STEER                   ;
            JMP     OBJAVOIDX               ; get out

HARDL

#ifdef __PRINTOBJAVOID2_
            PSHX                            ; save reg X
            LDX     #LHARDSTR               ; print left hard string
            JSR     OUTSTR                  ;
            PULX                            ; restore reg X
#endif

            LDX     #HARDLEFT               ; go hard left
            JSR     STEER                   ;
            JMP     OBJAVOIDX               ; get out
*
*******************************************************************************
*                       SUBROUTINE -  SOFT
* Description: Turns soft in a random direction (left or right).
* Input         : None.
* Output        : None.
* Destroys      : Reg B,Reg X.
* Calls         : STEER,WAIT.
*******************************************************************************
*
SOFT
            LDAB    PREVREACT               ; get previous reaction
            CMPB    #REACTSOFT              ; was it react soft?
            BEQ     SOFT1                   ; yes, so continue
            CLR     PREVRAND                ; no, clear previous turn direction
            MOVB    #REACTSOFT,PREVREACT    ; save this reaction
SOFT1
            JSR     LEFTRIGHT       ; do we have a preference left or right
            CMPB    #REACTLEFT      ; if we need to go left, go left
            BEQ     SOFTL           ; go left

#ifdef __PRINTOBJAVOID2_
            PSHX                    ; save reg X
            LDX     #RSOFTSTR       ; print soft right string
            JSR     OUTSTR          ;
            PULX                    ; restore reg X
#endif

SOFTR       LDX     #SOFTRIGHT      ; go soft right
            JSR     STEER           ;
            JMP     OBJAVOIDX       ; get out

SOFTL
#ifdef __PRINTOBJAVOID2_
            PSHX                    ; save reg X
            LDX     #LSOFTSTR       ; print soft left string
            JSR     OUTSTR          ;
            PULX                    ; restore reg X
#endif

            LDX     #SOFTLEFT       ; go soft left
            JSR     STEER           ;
            JMP     OBJAVOIDX       ; get out
*
*******************************************************************************
