![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |