/* In this program the robot scans the environment using the  12 sonars and registers the values returned to the sonars.  After the values are registered, the robot scans the environment again.  If it detects any change from the previous sonar readings, it moves forwards and backwards.  If no change was detected, it rotates left and right.  The program runs forever.
*/


#include "define.h"
#define  obscale_detect   20


void Delay();
void Initialize_B12();
void  Get_BaseP_X();
void Kill_B12();
void Rotate_Const_Clock();
void Rotate_Const_AntiClock();
void Move_Const_Forward();
void Move_Const_Backward();
void Rotate_Clock();
void Rotate_Anti_Clock();
void Move_Forward();
void Move_Back();
void Ping_Sonars();

/*local procedure definination*/

void  cpcur();
void  showno();
void  showchange();
void  detect_enviroment();
/***************************************************************/

/* Declare the code section for the program  
                 */ 
/***************************************************************/



short cur_array[12];

main()
{
extern short Sonar[];
extern short  cur_array[];
extern short right;

Initialize_B12();
detect_enviroment();

/************************************************************/
/* ALL CODE SHOULD BE DECLARED BEFORE THIS                  */
/* this trap should always be present in the main program   */
/************************************************************/
#asm
    trap #0
#endasm
}
                      /* End of Main */
void   detect_enviroment()
{
 int  change,i;
 short diff;
	Ping_Sonars();
	cpcur();
	while  (1)
	{
		Ping_Sonars();
		change=0;
		for (i=0;i<12;i++)
		{
			diff = (cur_array[i]-Sonar[i]);
			if (diff > 25)
				change=1;
		}	
		if  (change==1) 
			showchange();
		else 
			 showno();

		cpcur();
	}
}

void   cpcur()     
{   extern short cur_array[];
   int  i1;

   for(i1=0;i1<12;i1++)
   cur_array[i1]=Sonar[i1];
 }

void  showno()
{    
 extern short Sonar[];
      Rotate_Clock(90);  
     Rotate_Anti_Clock(90);
}

void   showchange()
{
 extern short Sonar[];
     Move_Forward(15);
     Move_Back(15);
}