* Filename      : OBJAVOID.ASM
* Programmer    : Michael Hattermann
* Date          : February 4, 2002
* Version       : 1.0
* Description   : This file contains the code for
*                 object avoidance.
*
*

#include "hc12.asm"

#define __DEBUGOBJAVOID_ 1
*
************************************************************
* Object Avoidance Equates
************************************************************
*

IRLEFT      EQU         CHANNEL0
IRRIGHT     EQU         CHANNEL1

THRESHOLD   EQU         $70

*
************************************************************
* Constant Definitions
************************************************************
*
#ifdef __DEBUGOBJAVOID_
            ORG     USERPROG_PVECT
            JMP     TEST

            ORG     $0900
TEST        LDAA    #$00            ; turn off COP watchdog timer
            STAA    COPCTL
            LDS     #$0bff          ; init the stack pointer

            JSR     INITTIME        ; init timer system
            JSR     INITATD         ; init ATD system
            JSR     INITBUMP        ; init bumper system

            MOVB    #$C3,LED1PORT   ; flash
            LDX     #250            ; wait
            JSR     WAIT            ;
            MOVB    #$00,LED1PORT   ; flash
            LDX     #250            ; wait
            JSR     WAIT            ;
            MOVB    #$C3,LED1PORT   ; flash
            LDX     #250            ; wait
            JSR     WAIT            ;
            MOVB    #$00,LED1PORT   ; flash
            LDX     #500            ; wait
            JSR     WAIT            ;

TEST2       MOVB    #$81,LED1PORT   ; set motors moving foward
            LDX     #50             ; wait 50 ms
            JSR     WAIT

            LDAA    #LEFTIR         ; get left IR value
            JSR     ANALOG          ;

            CMPA    #THRESHOLD      ; is left IR greater than the threshold
            BLT     TEST3           ; no, continue

            JSR     BACKUP          ; backup
            JSR     RIGHT           ; turn right
            BRA     TEST2           ; continue

TEST3       LDAA    #RIGHTIR        ; get right IR value
            JSR     ANALOG          ;

            CMPA    #THRESHOLD      ; is right IR greater than the threshold
            BLT     TEST4           ; no, continue

            JSR     BACKUP          ; backup
            JSR     LEFT            ; turn left

TEST4       BRA     TEST2           ; end of program

************************************************************

BACKUP      PSHX                    ; save reg X
            MOVB    #$42,LED1PORT   ; turn on backup lights
            LDX     #500            ; wait 1/2 second
            JSR     WAIT            ;
            PULX                    ; restore reg X
            RTS                     ; return to caller

************************************************************

LEFT        PSHX                    ; save reg X
            PSHD                    ; save reg D
            MOVB    #$80,LED1PORT   ; turn on left turn lights

            LDX     #400            ; wait a random amount of time
            LDD     TCNT            ;
            LSLB                    ;
            LSLB                    ;
            ABX                     ;
            JSR     WAIT            ;

            PULD                    ; restore reg D
            PULX                    ; restore reg X
            RTS                     ; return to caller

************************************************************

RIGHT       PSHX                    ; save reg X
            PSHD                    ; save reg D
            MOVB    #$01,LED1PORT   ; turn on left turn lights

            LDX     #400            ; wait a random amount of time
            LDD     TCNT            ;
            LSLB                    ;
            LSLB                    ;
            ABX                     ;
            JSR     WAIT            ;

            PULD                    ; restore reg D
            PULX                    ; restore reg X
            RTS                     ; return to caller

************************************************************

#include "time.asm"
#include "atd.asm"
#include "bump.asm"

#endif