/*
A simple programm for avoiding
obstacles using the sonic sensor.
It works with the standard NXT bot.

author: Jan-Klaas Kollhof
last changed by: $LastChangedBy$
last changed date: $Date$
revision: $Revision$
*/

#define VeryClose 20
#define Close 40
#define Slow 30
#define Fast 70

task main(){
    SetSensorLowspeed(IN_4);

    
    while(1){
        TextOut(0, LCD_LINE1, "sonic", true);
        NumOut(70, LCD_LINE1, SensorUS(IN_4));

        int dist = SensorUS(IN_4);
        
        
        if(dist  < VeryClose){
             //lets spin 45 deg in a random direction
             if(Random(2) == 0){
                RotateMotorEx(OUT_BC, Fast, 180, -100, true, false);
            }else{
                RotateMotorEx(OUT_BC, Fast, 180, 100, true, false);
            }
            
        }else{
            OnFwd(OUT_BC, Fast);
        }
        Wait(100);
    }
}