'****************************************************************
'*  Name    : DIAG025.BAS                                       *
'*  Author  : Giuseppe Marullo                                  *
'*  Notice  : Copyright (c) 2002 Copyright 2002                 *
'*          : All Rights Reserved                               *
'*  Date    : 8/11/2002                                         *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :  TO DO:                                           *
'*          :  - Complete the defines for Controller board      *
'*          :  - Complete the defines for Sensor Board          *
'*          :  - optimize the IRQ routine                       *
'*          :  - Routines for Sensor Board                      *                          
'*          :    - Init                                         *
'*          :    - Beep                                         *
'*          :                                                   *
'****************************************************************

'General h/w definitions
DEFINE OSC 20
DEFINE HSER_BAUD 38400
DEFINE I2C_SLOW 1

FORWARD CON 0
LEFT    CON 1
RIGHT   CON 2

' Set starting Data bit (0 or 4) if 4-bit bus
 DEFINE LCD_DBIT 0
' Set LCD Register Select port
 DEFINE LCD_DREG PORTD
 DEFINE LCD_EREG PORTD
 DEFINE LCD_RSREG PORTD
 DEFINE LCD_WRREG PORTD
' Set LCD Register Select bit
 DEFINE LCD_RSBIT 5
' Set LCD Enable port
' Set LCD Enable bit
 DEFINE LCD_EBIT 4
' Set LCD bus size (4 or 8 bits)
 DEFINE LCD_BITS 4
' Set number of lines on LCD

 DEFINE LCD_RWBIT 6 ' LCD read/write pin bit

 DEFINE LCD_LINES 4
' Set command delay time in us
 DEFINE LCD_COMMANDUS 6000
' Set data delay time in us
 DEFINE LCD_DATAUS 254

LCD_E var PORTD.4
LCD_RS var PORTD.5

' Assembly language interrupt example
wsave var byte $20 system
ssave var byte bank0 system
psave var byte bank0 system
wsave1		var	BYTE 	$0a0 SYSTEM
wsave2		var	BYTE 	$120 SYSTEM
wsave3		var	BYTE 	$1a0 SYSTEM


SERVOL var BYTE
SERVOR var BYTE

REYE var word
REYE_H var REYE.BYTE1
REYE_L var REYE.BYTE0

LEYE var word
LEYE_H var LEYE.BYTE1
LEYE_L var LEYE.BYTE0

WVAR_H var BYTE
WVAR_L var BYTE

LINER var word
LINER_H var LINER.byte1
LINER_L var LINER.byte0

LINEL var word
LINEL_H var LINEL.byte1
LINEL_L var LINEL.byte0

LINEC var word
LINEC_H var LINEC.byte1
LINEC_L var LINEC.byte0

TEMP_STRING var byte[5]
addr  var word
eeprom_read var byte

SCL_TRIS var TRISC.3
SDA_TRIS var TRISC.4

I2CLOW   con 0
I2CHIGH   con 1

I2CSTARTDELAY con 25
I2CSTOPDELAY  con 25
I2CDATASETTLE con 5
I2CCLOCKHIGH  con 50
I2CCLOCKLOW   con 50
I2CHALFCLOCK  con 25
I2CACKWAITMIN con 25


XAXIS_PIN var PORTB.6
YAXIS_PIN var PORTB.7

T2 var word
T2_H var T2.Byte1
T2_L var T2.Byte0


CNT var byte
TEMP var word
TEMP_H var TEMP.Byte1
TEMP_L var TEMP.Byte0

TEMP2 var word
TEMP2_H var TEMP2.Byte1
TEMP2_L var TEMP2.Byte0

XAXIS var word
XAXIS_H var XAXIS.Byte1
XAXIS_L var XAXIS.Byte0

MIN_XAXIS var word
MAX_XAXIS var word
MIN_YAXIS var word
MAX_YAXIS var word

SPEED var word
NEUTRAL var word
NOW  VAR BYTE
CHOICE VAR WORD
t var byte
YAXIS var word
YAXIS_H var YAXIS.Byte1
YAXIS_L var YAXIS.Byte0

