SLRC-2019 / Mbed 2 deprecated speed_robot

code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

tof.h@5:bb0763ae79ff, 2019-09-30 (annotated)

Committer:
deepanaishtaweera174
Date:
Mon Sep 30 10:37:06 2019 +0000
Revision:
5:bb0763ae79ff
Parent:
4:538efc40388e
Child:
6:a635edf31d8b
changes added;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
deepanaishtaweera174 1:b188e27eb7da 1 void InitTOFSensors()
deepanaishtaweera174 1:b188e27eb7da 2 {
deepanaishtaweera174 3:9b2f15b0d47b 3 DigitalOut xshut1(XSHUT_pin1);
deepanaishtaweera174 3:9b2f15b0d47b 4 DigitalOut xshut2(XSHUT_pin2);
deepanaishtaweera174 3:9b2f15b0d47b 5 DigitalOut xshut3(XSHUT_pin3);
deepanaishtaweera174 3:9b2f15b0d47b 6
deepanaishtaweera174 5:bb0763ae79ff 7 DigitalIn xshut3_(XSHUT_pin3);
deepanaishtaweera174 3:9b2f15b0d47b 8 wait(0.01);
deepanaishtaweera174 3:9b2f15b0d47b 9 Sensor3.setAddress(Sensor3_newAddress);
deepanaishtaweera174 3:9b2f15b0d47b 10
deepanaishtaweera174 5:bb0763ae79ff 11 DigitalIn xshut2_(XSHUT_pin2);
deepanaishtaweera174 3:9b2f15b0d47b 12 wait(0.01);
deepanaishtaweera174 3:9b2f15b0d47b 13 Sensor2.setAddress(Sensor2_newAddress);
deepanaishtaweera174 3:9b2f15b0d47b 14
deepanaishtaweera174 5:bb0763ae79ff 15 DigitalIn xshut1_(XSHUT_pin1);
deepanaishtaweera174 3:9b2f15b0d47b 16 wait(0.01);
deepanaishtaweera174 3:9b2f15b0d47b 17 Sensor1.setAddress(Sensor1_newAddress);
deepanaishtaweera174 4:538efc40388e 18
deepanaishtaweera174 3:9b2f15b0d47b 19 if (Sensor1.init())
deepanaishtaweera174 3:9b2f15b0d47b 20 {
deepanaishtaweera174 3:9b2f15b0d47b 21 PrintSerial("Sensor 1 : Configured");
deepanaishtaweera174 3:9b2f15b0d47b 22 }
deepanaishtaweera174 3:9b2f15b0d47b 23 else
deepanaishtaweera174 3:9b2f15b0d47b 24 {
deepanaishtaweera174 3:9b2f15b0d47b 25 PrintSerial("Sensor 1 : Configuration Failed");
deepanaishtaweera174 3:9b2f15b0d47b 26 }
deepanaishtaweera174 3:9b2f15b0d47b 27 if (Sensor2.init())
deepanaishtaweera174 3:9b2f15b0d47b 28 {
deepanaishtaweera174 3:9b2f15b0d47b 29 PrintSerial("Sensor 2 : Configured");
deepanaishtaweera174 3:9b2f15b0d47b 30 }
deepanaishtaweera174 3:9b2f15b0d47b 31 else
deepanaishtaweera174 3:9b2f15b0d47b 32 {
deepanaishtaweera174 3:9b2f15b0d47b 33 PrintSerial("Sensor 2 : Configuration Failed");
deepanaishtaweera174 3:9b2f15b0d47b 34 }
deepanaishtaweera174 3:9b2f15b0d47b 35 if (Sensor3.init())
deepanaishtaweera174 3:9b2f15b0d47b 36 {
deepanaishtaweera174 3:9b2f15b0d47b 37 PrintSerial("Sensor 3 : Configured");
deepanaishtaweera174 3:9b2f15b0d47b 38 }
deepanaishtaweera174 3:9b2f15b0d47b 39 else
deepanaishtaweera174 3:9b2f15b0d47b 40 {
deepanaishtaweera174 3:9b2f15b0d47b 41 PrintSerial("Sensor 3 : Configuration Failed");
deepanaishtaweera174 3:9b2f15b0d47b 42 }
deepanaishtaweera174 3:9b2f15b0d47b 43
deepanaishtaweera174 3:9b2f15b0d47b 44 Sensor1.setSignalRateLimit(SignalRateLimit);
deepanaishtaweera174 3:9b2f15b0d47b 45 Sensor2.setSignalRateLimit(SignalRateLimit);
deepanaishtaweera174 3:9b2f15b0d47b 46 Sensor3.setSignalRateLimit(SignalRateLimit);
deepanaishtaweera174 3:9b2f15b0d47b 47
deepanaishtaweera174 3:9b2f15b0d47b 48 Sensor1.setMeasurementTimingBudget(MeasurementBudget);
deepanaishtaweera174 3:9b2f15b0d47b 49 Sensor2.setMeasurementTimingBudget(MeasurementBudget);
deepanaishtaweera174 3:9b2f15b0d47b 50 Sensor3.setMeasurementTimingBudget(MeasurementBudget);
deepanaishtaweera174 3:9b2f15b0d47b 51
deepanaishtaweera174 3:9b2f15b0d47b 52 Sensor1.setTimeout(Timeout);
deepanaishtaweera174 3:9b2f15b0d47b 53 Sensor2.setTimeout(Timeout);
deepanaishtaweera174 3:9b2f15b0d47b 54 Sensor3.setTimeout(Timeout);
deepanaishtaweera174 3:9b2f15b0d47b 55
deepanaishtaweera174 3:9b2f15b0d47b 56 Sensor1.startContinuous();
deepanaishtaweera174 3:9b2f15b0d47b 57 Sensor2.startContinuous();
deepanaishtaweera174 3:9b2f15b0d47b 58 Sensor3.startContinuous();
deepanaishtaweera174 1:b188e27eb7da 59 }
deepanaishtaweera174 1:b188e27eb7da 60
deepanaishtaweera174 3:9b2f15b0d47b 61 void PrintTOFDistances()
deepanaishtaweera174 1:b188e27eb7da 62 {
deepanaishtaweera174 3:9b2f15b0d47b 63 front_Distance = Sensor1.readRangeContinuousMillimeters();
deepanaishtaweera174 3:9b2f15b0d47b 64 float front_DistanceCM = (float)( front_Distance / 10.0);
deepanaishtaweera174 3:9b2f15b0d47b 65
deepanaishtaweera174 3:9b2f15b0d47b 66 left_Distance = Sensor2.readRangeContinuousMillimeters() ;
deepanaishtaweera174 3:9b2f15b0d47b 67 float left_DistanceCM = (float)(left_Distance / 10.0);
deepanaishtaweera174 3:9b2f15b0d47b 68
deepanaishtaweera174 3:9b2f15b0d47b 69 right_Distance = Sensor3.readRangeContinuousMillimeters();
deepanaishtaweera174 3:9b2f15b0d47b 70 float right_DistanceCM = (float)(right_Distance / 10.0);
deepanaishtaweera174 3:9b2f15b0d47b 71
deepanaishtaweera174 3:9b2f15b0d47b 72 printf("F: %.2f L: %.2f R: %.2f",front_DistanceCM,left_DistanceCM,right_DistanceCM);
deepanaishtaweera174 3:9b2f15b0d47b 73 }