'Mars Rover, New design uesing stepper motors and alum box tube for chassis
'home made US sensor useing 16F628 
'software pulse to drive US trasduser via MAX489
'also driveing tilt and gripper motors, gripper IR finger gap, grip switch and a thermister for reading
'ver 1.0  July 14, 2004 by Edward Rupp

DEFINE OSC 20
INCLUDE "modedefs.bas"
 CMCON = 7					'For 16F628, unrem to turnoff compariters
 
 Output PORTB.2	'TX to LCD pic
 Low PORTB.2 'set ser out to master low to make shure no confusion of send request

Output PORTB.4	'LED 1 used to indicate if sonar is running ok by flashing
 PORTB.4 = 1		'set toggle state for blik LED system running states light
	
Output PORTB.0	'Pulse out for Ultra Sonic
Input PORTA.5	'Echo in for Ultra Sonic

DataCF VAR WORD 'echo value for Center Front Sonor
SonarRange VAR BYTE	'value reduced to send ser

DataCF = 0	'reset counter for sonar range counter, counting time tell echo is seen or time out, no echo

US_pulseCF VAR BYTE	'For next loop counters for 8 Pulse out train



keepGoing:	'start of main loop

' ***************** Ping Center Front Sonor - Start *********
For US_pulseCF = 1 TO 8
High PORTB.0
 Asm
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 EndAsm

Low PORTB.0
 Asm
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 nop
 EndAsm
Next US_pulseCF

PauseUs 2	'blank for receaver response to tranmiter ping
countTimeCF:
DataCF = DataCF + 1

IF DataCF > 500 Then
	SonarRange = 0	'nothing seen set value to zero
	GoTo noechoCF		'if long time, probibly no close up object, abort count
	EndIF
	
IF PORTA.5 = 1 Then countTimeCF	'if high no echo back yet, keep counting, when low seen echo, use time count

IF  DataCF <= 2 Then	'check to see for a devide by 0 condion'
	SonarRange = 0				'set to zero if so and jump past normal math to convert sonar val for ser
	GoTo noechoCF
		EndIF

SonarRange = DataCF / 2		'devide to convert word to byte to be able to send serial

noechoCF:
' ***************** Ping Center Front Sonor - End *********

SerToMaster:
SerOut PORTB.2,N9600,[SonarRange]

DataCF = 0	'zero the echo timer

Toggle PORTB.4	'flash led on or off each loop, help tell if sonar is stalled

GoTo keepGoing	'goback to top of main loop and start over again