SLRC-2019 / Mbed 2 deprecated speed_robot

code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

tof.h@4:538efc40388e, 2019-09-30 (annotated)

Committer:
deepanaishtaweera174
Date:
Mon Sep 30 10:33:12 2019 +0000
Revision:
4:538efc40388e
Parent:
3:9b2f15b0d47b
Child:
5:bb0763ae79ff
changed xshit code

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