// Program to move forward and avoid obsticles.
// Servo are modified for continious rotation. 
// Servo=2550 equals zero speed. Calibrate servo or change value as needed.

void main()

{    
    printf("AVOID1 Test\n");
    
    // delay startup when turning on robot
    msleep (1000L);      
    beep();
    msleep (1000L);    
    beep();
    msleep (1000L);
    printf("GO FORWARD AND  AVOID ALL STUFF.\n");
    init_expbd_servos(1);
    
    while(1) 
      {
        // if right whisker is touched, backup and turn left
        if(digital(14)==0 )  
          {
            bit_clear (0x5000,0b00000110);
            servo0=2600; servo1=2500;
        }
        else
          { 
            beep();
            servo0=2500; servo1=2600;
            bit_set (0x5000,0b00000100);
            printf("GO BACKWARD\n");
            msleep (2000L);
            printf("TURN LEFT\n");
            servo0=2500; servo1=2500;
            beep();
            beep();
            msleep (500L);
            printf("GO FORWARD AND  AVOID ALL STUFF.\n");
        }
        // if left side whisker is touched backup and turn right
        if (digital (15)==0 )
          {
            servo0=2600; servo1=2500;
        }           
        else
          {  
            servo0=2500; servo1=2600;
            beep();
            printf("GO BACKWARD\n");
            bit_set (0x5000,0b00000010); 
            msleep (2000L);
            printf("TURN RIGHT\n");
            servo0=2600; servo1=2600;
            beep();
            beep();
            msleep (600L);
            printf("GO FORWARD AND  AVOID ALL STUFF.\n");
            msleep (500L);
        }
        if (analog (16)<70)
          {
            printf ("ET SENSOR= %d \n",analog (16));
            msleep (100L);
            servo0=2600; servo1=2500;
        }
        else   
          {
            printf("OBJECT AHEAD\n");
            servo0=2500; servo1=2600;
            bit_set (0x5000,0b00000110);
            msleep (2000L);
            printf("TURN RIGHT\n");
            servo0=2600; servo1=2600;
            beep();
            beep();
            msleep (700L);
            printf("GO FORWARD AND  AVOID ALL STUFF.\n");
            msleep(500L); 
        }
        
    }
    
}