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