awesome_car.c

#define LOWERLIGHT 150
#define UPPERLIGHT 600
#define NEAR 50 //cm

string light;
string distance;
string touch;
int US;
int TOUCH;

sub turn()
{
    OnFwd(OUT_A, 50);
    OnRev(OUT_B, 50);
}

sub forward()
{
    OnRev(OUT_A, 100);
    OnRev(OUT_B, 100);
}

sub stopMotor()
{
    Off(OUT_A);
    Off(OUT_B);
}

sub controlMotor()
{
    US = SensorUS(IN_2);
    TOUCH = Sensor(IN_3);

    if (US > NEAR && TOUCH > 500 )
    {
        forward();
    }
    else
    {
        turn();
        //stopMotor();
    }
}

task main ()
{
    // Configure port 1 for light sensor
    SetSensorColorNone(IN_1);

    // Configure port 2 for UltraSound sensor
    SetSensorLowspeed(IN_2);

    // Configure port 3 for Touch sensor
    SetSensorTouch(IN_3);
    
    // Configure all ports to use maximum resolution in the ADC

    SetSensorMode(IN_1, SENSOR_MODE_RAW);
    ResetSensor(IN_1);

    SetSensorMode(IN_2, SENSOR_MODE_RAW);
    ResetSensor(IN_2);

    SetSensorMode(IN_3, SENSOR_MODE_RAW);
    ResetSensor(IN_3);

    while(true)
    {
        // Convert the sensor input to text strings
        light = NumToStr(Sensor(IN_1));
        distance = NumToStr(SensorUS(IN_2));
        touch = NumToStr(Sensor(IN_3));

        // Print the current distance to the display
        TextOut(0,0,distance,true);

        Wait(200);

        // Access subfroutine with motor control...
        controlMotor();
    }
}