XAXIS0 var word
XAXIS0_H var XAXIS0.Byte1
XAXIS0_L var XAXIS0.Byte0

YAXIS0 var word
YAXIS0_H var YAXIS0.Byte1
YAXIS0_L var YAXIS0.Byte0
'-----------------------------------

     device var byte          ' device 0-7
        
     o_byte var byte          ' byte to send to device
     i_byte var byte          ' byte from device

     o_patt var byte          ' byte to appear on output of 8574
     i_patt var byte          ' byte read from input of 8574

     directions var byte ' defines which bits on 8574 are outputs
        
     n var byte          ' index
     b var bit           ' bit 
          

     SDA_PIN var PORTC.4
     SCL_PIN var PORTC.3
     
'     SDA_OUT var out4
'     SCL_OUT var out5
'     SDA_IN  var in4
'     SDA_DIR var dir4

     OUT con 1
     IN con 0

'     dirs=$f0ff
     INPUT SDA_PIN
               
'---------------------------

Goto start ' Skip around interrupt handler
DEFINE INTHAND myint
asm
AUX1_L EQU  0X24
SERVOVALUE EQU  0X45
I EQU  0X42
    
; Save W, STATUS and PCLATH registers
myint 
; Insert interrupt qcode here
    bcf    STATUS, RP1
    bcf    STATUS, RP0
    bsf PORTB , 1

    MOVF   _SERVOR, W
    MOVWF  SERVOVALUE
    PAGESEL (DODUTYDELAY)
    CALL   DODUTYDELAY
    BCF    STATUS, RP1
    BCF    STATUS, RP0
    MOVF   SERVOVALUE, W
    MOVWF  _SERVOR
    BCF    PORTB, 1
    BCF    PORTB, 1
    
    bsf PORTB , 2

    MOVF   _SERVOL, W
    MOVWF  SERVOVALUE
    PAGESEL (DODUTYDELAY)
    CALL   DODUTYDELAY
    BCF    STATUS, RP1
    BCF    STATUS, RP0
    MOVF   SERVOVALUE, W
    MOVWF  _SERVOL
    BCF    PORTB, 2
    BCF    PORTB, 2
    
; Save and restore FSR if used

; Restore PCLATH, STATUS and W registers
    bcf PIR1 , 0    ; Clear Timer 1 Interrupt Flag
    movf psave, W
    movwf PCLATH
    swapf ssave, W
    movwf STATUS
    swapf wsave, F
    swapf wsave, W
    retfie
DODUTYDELAY
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVLW  1
     MOVWF  I
AUXLAB2
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     PAGESEL(DELAYLAB13);*
     MOVLW  165;*
     MOVWF  AUX1_L
DELAYLAB13;*
     DECFSZ AUX1_L, F;*
     GOTO   DELAYLAB13;*
     NOP;*
     NOP;*
     PAGESEL (NEXTLAB2)
     MOVF   SERVOVALUE, W
     SUBWF  I, W
     BTFSC  STATUS, Z
     GOTO   NEXTLAB2
     INCF   I, F
     PAGESEL (AUXLAB2)
     GOTO   AUXLAB2
NEXTLAB2
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     RETURN

READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     ;   READ THE A2D AND STORE THE ANSWER IN WVAR
     ;    DELAY 0.02, 0
     ;  NEED TO WAIT AT LEAST 2TAD (32 20MHZ CLOCK CYCLES 3.2USEC)
     ;  BETWEEN A/D SAMPLINGS.
     PAGESEL(DELAYLAB17);*
     MOVLW  32;*
     MOVWF  AUX1_L
DELAYLAB17;*
     DECFSZ AUX1_L, F;*
     GOTO   DELAYLAB17;*
     NOP;*
     BSF    ADCON0, GO
     PAGESEL(DELAYLAB18);*
     MOVLW  65;*
     MOVWF  AUX1_L
DELAYLAB18;*
     DECFSZ AUX1_L, F;*
     GOTO   DELAYLAB18;*
     NOP;*
     NOP;*
