; This program, "avoid.was", uses the first six sensors
; to perform a simple, dynamic obstacle avoidance routine. 

proc main

string sensor		; Variable declaration section
string bogus

string data1
string data2
string data3
string data4
string data5
string data6
string data7
string data8

integer index
integer straight
integer rght
integer data1_i
integer data2_i
integer data3_i
integer data4_i
integer data5_i
integer data6_i
integer data7_i
integer data8_i

straight = 1
rght = 1
while 1                       	

;transmit "D,1,1^M"        	; Move straight ahead		
;set aspect rxdata ON
;rget bogus
;set aspect rxdata OFF
;
transmit "N^M"        		; Get the sensor data with "N" command       
set aspect rxdata ON		; the robot's response will be 
rget sensor				; n,s1,s2,s3,s4,s4,s5,s6,s7,s8
set aspect rxdata OFF

strfind sensor "," index	; Remove n,
index++
strdelete sensor 0 index

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

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

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

strfind sensor "," index	; data4_i
substr data4 sensor 0 index
index++
strdelete sensor 0 index
atoi data4 data4_i
statmsg "%d" data4_i

strfind sensor "," index	; data5_i
substr data5 sensor 0 index
index++
strdelete sensor 0 index
atoi data5 data5_i
statmsg "%d" data5_i

strfind sensor "," index	; data6_i
substr data6 sensor 0 index
index++
strdelete sensor 0 index
atoi data6 data6_i
statmsg "%d" data6_i

strfind sensor "," index	; data7_i
substr data7 sensor 0 index
index++
strdelete sensor 0 index
atoi data7 data7_i
statmsg "%d" data7_i

data8 = sensor			; data8_i
atoi data8 data8_i
statmsg "%d" data8_i

; This is a simple avoidance routine. When an obstacle is in 
; front of a sensor, the reading is high. The robot reacts by
; stopping, then turning away. 

; An improvement to the algorithm would be to consider the 
; case where multiple sensors are being activated at a time. 

; Another improvement would be to handle the case where the
; robot is in a corner. Which sensors would indicate that the
; robot is in a corner? How then, should the robot react. 

;if ((data3_i > 500)&&(data4_i >500)) 	; Two front sensors
;	transmit "D,0,0^M"        		
;	set aspect rxdata ON
;	rget bogus
;	set aspect rxdata OFF
;
;	transmit "D,4,-1^M"        		
;	set aspect rxdata ON
;	rget bogus
;	set aspect rxdata OFF
;
;endif

if (straight==1)	;This causes the robot to quickly move forward until it finds a wall
	transmit "D,4,4^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	if ((data1_i>100)||(data2_i>100)||(data3_i>100)||(data4_i>100)||(data5_i>100)||(data6_i>100))
		straight=0
	endif
elseif((straight==0)&&(rght==1)) ;Once the wall is found it will then rotate until // to wall
	transmit "D,-2,4^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	if (data6_i>400)
		rght=0
	endif
elseif (( data5_i>=600)||(data6_i>=500))   ;hard turn if there is a wall
	transmit "D,0,0^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "G,0,0^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "C,-400,400^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
elseif (data4_i>=400)  ;soft turn if there is a wall
	transmit "D,0,0^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "G,0,0^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "C,-300,300^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
elseif (data4_i<400) ; move straight
	transmit "D,1,1^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
endif
if ((straight==0)&&(data6_i<100)&&(rght==0))  ;if it looses the wall it turns to find it
	transmit "D,0,0^M"        		
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "G,0,0^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
	transmit "C,300,-100^M"
	set aspect rxdata ON
	rget bogus
	set aspect rxdata OFF
endif
endwhile

endproc