'{$STAMP BS2}
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'''''											  ''''' 
'''''             File:  Alder.bs2							  '''''
'''''     Date created:  9/4/02								  '''''
'''''       Programmer:  Lior Elazary (lelazary@yahoo.com)				  '''''
'''''          Purpose:  A simple neural conroled robot based on a robot called		  '''''
'''''                    Alder by Ulrigh Marchove					  '''''
'''''											  '''''
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'''''											  ''''' 
'''''                    Variable difinition           					  '''''
'''''											  '''''
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
temp				var  word
x				var  byte
time				var  byte

''''''''''''''''''''''' sensor related variables ''''''''''''''''''''''''
Right_Dist 			var  byte
Left_Dist  			var  byte

Right_Sensor_Clock		Con 15
Right_Sensor_Data		Con 14
Right_Sensor_vData		Var IN14

Left_Sensor_Clock		Con 13
Left_Sensor_Data		Con 12
Left_Sensor_vData		Var IN12


''''''''''''''''''''''' motors related variables ''''''''''''''''''''''''
Right_Wheel 		Con 0
Left_Wheel		Con 1

Right_Speed		Var Word
Left_Speed		Var Word

Right_Stop 		Con 646 
Left_Stop		Con 642 

Robot_Speed		Con 40


'''''''''''''''''''''' Nural network variables ''''''''''''''''''''''''''

LW				var bit
RW				var bit
OLW				var bit
ORW				var bit

RT				var bit
LT				var bit
OLT				var bit
ORT				var bit

WlwLT				var word
WlwRT				var word
WrwLT				var word
WrwRT				var word

Learning_Time			Con 20
Learning_Rate			Con 3
Theata				Con 0


'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'''''											  ''''' 
'''''                   initilazation 							  '''''
'''''											  '''''
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

'initilize network

Wlwlt = 0
WlwRT = 0
WrwLT = 0
WrwRT = 0


'set the input and outputs
INPUT	Right_Sensor_Data
HIGH  Right_Sensor_Clock

INPUT	Left_Sensor_Data
HIGH  Left_Sensor_Clock


main:

   GOSUB Move_Robot
   GOSUB Check_Sensors
   time = Learning_Time                              'set the about of time to try out each move

   debug dec RW, ":", dec RT, " ", dec LW, ":", dec LT, CR


   if (LW <> 1) AND (RW <> 1) then continue_robot    'check instick rule
       GOSUB Caculate_net                            'get the network output
   
       instinck:
       debug "instink:", dec RW, ":", dec RT, " ", dec LW, ":", dec LT, CR
       IF (RT < LT) Then Rotate_Right_if             'check which output is bigger and try that
           gosub Rotate_Right
           LT = 1                                    'set wisker values so next time next out is tried
           RT = 0
           debug dec RT, " ", dec LT, CR
           goto End_if_Rotate_Right
       
       Rotate_Right_if:
           gosub Rotate_Left
           RT = 1
           LT = 0
       End_if_Rotate_Right:

       GOSUB Check_Sensors			    'check the insticnk rules again
       if (LW <> 1) AND (RW <> 1) then Learning     'if insticnk rules are fine then learn the mapping  
       
       time = time + (Learning_Time/2)              'if not then increse the time to try the next move
   
       goto instinck:

      
   continue_robot:
   goto main

   Learning:
      IF (time = Learning_Time) Then Main           'if this is the first try then not learn
      RT = RT^1     'flip bits
      LT = LT^1
      GOSUB Learn_Net

   goto main


check_sensors:
	OLW = LW
  	ORW = RW
	GOSUB get_left_sensor_data
	GOSUB Move_Robot
	GOSUB get_right_sensor_data
	GOSUB Move_Robot

	LW = 0
	if Left_Dist < 150 THEN no_lw_change
   		LW = 1
	no_lw_change:

	RW = 0
	if Right_Dist < 150 THEN no_rw_change
   		RW = 1
	no_rw_change:
	RETURN




Caculate_net:

  temp = (LW * WlwLT) + (RW * WrwLT)
  LT = 1
  if (temp <= Theata) then no_LT_change
      LT = 0
  no_LT_change

  temp = (RW * WrwRT) + (LW * WlwRT)
  RT = 1
  if (temp <= Theata) then no_RT_change
      RT = 0
  no_RT_change

  ORT = RT
  OLT = LT

RETURN

Learn_Net:
  debug "Learning:", dec ORW, ":", dec RT, " ", dec OLW, ":", dec LT, CR
  Wlwlt = Wlwlt + (Learning_Rate * (LT - OLT) * OLW)
  Wrwlt = Wrwlt + (Learning_Rate * (LT - OLT) * ORW)

  Wlwrt = Wlwrt + (Learning_Rate * (RT - ORT) * OLW)
  Wrwrt = Wrwrt + (Learning_Rate * (RT - ORT) * ORW)

  RETURN


Move_Robot:
        
        pulsout Right_Wheel, Right_Stop - Robot_Speed
	pulsout Left_Wheel,  Left_Stop +  Robot_Speed 
	return


Rotate_Right:
        debug "Robot Rotating right for:", dec time,CR
        For x = 1 to time
           pulsout Right_Wheel, Right_Stop + Robot_Speed
           pulsout Left_Wheel,  Left_Stop +  Robot_Speed
	   pause 20
        next  

        RETURN

Rotate_Left:
	debug "Robot Rotating Left for:", dec time, CR
        for x = 1 to time
	    pulsout Right_Wheel, Right_Stop - Robot_Speed
      	    pulsout Left_Wheel,  Left_Stop -  Robot_Speed
            pause 20 
        next

     	RETURN

get_right_sensor_data:
	LOW Right_Sensor_Clock
	
	
	right_sensor_loop:
		if Right_Sensor_vData = 0 THEN right_sensor_loop
		SHIFTIN Right_Sensor_Data, Right_Sensor_Clock, MSBPOST, [Right_Dist]
		HIGH Right_Sensor_Clock	
	RETURN   


get_left_sensor_data:
	LOW Left_Sensor_Clock
	
	
	left_sensor_loop:
		if Left_Sensor_vData = 0 THEN left_sensor_loop
		SHIFTIN Left_Sensor_Data, Left_Sensor_Clock, MSBPOST, [Left_Dist]
		HIGH Left_Sensor_Clock

	RETURN