; This program stop.was stops the robot when an obstacle 
; is in front of forward facing motion. The robot moves 
; straight ahead by default. 

proc main

string sensor		; Variable declaration section
string bogus
string data1
string data2
string data3
string data4

integer index
integer data1_i
integer data2_i
integer data3_i
integer data4_i
                  
transmit "D,1,1^M"           	; Move straight ahead         
set aspect rxdata ON
rget bogus
set aspect rxdata OFF
statmsg "D was transmitted"

while 1   

transmit "N^M"                	; Get the sensor data       
set aspect rxdata ON         	; The robot's response will be of the form 
rget sensor                   	; n,s1,s2,s3,s4,s5,s6,s7,s8
set aspect rxdata OFF          	; We only need to check s3 & s4-the forward facing sensors
                         
strfind sensor "," index        ; Remove n,
inc index
strdelete sensor 0 index
statmsg "%s" sensor

strfind sensor "," index      	; data1_i
substr data1 sensor 0 index   
inc index
strdelete sensor 0 index
atoi data1 data1_i            
statmsg "%d" data1_i

strfind sensor "," index      	; data2_i
substr data2 sensor 0 index
inc index
strdelete sensor 0 index
atoi data2 data2_i
statmsg "%d" data2_i

strfind sensor "," index   	; data3_i
substr data3 sensor 0 index
inc index
strdelete sensor 0 index
atoi data3 data3_i
statmsg "%d" data3_i

strfind sensor "," index  	; data4_i
substr data4 sensor 0 index
inc index
strdelete sensor 0 index
atoi data4 data4_i
statmsg "%d" data4_i
					; Head-on obstacle avoidance 
if ((data3_i > 500)&&(data4_i > 500)) 	; Two front sensors
     transmit "D,0,0^M"                 ; Stop if there's an obstacle
     set aspect rxdata ON
     rget bogus
     set aspect rxdata OFF
endif

if ((data3_i < 500)&&(data4_i < 500)) 	; Two front sensors
     transmit "D,1,1^M"                	; Move ahead if it is clear 
     set aspect rxdata ON
     rget bogus
     set aspect rxdata OFF
endif

endwhile

endproc