code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

tof.h

Committer:
deepanaishtaweera174
Date:
2019-09-30
Revision:
4:538efc40388e
Parent:
3:9b2f15b0d47b
Child:
5:bb0763ae79ff

File content as of revision 4:538efc40388e:

void InitTOFSensors()
{
    DigitalOut xshut1(XSHUT_pin1);
    DigitalOut xshut2(XSHUT_pin2);
    DigitalOut xshut3(XSHUT_pin3);
    xshut1 = 0;
    xshut2 = 0;
    xshut3 = 0;

    xshut1 = 0;
    xshut2 = 0;
    xshut3 = 1;
    wait(0.01);
    Sensor3.setAddress(Sensor3_newAddress);

    xshut1 = 0;
    xshut2 = 1;
    xshut3 = 0;
    wait(0.01);
    Sensor2.setAddress(Sensor2_newAddress);

    xshut1 = 1;
    xshut2 = 0;
    xshut3 = 0;
    wait(0.01);
    Sensor1.setAddress(Sensor1_newAddress);

    xshut1 = 0;
    xshut2 = 0;
    xshut3 = 0;
    
    if (Sensor1.init())
    {
        PrintSerial("Sensor 1 : Configured");
    }
    else
    {
        PrintSerial("Sensor 1 : Configuration Failed");
    }
    if (Sensor2.init())
    {
        PrintSerial("Sensor 2 : Configured");
    }
    else
    {
        PrintSerial("Sensor 2 : Configuration Failed");
    }
    if (Sensor3.init())
    {
        PrintSerial("Sensor 3 : Configured");
    }
    else
    {
        PrintSerial("Sensor 3 : Configuration Failed");
    }

    Sensor1.setSignalRateLimit(SignalRateLimit);
    Sensor2.setSignalRateLimit(SignalRateLimit);
    Sensor3.setSignalRateLimit(SignalRateLimit);

    Sensor1.setMeasurementTimingBudget(MeasurementBudget);
    Sensor2.setMeasurementTimingBudget(MeasurementBudget);
    Sensor3.setMeasurementTimingBudget(MeasurementBudget);

    Sensor1.setTimeout(Timeout);
    Sensor2.setTimeout(Timeout);
    Sensor3.setTimeout(Timeout);

    Sensor1.startContinuous();
    Sensor2.startContinuous();
    Sensor3.startContinuous();
}

void PrintTOFDistances()
{
    front_Distance = Sensor1.readRangeContinuousMillimeters();
    float front_DistanceCM = (float)( front_Distance  / 10.0);

    left_Distance = Sensor2.readRangeContinuousMillimeters() ;
    float left_DistanceCM = (float)(left_Distance / 10.0);

    right_Distance = Sensor3.readRangeContinuousMillimeters();
    float right_DistanceCM = (float)(right_Distance / 10.0);
    
    printf("F: %.2f L: %.2f R: %.2f",front_DistanceCM,left_DistanceCM,right_DistanceCM);
}