Fabian Wenger / Mbed 2 deprecated Rainbow

Grundfunktionen für Micromouse

Dependencies:   AutomationElements mbed

ReadSensor.cpp@0:e38b500d6e74, 2018-04-19 (annotated)

Committer:
wengefa1
Date:
Thu Apr 19 11:31:49 2018 +0000
Revision:
0:e38b500d6e74
Child:
1:4808f55970e8
keine Regelung vorhanden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wengefa1 0:e38b500d6e74 1 #include "mbed.h"
wengefa1 0:e38b500d6e74 2 #include "HCSR04.h"
wengefa1 0:e38b500d6e74 3 #include "AutomationElements.h"
wengefa1 0:e38b500d6e74 4
wengefa1 0:e38b500d6e74 5 InterruptIn EchoL(PA_0);
wengefa1 0:e38b500d6e74 6 InterruptIn EchoF(PA_1);
wengefa1 0:e38b500d6e74 7 InterruptIn EchoR(PA_4);
wengefa1 0:e38b500d6e74 8
wengefa1 0:e38b500d6e74 9 DigitalOut TriggerL(PB_3);
wengefa1 0:e38b500d6e74 10 DigitalOut TriggerF(PB_4);
wengefa1 0:e38b500d6e74 11 DigitalOut TriggerR(PB_5);
wengefa1 0:e38b500d6e74 12
wengefa1 0:e38b500d6e74 13 HCSR04 sensor_Left(EchoL, TriggerL);
wengefa1 0:e38b500d6e74 14 HCSR04 sensor_Front(EchoF, TriggerF);
wengefa1 0:e38b500d6e74 15 HCSR04 sensor_Right(EchoR, TriggerR);
wengefa1 0:e38b500d6e74 16
wengefa1 0:e38b500d6e74 17 //PwmOut& pwmLeft
wengefa1 0:e38b500d6e74 18
wengefa1 0:e38b500d6e74 19 //float sampleTime = 0.5f;
wengefa1 0:e38b500d6e74 20 //PT1 filter(1, 2, sampleTime);
wengefa1 0:e38b500d6e74 21 //Ticker ticker_sens;
wengefa1 0:e38b500d6e74 22 float distance;
wengefa1 0:e38b500d6e74 23 float filteredDistance;
wengefa1 0:e38b500d6e74 24 int Glob_Position;
wengefa1 0:e38b500d6e74 25
wengefa1 0:e38b500d6e74 26 void calc()
wengefa1 0:e38b500d6e74 27 {
wengefa1 0:e38b500d6e74 28 switch(Glob_Position) {
wengefa1 0:e38b500d6e74 29 case 1:
wengefa1 0:e38b500d6e74 30 sensor_Left.startMeasurement();
wengefa1 0:e38b500d6e74 31 break;
wengefa1 0:e38b500d6e74 32
wengefa1 0:e38b500d6e74 33 case 2:
wengefa1 0:e38b500d6e74 34 sensor_Front.startMeasurement();
wengefa1 0:e38b500d6e74 35 break;
wengefa1 0:e38b500d6e74 36
wengefa1 0:e38b500d6e74 37 case 3:
wengefa1 0:e38b500d6e74 38 sensor_Right.startMeasurement();
wengefa1 0:e38b500d6e74 39 break;
wengefa1 0:e38b500d6e74 40
wengefa1 0:e38b500d6e74 41 default:
wengefa1 0:e38b500d6e74 42 break;
wengefa1 0:e38b500d6e74 43 }
wengefa1 0:e38b500d6e74 44 }
wengefa1 0:e38b500d6e74 45
wengefa1 0:e38b500d6e74 46 int ReadSensor(int Position)
wengefa1 0:e38b500d6e74 47 {
wengefa1 0:e38b500d6e74 48 Glob_Position = Position;
wengefa1 0:e38b500d6e74 49 //PT1 filter(1, 2, sampleTime);
wengefa1 0:e38b500d6e74 50
wengefa1 0:e38b500d6e74 51 sensor_Left.setRanges(2, 400);
wengefa1 0:e38b500d6e74 52 sensor_Front.setRanges(2,400);
wengefa1 0:e38b500d6e74 53 sensor_Right.setRanges(2, 400);
wengefa1 0:e38b500d6e74 54 //ticker_sens.attach(callback(this, &ReadSensor::calc, sampleTime));
wengefa1 0:e38b500d6e74 55 while(true) {
wengefa1 0:e38b500d6e74 56 switch(Position) {
wengefa1 0:e38b500d6e74 57 case 1:
wengefa1 0:e38b500d6e74 58 while(!sensor_Left.isNewDataReady()) {
wengefa1 0:e38b500d6e74 59 // wait for new data
wengefa1 0:e38b500d6e74 60 }
wengefa1 0:e38b500d6e74 61 distance = sensor_Left.getDistance_cm();
wengefa1 0:e38b500d6e74 62 break;
wengefa1 0:e38b500d6e74 63
wengefa1 0:e38b500d6e74 64 case 2:
wengefa1 0:e38b500d6e74 65 while(!sensor_Front.isNewDataReady()) {
wengefa1 0:e38b500d6e74 66 // wait for new data
wengefa1 0:e38b500d6e74 67 }
wengefa1 0:e38b500d6e74 68 distance = sensor_Front.getDistance_cm();
wengefa1 0:e38b500d6e74 69 break;
wengefa1 0:e38b500d6e74 70
wengefa1 0:e38b500d6e74 71 case 3:
wengefa1 0:e38b500d6e74 72 while(!sensor_Right.isNewDataReady()) {
wengefa1 0:e38b500d6e74 73 // wait for new data
wengefa1 0:e38b500d6e74 74 }
wengefa1 0:e38b500d6e74 75 distance = sensor_Right.getDistance_cm();
wengefa1 0:e38b500d6e74 76 break;
wengefa1 0:e38b500d6e74 77
wengefa1 0:e38b500d6e74 78 default:
wengefa1 0:e38b500d6e74 79 return true;
wengefa1 0:e38b500d6e74 80 break;
wengefa1 0:e38b500d6e74 81 }
wengefa1 0:e38b500d6e74 82
wengefa1 0:e38b500d6e74 83 if(distance <= 9.00) {
wengefa1 0:e38b500d6e74 84 //printf("t%f\n",filteredDistance);
wengefa1 0:e38b500d6e74 85 printf("t%f\n",distance);
wengefa1 0:e38b500d6e74 86 return 1;
wengefa1 0:e38b500d6e74 87
wengefa1 0:e38b500d6e74 88 } else {
wengefa1 0:e38b500d6e74 89 //printf("f%f\n",filteredDistance);
wengefa1 0:e38b500d6e74 90 printf("t%f\n",distance);
wengefa1 0:e38b500d6e74 91 return 0;
wengefa1 0:e38b500d6e74 92
wengefa1 0:e38b500d6e74 93 }
wengefa1 0:e38b500d6e74 94
wengefa1 0:e38b500d6e74 95 }
wengefa1 0:e38b500d6e74 96 }
wengefa1 0:e38b500d6e74 97
wengefa1 0:e38b500d6e74 98 float ReadSensorValue(int Position)
wengefa1 0:e38b500d6e74 99 {
wengefa1 0:e38b500d6e74 100 Glob_Position = Position;
wengefa1 0:e38b500d6e74 101 float distancetot = 0;
wengefa1 0:e38b500d6e74 102 int i = 0;
wengefa1 0:e38b500d6e74 103
wengefa1 0:e38b500d6e74 104 sensor_Left.setRanges(2, 400);
wengefa1 0:e38b500d6e74 105 sensor_Front.setRanges(2,400);
wengefa1 0:e38b500d6e74 106 sensor_Right.setRanges(2, 400);
wengefa1 0:e38b500d6e74 107 while(true) {
wengefa1 0:e38b500d6e74 108 for(i = 0; i < 3; i++) {
wengefa1 0:e38b500d6e74 109 switch(Position) {
wengefa1 0:e38b500d6e74 110 case 1:
wengefa1 0:e38b500d6e74 111 while(!sensor_Left.isNewDataReady()) {
wengefa1 0:e38b500d6e74 112 // wait for new data
wengefa1 0:e38b500d6e74 113 }
wengefa1 0:e38b500d6e74 114 distance = sensor_Left.getDistance_cm();
wengefa1 0:e38b500d6e74 115 break;
wengefa1 0:e38b500d6e74 116
wengefa1 0:e38b500d6e74 117 case 2:
wengefa1 0:e38b500d6e74 118 while(!sensor_Front.isNewDataReady()) {
wengefa1 0:e38b500d6e74 119 // wait for new data
wengefa1 0:e38b500d6e74 120 }
wengefa1 0:e38b500d6e74 121 distance = sensor_Front.getDistance_cm();
wengefa1 0:e38b500d6e74 122 break;
wengefa1 0:e38b500d6e74 123
wengefa1 0:e38b500d6e74 124 case 3:
wengefa1 0:e38b500d6e74 125 while(!sensor_Right.isNewDataReady()) {
wengefa1 0:e38b500d6e74 126 // wait for new data
wengefa1 0:e38b500d6e74 127 }
wengefa1 0:e38b500d6e74 128 distance = sensor_Right.getDistance_cm();
wengefa1 0:e38b500d6e74 129 break;
wengefa1 0:e38b500d6e74 130
wengefa1 0:e38b500d6e74 131 default:
wengefa1 0:e38b500d6e74 132 return true;
wengefa1 0:e38b500d6e74 133 break;
wengefa1 0:e38b500d6e74 134 }
wengefa1 0:e38b500d6e74 135 distancetot = distancetot + distance;
wengefa1 0:e38b500d6e74 136 }
wengefa1 0:e38b500d6e74 137 //ticker_sens.detach();
wengefa1 0:e38b500d6e74 138 distancetot = distancetot / i;
wengefa1 0:e38b500d6e74 139 return distancetot;
wengefa1 0:e38b500d6e74 140 //filter.in(distance);
wengefa1 0:e38b500d6e74 141 //filteredDistance = filter.out();
wengefa1 0:e38b500d6e74 142
wengefa1 0:e38b500d6e74 143
wengefa1 0:e38b500d6e74 144 }
wengefa1 0:e38b500d6e74 145 }