SLRC-2019 / Mbed 2 deprecated speed_robot

code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

tof.h@3:9b2f15b0d47b, 2019-09-30 (annotated)

Committer:
deepanaishtaweera174
Date:
Mon Sep 30 10:19:31 2019 +0000
Revision:
3:9b2f15b0d47b
Parent:
1:b188e27eb7da
Child:
4:538efc40388e
added tof code for 3 sensors

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