Fabian Wenger / Mbed 2 deprecated Rainbow

Grundfunktionen für Micromouse

Dependencies:   AutomationElements mbed

ReadSensor.cpp@1:4808f55970e8, 2018-04-22 (annotated)

Committer:
wengefa1
Date:
Sun Apr 22 09:55:40 2018 +0000
Revision:
1:4808f55970e8
Parent:
0:e38b500d6e74 
Grundfunktionen funktionieren

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 float distance;
wengefa1 0:e38b500d6e74 18 float filteredDistance;
wengefa1 0:e38b500d6e74 19 int Glob_Position;
wengefa1 0:e38b500d6e74 20
wengefa1 0:e38b500d6e74 21 void calc()
wengefa1 0:e38b500d6e74 22 {
wengefa1 0:e38b500d6e74 23 switch(Glob_Position) {
wengefa1 0:e38b500d6e74 24 case 1:
wengefa1 0:e38b500d6e74 25 sensor_Left.startMeasurement();
wengefa1 0:e38b500d6e74 26 break;
wengefa1 0:e38b500d6e74 27
wengefa1 0:e38b500d6e74 28 case 2:
wengefa1 0:e38b500d6e74 29 sensor_Front.startMeasurement();
wengefa1 0:e38b500d6e74 30 break;
wengefa1 0:e38b500d6e74 31
wengefa1 0:e38b500d6e74 32 case 3:
wengefa1 0:e38b500d6e74 33 sensor_Right.startMeasurement();
wengefa1 0:e38b500d6e74 34 break;
wengefa1 0:e38b500d6e74 35
wengefa1 0:e38b500d6e74 36 default:
wengefa1 0:e38b500d6e74 37 break;
wengefa1 0:e38b500d6e74 38 }
wengefa1 0:e38b500d6e74 39 }
wengefa1 0:e38b500d6e74 40
wengefa1 1:4808f55970e8 41 int readSensor(int Position)
wengefa1 0:e38b500d6e74 42 {
wengefa1 0:e38b500d6e74 43 Glob_Position = Position;
wengefa1 0:e38b500d6e74 44 //PT1 filter(1, 2, sampleTime);
wengefa1 0:e38b500d6e74 45
wengefa1 0:e38b500d6e74 46 sensor_Left.setRanges(2, 400);
wengefa1 0:e38b500d6e74 47 sensor_Front.setRanges(2,400);
wengefa1 0:e38b500d6e74 48 sensor_Right.setRanges(2, 400);
wengefa1 0:e38b500d6e74 49 //ticker_sens.attach(callback(this, &ReadSensor::calc, sampleTime));
wengefa1 0:e38b500d6e74 50 while(true) {
wengefa1 0:e38b500d6e74 51 switch(Position) {
wengefa1 0:e38b500d6e74 52 case 1:
wengefa1 0:e38b500d6e74 53 while(!sensor_Left.isNewDataReady()) {
wengefa1 0:e38b500d6e74 54 // wait for new data
wengefa1 0:e38b500d6e74 55 }
wengefa1 0:e38b500d6e74 56 distance = sensor_Left.getDistance_cm();
wengefa1 0:e38b500d6e74 57 break;
wengefa1 0:e38b500d6e74 58
wengefa1 0:e38b500d6e74 59 case 2:
wengefa1 0:e38b500d6e74 60 while(!sensor_Front.isNewDataReady()) {
wengefa1 0:e38b500d6e74 61 // wait for new data
wengefa1 0:e38b500d6e74 62 }
wengefa1 0:e38b500d6e74 63 distance = sensor_Front.getDistance_cm();
wengefa1 0:e38b500d6e74 64 break;
wengefa1 0:e38b500d6e74 65
wengefa1 0:e38b500d6e74 66 case 3:
wengefa1 0:e38b500d6e74 67 while(!sensor_Right.isNewDataReady()) {
wengefa1 0:e38b500d6e74 68 // wait for new data
wengefa1 0:e38b500d6e74 69 }
wengefa1 0:e38b500d6e74 70 distance = sensor_Right.getDistance_cm();
wengefa1 0:e38b500d6e74 71 break;
wengefa1 0:e38b500d6e74 72
wengefa1 0:e38b500d6e74 73 default:
wengefa1 0:e38b500d6e74 74 return true;
wengefa1 0:e38b500d6e74 75 break;
wengefa1 0:e38b500d6e74 76 }
wengefa1 0:e38b500d6e74 77
wengefa1 0:e38b500d6e74 78 if(distance <= 9.00) {
wengefa1 0:e38b500d6e74 79 //printf("t%f\n",filteredDistance);
wengefa1 0:e38b500d6e74 80 printf("t%f\n",distance);
wengefa1 0:e38b500d6e74 81 return 1;
wengefa1 0:e38b500d6e74 82
wengefa1 0:e38b500d6e74 83 } else {
wengefa1 0:e38b500d6e74 84 //printf("f%f\n",filteredDistance);
wengefa1 0:e38b500d6e74 85 printf("t%f\n",distance);
wengefa1 0:e38b500d6e74 86 return 0;
wengefa1 0:e38b500d6e74 87
wengefa1 0:e38b500d6e74 88 }
wengefa1 0:e38b500d6e74 89
wengefa1 0:e38b500d6e74 90 }
wengefa1 0:e38b500d6e74 91 }
wengefa1 0:e38b500d6e74 92
wengefa1 1:4808f55970e8 93 float readSensorValue(int Position)
wengefa1 0:e38b500d6e74 94 {
wengefa1 0:e38b500d6e74 95 Glob_Position = Position;
wengefa1 0:e38b500d6e74 96 float distancetot = 0;
wengefa1 0:e38b500d6e74 97 int i = 0;
wengefa1 0:e38b500d6e74 98
wengefa1 0:e38b500d6e74 99 sensor_Left.setRanges(2, 400);
wengefa1 0:e38b500d6e74 100 sensor_Front.setRanges(2,400);
wengefa1 0:e38b500d6e74 101 sensor_Right.setRanges(2, 400);
wengefa1 0:e38b500d6e74 102 while(true) {
wengefa1 1:4808f55970e8 103 for(i = 0; i < 2; i++) {
wengefa1 0:e38b500d6e74 104 switch(Position) {
wengefa1 0:e38b500d6e74 105 case 1:
wengefa1 0:e38b500d6e74 106 while(!sensor_Left.isNewDataReady()) {
wengefa1 0:e38b500d6e74 107 // wait for new data
wengefa1 0:e38b500d6e74 108 }
wengefa1 0:e38b500d6e74 109 distance = sensor_Left.getDistance_cm();
wengefa1 0:e38b500d6e74 110 break;
wengefa1 0:e38b500d6e74 111
wengefa1 0:e38b500d6e74 112 case 2:
wengefa1 0:e38b500d6e74 113 while(!sensor_Front.isNewDataReady()) {
wengefa1 0:e38b500d6e74 114 // wait for new data
wengefa1 0:e38b500d6e74 115 }
wengefa1 0:e38b500d6e74 116 distance = sensor_Front.getDistance_cm();
wengefa1 0:e38b500d6e74 117 break;
wengefa1 0:e38b500d6e74 118
wengefa1 0:e38b500d6e74 119 case 3:
wengefa1 0:e38b500d6e74 120 while(!sensor_Right.isNewDataReady()) {
wengefa1 0:e38b500d6e74 121 // wait for new data
wengefa1 0:e38b500d6e74 122 }
wengefa1 0:e38b500d6e74 123 distance = sensor_Right.getDistance_cm();
wengefa1 0:e38b500d6e74 124 break;
wengefa1 0:e38b500d6e74 125
wengefa1 0:e38b500d6e74 126 default:
wengefa1 0:e38b500d6e74 127 return true;
wengefa1 0:e38b500d6e74 128 break;
wengefa1 0:e38b500d6e74 129 }
wengefa1 0:e38b500d6e74 130 distancetot = distancetot + distance;
wengefa1 0:e38b500d6e74 131 }
wengefa1 0:e38b500d6e74 132 //ticker_sens.detach();
wengefa1 1:4808f55970e8 133 distancetot = distancetot / (i);
wengefa1 1:4808f55970e8 134 printf("%f\n",distancetot);
wengefa1 0:e38b500d6e74 135 return distancetot;
wengefa1 0:e38b500d6e74 136 //filter.in(distance);
wengefa1 0:e38b500d6e74 137 //filteredDistance = filter.out();
wengefa1 0:e38b500d6e74 138
wengefa1 0:e38b500d6e74 139
wengefa1 0:e38b500d6e74 140 }
wengefa1 0:e38b500d6e74 141 }