ADCLAB9
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     PAGESEL (ADCLAB9)
     BTFSC  ADCON0, GO
     GOTO   ADCLAB9
     BSF    STATUS, RP0
     MOVF   ADRESL, W
     BCF    STATUS, RP0
     MOVWF  _WVAR_L
     MOVF   ADRESH, W
     MOVWF  _WVAR_H
     BCF    ADCON0, ADON
     RETURN

_READEYES
     BCF    STATUS, RP1
     BCF    STATUS, RP0

     BSF    STATUS, RP0
     BCF    PIE1, ADIE
     MOVLW  145
     BCF    STATUS, RP0
     MOVWF  ADCON0
     MOVLW  0
     BSF    STATUS, RP0
     MOVWF  ADCON1
     BCF    STATUS, RP0
     BSF    ADCON0, ADON
     PAGESEL (READA2D)
     CALL   READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVF   _WVAR_H, W
     MOVWF  _REYE_L
     CLRF   _REYE_H

     BSF    STATUS, RP0
     BCF    PIE1, ADIE
     MOVLW  153
     BCF    STATUS, RP0
     MOVWF  ADCON0
     MOVLW  0
     BSF    STATUS, RP0
     MOVWF  ADCON1
     BCF    STATUS, RP0
     BSF    ADCON0, ADON
     PAGESEL (READA2D)
     CALL   READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVF   _WVAR_H, W
     MOVWF  _LEYE_L
     CLRF   _LEYE_H
     RETURN
_READSENSORS
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     BSF    STATUS, RP0
     BCF    PIE1, ADIE
     MOVLW  169
     BCF    STATUS, RP0
     MOVWF  ADCON0
     MOVLW  0
     BSF    STATUS, RP0
     MOVWF  ADCON1
     BCF    STATUS, RP0
     BSF    ADCON0, ADON
     PAGESEL (READA2D)
     CALL   READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVF   _WVAR_H, W
     MOVWF  _LINER_L
     CLRF   _LINER_H
     BSF    STATUS, RP0
     BCF    PIE1, ADIE
     MOVLW  177
     BCF    STATUS, RP0
     MOVWF  ADCON0
     MOVLW  0
     BSF    STATUS, RP0
     MOVWF  ADCON1
     BCF    STATUS, RP0
     BSF    ADCON0, ADON
     PAGESEL (READA2D)
     CALL   READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVF   _WVAR_H, W
     MOVWF  _LINEC_L
     CLRF   _LINEC_H
     BSF    STATUS, RP0
     BCF    PIE1, ADIE
     MOVLW  185
     BCF    STATUS, RP0
     MOVWF  ADCON0
     MOVLW  0
     BSF    STATUS, RP0
     MOVWF  ADCON1
     BCF    STATUS, RP0
     BSF    ADCON0, ADON
     PAGESEL (READA2D)
     CALL   READA2D
     BCF    STATUS, RP1
     BCF    STATUS, RP0
     MOVF   _WVAR_H, W
     MOVWF  _LINEL_L
     CLRF   _LINEL_H

     BCF    STATUS, RP1
     BCF    STATUS, RP0
     RETURN

endasm
start:

' Init timer to trigger ISR 
asm
    MOVLW  B'00010000'
    BCF    STATUS, RP0
    MOVWF  T1CON
    BCF    STATUS, RP0
    BCF    PIR1, TMR1IF
    MOVLW  60
    MOVWF  TMR1H
    MOVLW  176
    MOVWF  TMR1L
    BSF    T1CON, TMR1ON
    BSF    STATUS, RP0
    BSF    PIE1, TMR1IE
    BCF    STATUS, RP0
endasm

' Enable interrupts
asm
    BSF    INTCON, GIE
    BSF    INTCON, PEIE
endasm

High PORTB.1 ' Initialize Servo port as output
High PORTB.2 ' Initialize Servo port as output


NEUTRAL = 15

LCDOUT $FE,1,"MarkIII Template 25"
HSEROUT ["MarkIII Template 25 ", 13, 10]


pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535

'LCDOUT $FE,64,$1f,$11,$11,$11,$11,$11,$11,$1f, ' Square 
LCDOUT $FE,64,0,28,20,23,0,15,9,25

LCDOUT $FE,$1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
LCDOUT $FE,$c0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0


pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535
pauseUS 65535

SERVOL = NEUTRAL ' Neutral position
SERVOR = NEUTRAL ' Neutral position
loop:
CALL READEYES
CALL READSENSORS

LCDOUT $FE,1,"<",dec3 LEYE,">          <",DEC3 REYE,">"
LCDOUT $FE,$C0,"[",DEC3 LINEL,"]   [", DEC3 LINEC,"]  [", DEC3 LINER,"]"
PAUSEUS 65535

ledgeloop:
CALL READSENSORS
IF (LINEL < 30) then
    HSEROUT ["Left edge...",13,10]
'    SERVOL = NEUTRAL + 2
    SERVOR = NEUTRAL - 1
    IF LINER > 30 then goto ledgeloop
endif

redgeloop:
CALL READSENSORS
if LINER < 30 then
    HSEROUT ["Right edge...",13,10]
'    SERVOR = NEUTRAL - 2
    SERVOL = NEUTRAL + 1
    IF LINEL > 30 then goto redgeloop
endif

IF (LINEC < 30) or ((LINER < 30) and (LINEL < 30)) THEN
    SERVOL = NEUTRAL - 1
    SERVOR = NEUTRAL + 1
    HSEROUT ["Backing up for 1 sec..."]
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    PAUSEUS 50000
    HSEROUT ["Done. Now turning for 1/2 secs..."]

    SERVOL = NEUTRAL + 1
    RANDOM CHOICE
    CHOICE = CHOICE / 12000       
    CHOICE = CHOICE + 5
    for T = 1 to CHOICE
    CALL READEYES
    if abs(LEYE - REYE) > 30 then EXITURN
    PAUSEUS 50000
    next
    exiturn:

    SERVOR = NEUTRAL - 1
    HSEROUT ["Done",13,10]
ENDIF

if (LEYE > 40) and (REYE < 30) THEN
    goto targetleft
endif

if (REYE > 40) and (LEYE < 30) THEN
    goto targetright
endif

IF (LEYE > 40) ANd (REYE > 40) THEN
    GOTO pushtarget
ENDIF

RANDOM CHOICE

IF CHOICE > 21845 then
    IF CHOICE < 43690 THEN
        now = left
        SERVOL = NEUTRAL + 1
        SERVOR = NEUTRAL - 1
    ELSE
        SERVOL = NEUTRAL + 1
        SERVOR = NEUTRAL - 1
    endif 
ELSE
    now = right
    SERVOL = NEUTRAL + 1
    SERVOR = NEUTRAL - 1
ENDIF
goto loop
targetleft:
 NOW = LEFT
 HSEROUT["Target on left!",13,10]
 SERVOL = NEUTRAL - 1
 SERVOR = NEUTRAL - 1
goto loop

targetright:
 HSEROUT["Target on right!",13,10]
 now = right
 SERVOR = NEUTRAL + 1
 SERVOL = NEUTRAL + 1
goto loop

pushtarget:
 HSEROUT["Target forward!",13,10]
if now <> forward then
 SERVOL = NEUTRAL + 1
 SERVOR = NEUTRAL - 1
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
    PAUSEUS 50000
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
    PAUSEUS 50000
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
 SERVOL = NEUTRAL + 1
 SERVOR = NEUTRAL - 1
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
    PAUSEUS 50000
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
 SERVOL = NEUTRAL + 1
 SERVOR = NEUTRAL - 1
    PAUSEUS 50000
if linel < 30 then goto ledgeloop
if liner < 30 then goto redgeloop
 SERVOL = NEUTRAL + 1
 SERVOR = NEUTRAL - 1
    PAUSEUS 50000
 SERVOL = NEUTRAL + 1
 SERVOR = NEUTRAL - 1
 now = forward
 endif
goto loop