' {$STAMP BS2sx}
' {$PBASIC 2.5}
' {$PORT COM1}

CH       VAR 		Nib
CNT		VAR		Byte
ADC      VAR 		Word
ADC1     VAR 		Word
ADC2     VAR       Word
Init:    DIRD = %1111

         PAUSE 1000
Main:    GOSUB GP2
         IF ((ADC1<10) AND (ADC2<10)) THEN
              GOSUB S_Left
         ENDIF
		IF (ADC1 > (ADC2+100)) THEN
              GOSUB S_Left
         ENDIF
		IF (ADC2 > (ADC1+100)) THEN
              GOSUB S_Right
         ENDIF
         IF ((ABS(ADC1-ADC2)) < 100) THEN
              GOSUB Forward
         ENDIF
         CH=1 : GOSUB RD_ADC
         IF (ADC>350) THEN
            	GOSUB Backward
              PAUSE 1000
              GOSUB S_right
              PAUSE 2000
         ENDIF
GOTO Main


GP2:     CH=0 : GOSUB RD_ADC : ADC1 = ADC
		CH=2 : GOSUB RD_ADC : ADC2 = ADC
         RETURN

'+++++ Analog to Digital Converter Procedure ++++++++++
RD_ADC:	LOW 10: PAUSE 1: HIGH 10  					' Send Acknowledge
        	SEROUT 10,240,[CH]							' Send Select Chip
        	SERIN 10,240,[ADC.BYTE0,ADC.BYTE1] 			' Read ADC
         RETURN
'++++++++++++++++++++++++++++++++++++++++++++++++++++++

'+++++ Generate Sound +++++++++++++++++++++++++++++++++
Beep:    	FREQOUT 11,300,800					' Sound subroutine
			RETURN
'++++++++++++++++++++++++++++++++++++++++++++++++++++++

'+++++ Movement Procedure +++++++++++++++++++++++++++++
Forward:  	OUTD = %1010 : RETURN  	' Motor A --> and Motor B --> Go Forward
Backward: 	OUTD = %0101 : RETURN   	' Motor A <-- and Motor B <-- Go Backward
S_Left:   	OUTD = %0110 : RETURN  	' Robot Spin Left
S_Right:  	OUTD = %1001 : RETURN 	' Robot Spin Right
Motor_OFF: 	OUTD = %0000 : RETURN  	' Stop All Motor
'++++++++++++++++++++++++++++++++++++++++++++++++++++++