#define SONAR_HB 1
#define SONAR_0 2
#define SONAR_1 3
#define SONAR_2 4
#define SONAR_3 5
#define SONAR_4 6
#define SONAR_5 7
#define SONAR_6 8
#define SONAR_7 9

#define SafeDist 650


#use "alc_cbus.icb"
// network assembly routines
// plug network into digital input 9

#use "alc_cmps03.ic"
// compass stuff
/*
// plug in compass into digital input 7
int compass();
*/

#use "movement_lib.ic"
// simple movement commands
/* MOVEMENT PROTOTYPES
int forward(int speed);
int back(int speed);
int right(int speed);
int rev_right(int speed);
int left(int speed);
int rev_left(int speed);
int stop();
void movement_init();
*/

#use "sonar_lib.ic"
// sonar functions
/* SONAR PROTOTYPES
int inDanger();
int querySonar(int i);
*/


void main()
{
    int s0, s1, s2, s3, s4, s5, s6, s7;
    int ran;
    movement_init();
    install_PAI_interrupt(0);   // install interrupt into vector table
    stop();
    start_press();              // wait for all connected Handyboards to be turned on before pressing start
    listen_to_cbus(0);          // this enables the interrupt to happen 
    
    
    
    while(1)
      {
        s2 = querySonar(0);
        msleep(5L);
        s4 = querySonar(4);
        
        if(s4 >= 1000 && s2 > SafeDist){
            left(10);
        }
        
        if(s4 < 1000 && s2 > SafeDist){
            right(10);
        }
        if(s2 <= SafeDist){
            right(10);
            sleep(3.);
            forward(10);
        }
    }
}