// 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.
// Forward white leds flash.
// Batt voltage is read and shown.
// Move toward light.

int leftspeed;
int rightspeed;
int diff;
#define leftL analog(3)
#define rightL analog(2)


void main()

{    
    printf("SeekAvoid Test\n");
    
    // delay startup when turning on robot
    
    bit_set (0x5000,0b00011000);
    msleep (1000L);  
    bit_clear (0x5000,0b00011000);
    beep();
    msleep (1000L);    
    bit_set (0x5000,0b00000110);
    beep();
    msleep (1000L);
    bit_clear (0x5000,0b00000110);
    
    init_expbd_servos(1);
    
    while(1) 
      {
        // if right whisker is touched, backup and turn left
        if(digital(14)==1 )  
          // backup and turn left
          { 
            bit_set (0x5000,0b00000010);
            beep();
            servo0=2500; servo1=2600;
            printf("GO BACKWARD\n");
            msleep (2000L); 
            bit_clear (0x5000,0b00000010);
            printf("TURN LEFT\n");
            servo0=2500; servo1=2500;
            beep();   
            beep();
            msleep (500L);
            printf("GO FORWARD AND  AVOID ALL STUFF.\n");
        } 
        
        else   
          
          
          // check left whisker
          if (digital (15)==1 )
            
            {
              
              //backup and turn right  
              servo0=2500; servo1=2600;
              beep();
              printf("GO BACKWARD\n");
              bit_set (0x5000,0b00000100); 
              msleep (2000L);
              printf("TURN RIGHT\n");
              servo0=2600; servo1=2600;
              beep();
              beep();
              bit_clear (0x5000,0b00000100);
              msleep (600L);
              printf("GO FORWARD AND  AVOID OBJECTS.\n");
              msleep (500L);
          }
          
          
          
          //check in front with ET sensor
          if (analog (16)>70)
          
          {
            //backup and turn right    
            
            msleep (100L);          
            beep();
            printf("OBJECT AHEAD\n");
            servo0=2500; servo1=2600;
            bit_set (0x5000,0b00000110);
            beep();
            msleep (1000L);
            printf("TURN RIGHT\n");
            servo0=2600; servo1=2600;
            beep();
            beep();
            bit_clear (0x5000,0b00000110);
            msleep (700L);
            printf("GO FORWARD AND  AVOID OBJECTS.\n");
            msleep(500L); 
            
        }
        
        else
          // if left side whisker is touched backup and turn right
          
          {          
            // read light sensors and go forward toward light     
            
            diff = leftL-rightL; 
            bit_set (0x5000,0b00011000);
            
            leftspeed = 50 + (diff * 2);
            
            // limit leftspeed and rightspeed adjustment
            if (leftspeed > 100)
              leftspeed = 100;
            if (leftspeed < -100)
              leftspeed = -100;
              
              rightspeed = 50 - (diff * 2);
            
            if (rightspeed > 100)
              rightspeed = 100;
            if (rightspeed < -100)
              rightspeed = -100;
            
            servo0 = 2550 + leftspeed;
            servo1 = 2550 - rightspeed;
            
            printf ("Lt=%d   Rt=%d D=%d  V=%d\n",leftL,rightL,diff,(analog(4))); 
            
            bit_clear (0x5000,0b00011000);
            msleep (90L);
            
        }
        
    }
    
}