Jan Mader / Mbed OS PM2_Distanzhelm

Distanzhelm

Dependencies:   PM2_Libary

main.cpp@8:2ae54c6a9dc6, 2021-05-24 (annotated)

Committer:
madjan01
Date:
Mon May 24 20:44:16 2021 +0000
Revision:
8:2ae54c6a9dc6
Parent:
7:3b3a2c158803 
Abgabe Version 24.05.21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 1:93d997d6b232 2 #include "platform/mbed_thread.h"
pmic 1:93d997d6b232 3 #include "SDBlockDevice.h"
pmic 1:93d997d6b232 4 #include "FATFileSystem.h"
pmic 1:93d997d6b232 5 #include "EncoderCounter.h"
pmic 1:93d997d6b232 6 #include "Servo.h"
pmic 3:d22942631cd7 7 #include "SpeedController.h"
madjan01 7:3b3a2c158803 8 #include "RangeFinder.h"
pmic 1:93d997d6b232 9
madjan01 6:9b34fadd6628 10 using namespace std::chrono;
madjan01 6:9b34fadd6628 11
madjan01 6:9b34fadd6628 12
pmic 1:93d997d6b232 13 DigitalIn user_button(USER_BUTTON);
madjan01 6:9b34fadd6628 14 DigitalOut led(LED1);
pmic 1:93d997d6b232 15
pmic 1:93d997d6b232 16 // create servo objects
madjan01 8:2ae54c6a9dc6 17 Servo servo_0(PA_6); //PA_6
pmic 1:93d997d6b232 18
pmic 1:93d997d6b232 19
madjan01 7:3b3a2c158803 20 //Infrarotsensor
madjan01 8:2ae54c6a9dc6 21 DigitalIn pirrechts(PB_12);
madjan01 8:2ae54c6a9dc6 22 DigitalIn pirvorne(PC_7);
madjan01 8:2ae54c6a9dc6 23 DigitalIn pirhinten(PB_7);
madjan01 8:2ae54c6a9dc6 24 DigitalIn pirlinks(PA_1);
madjan01 8:2ae54c6a9dc6 25
madjan01 8:2ae54c6a9dc6 26 /*
madjan01 8:2ae54c6a9dc6 27 pc-6 pc_ 9 sicher mit putty besetzt pc_8 immer eins
madjan01 8:2ae54c6a9dc6 28
madjan01 8:2ae54c6a9dc6 29
madjan01 8:2ae54c6a9dc6 30 */
madjan01 8:2ae54c6a9dc6 31
madjan01 8:2ae54c6a9dc6 32
madjan01 8:2ae54c6a9dc6 33
madjan01 8:2ae54c6a9dc6 34 // Groove Ultrasonic Ranger V2.0
madjan01 8:2ae54c6a9dc6 35 // https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds
madjan01 8:2ae54c6a9dc6 36 /* create range finder object (ultra sonic distance sensor) */
madjan01 8:2ae54c6a9dc6 37 RangeFinder ultrarechts(PC_5, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 8:2ae54c6a9dc6 38 RangeFinder ultralinks(PC_3, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 8:2ae54c6a9dc6 39 RangeFinder ultravorne(PB_1, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 8:2ae54c6a9dc6 40 RangeFinder ultrahinten(PC_2, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
pmic 1:93d997d6b232 41
madjan01 6:9b34fadd6628 42 //Output
madjan01 6:9b34fadd6628 43 //Servo
madjan01 6:9b34fadd6628 44 DigitalOut pumpe(PB_6);
madjan01 8:2ae54c6a9dc6 45 DigitalOut summer(PA_0);
madjan01 7:3b3a2c158803 46
pmic 4:67506e285ad0 47 /* input your stuff here */
madjan01 6:9b34fadd6628 48
madjan01 6:9b34fadd6628 49 Timer loop_timer;
madjan01 8:2ae54c6a9dc6 50 int Ts_ms = 1000;
madjan01 7:3b3a2c158803 51 char CZustand = 0;
madjan01 8:2ae54c6a9dc6 52 int spritz = 1000; //spritzzeit, summerzeit = 2*spritzzeit
madjan01 8:2ae54c6a9dc6 53 //float dist_USSensor = 0.0f;
madjan01 6:9b34fadd6628 54
madjan01 6:9b34fadd6628 55
madjan01 6:9b34fadd6628 56
madjan01 8:2ae54c6a9dc6 57 /*Interrupt nciht verwendet möglich erweiterung*/
madjan01 6:9b34fadd6628 58
madjan01 6:9b34fadd6628 59 /*
madjan01 6:9b34fadd6628 60 class Counter {
madjan01 6:9b34fadd6628 61 public:
madjan01 6:9b34fadd6628 62 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
madjan01 6:9b34fadd6628 63 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
madjan01 6:9b34fadd6628 64 }
madjan01 6:9b34fadd6628 65
madjan01 6:9b34fadd6628 66 void increment() {
madjan01 6:9b34fadd6628 67 _count++;
madjan01 6:9b34fadd6628 68 }
madjan01 6:9b34fadd6628 69
madjan01 6:9b34fadd6628 70 int read() {
madjan01 6:9b34fadd6628 71 return _count;
madjan01 6:9b34fadd6628 72 }
madjan01 6:9b34fadd6628 73
madjan01 6:9b34fadd6628 74 private:
madjan01 6:9b34fadd6628 75 InterruptIn _interrupt;
madjan01 6:9b34fadd6628 76 volatile int _count;
madjan01 6:9b34fadd6628 77 };
madjan01 6:9b34fadd6628 78
madjan01 6:9b34fadd6628 79 */
madjan01 6:9b34fadd6628 80
madjan01 6:9b34fadd6628 81
madjan01 6:9b34fadd6628 82 int main()
madjan01 6:9b34fadd6628 83 {
madjan01 6:9b34fadd6628 84
madjan01 6:9b34fadd6628 85 servo_0.Enable(1500,20000);
madjan01 6:9b34fadd6628 86
madjan01 8:2ae54c6a9dc6 87 summer =1;
madjan01 8:2ae54c6a9dc6 88 thread_sleep_for(200);
madjan01 8:2ae54c6a9dc6 89 summer = 0;
madjan01 8:2ae54c6a9dc6 90 //thread_sleep_for(15000);
madjan01 8:2ae54c6a9dc6 91
pmic 4:67506e285ad0 92
madjan01 6:9b34fadd6628 93
pmic 1:93d997d6b232 94
pmic 1:93d997d6b232 95 while (true) {
madjan01 6:9b34fadd6628 96
madjan01 6:9b34fadd6628 97 loop_timer.reset();
madjan01 8:2ae54c6a9dc6 98 /* if(user_button == 0) // hilfsstart zustand varbiable muss auf Wert über 10 geändert werden
madjan01 6:9b34fadd6628 99 {
madjan01 6:9b34fadd6628 100 printf("nicht aktiv");
madjan01 6:9b34fadd6628 101 CZustand = 0;
madjan01 6:9b34fadd6628 102
madjan01 7:3b3a2c158803 103 }*/
madjan01 6:9b34fadd6628 104
madjan01 8:2ae54c6a9dc6 105 /* ------------- start hacking ------------- -------------*/
madjan01 6:9b34fadd6628 106 if(CZustand == 0) // Aufstarten
madjan01 6:9b34fadd6628 107 {
madjan01 6:9b34fadd6628 108 //servo_0.SetPosition(1000);
madjan01 6:9b34fadd6628 109 printf("Grundposition\n");
madjan01 8:2ae54c6a9dc6 110
pmic 1:93d997d6b232 111
madjan01 6:9b34fadd6628 112 thread_sleep_for(100);
madjan01 6:9b34fadd6628 113 CZustand = 1;
madjan01 6:9b34fadd6628 114 }
madjan01 6:9b34fadd6628 115 if(CZustand == 1)// Warten
madjan01 6:9b34fadd6628 116 {
madjan01 6:9b34fadd6628 117 led = 0;
madjan01 8:2ae54c6a9dc6 118 printf("Warten\n");
madjan01 6:9b34fadd6628 119 if(pirrechts == 1)
madjan01 6:9b34fadd6628 120 {
madjan01 6:9b34fadd6628 121 CZustand = 2;
madjan01 8:2ae54c6a9dc6 122 //printf("erkennt\n");
madjan01 8:2ae54c6a9dc6 123 //thread_sleep_for(100);
madjan01 6:9b34fadd6628 124 //servo_0.SetPosition(2000);
pmic 4:67506e285ad0 125
madjan01 6:9b34fadd6628 126 }
madjan01 6:9b34fadd6628 127 if(pirvorne == 1)
madjan01 6:9b34fadd6628 128 {
madjan01 6:9b34fadd6628 129 CZustand = 4;
madjan01 6:9b34fadd6628 130 }
madjan01 6:9b34fadd6628 131 if(pirlinks == 1)
madjan01 6:9b34fadd6628 132 {
madjan01 6:9b34fadd6628 133 CZustand = 3;
madjan01 6:9b34fadd6628 134 }
madjan01 6:9b34fadd6628 135 if(pirhinten == 1)
madjan01 6:9b34fadd6628 136 {
madjan01 8:2ae54c6a9dc6 137 //printf("pirhinten\n");
madjan01 6:9b34fadd6628 138 CZustand = 5;
madjan01 6:9b34fadd6628 139 }
madjan01 6:9b34fadd6628 140
madjan01 6:9b34fadd6628 141
madjan01 6:9b34fadd6628 142
madjan01 6:9b34fadd6628 143
madjan01 6:9b34fadd6628 144
madjan01 6:9b34fadd6628 145 }
madjan01 6:9b34fadd6628 146 else{
madjan01 6:9b34fadd6628 147
madjan01 6:9b34fadd6628 148 }
madjan01 6:9b34fadd6628 149
madjan01 6:9b34fadd6628 150 if(CZustand == 2)//rechts erkennt
madjan01 6:9b34fadd6628 151 {
madjan01 6:9b34fadd6628 152 led = 1;
madjan01 6:9b34fadd6628 153 printf("rechts\n");
madjan01 6:9b34fadd6628 154
madjan01 8:2ae54c6a9dc6 155 servo_0.SetPosition(1900); //1250
madjan01 8:2ae54c6a9dc6 156 if((30 < ultrarechts.read_cm()) & (ultrarechts.read_cm() < 200))
madjan01 6:9b34fadd6628 157 {
madjan01 8:2ae54c6a9dc6 158 //fhelp = ultrarechts.read_cm();
madjan01 8:2ae54c6a9dc6 159 //printf("%3.3f;",fhelp);
madjan01 8:2ae54c6a9dc6 160 CZustand = 6;
madjan01 8:2ae54c6a9dc6 161 }
madjan01 8:2ae54c6a9dc6 162 else{
madjan01 8:2ae54c6a9dc6 163 CZustand = 0;
madjan01 6:9b34fadd6628 164
madjan01 8:2ae54c6a9dc6 165 }
pmic 1:93d997d6b232 166
madjan01 6:9b34fadd6628 167
madjan01 6:9b34fadd6628 168 thread_sleep_for(100);
madjan01 6:9b34fadd6628 169 }
madjan01 8:2ae54c6a9dc6 170
madjan01 8:2ae54c6a9dc6 171
madjan01 6:9b34fadd6628 172 if(CZustand == 3)//links erkennt
madjan01 6:9b34fadd6628 173 {
madjan01 6:9b34fadd6628 174 printf("links\n");
madjan01 8:2ae54c6a9dc6 175 servo_0.SetPosition(900);
madjan01 8:2ae54c6a9dc6 176 if((30 < ultralinks.read_cm()) & (ultralinks.read_cm() < 200))
madjan01 6:9b34fadd6628 177 {
madjan01 8:2ae54c6a9dc6 178 //fhelp = ultralinks.read_cm();
madjan01 8:2ae54c6a9dc6 179 //thread_sleep_for(10);
madjan01 8:2ae54c6a9dc6 180 //printf("%3.3f;\r\n",fhelp);
madjan01 8:2ae54c6a9dc6 181
madjan01 8:2ae54c6a9dc6 182 CZustand = 6;
madjan01 6:9b34fadd6628 183 }
madjan01 6:9b34fadd6628 184 else{
madjan01 8:2ae54c6a9dc6 185 CZustand = 0;
madjan01 8:2ae54c6a9dc6 186 //thread_sleep_for(100);
madjan01 6:9b34fadd6628 187 }
madjan01 6:9b34fadd6628 188
madjan01 6:9b34fadd6628 189
madjan01 6:9b34fadd6628 190 thread_sleep_for(100);
madjan01 6:9b34fadd6628 191 }
madjan01 8:2ae54c6a9dc6 192
madjan01 8:2ae54c6a9dc6 193
madjan01 6:9b34fadd6628 194 if(CZustand == 4)//vorne erkennt
madjan01 6:9b34fadd6628 195 {
madjan01 8:2ae54c6a9dc6 196 printf("vorne\n");
madjan01 8:2ae54c6a9dc6 197 servo_0.SetPosition(1400);
madjan01 8:2ae54c6a9dc6 198 if((30 < ultravorne.read_cm()) & (ultravorne.read_cm() < 200))
madjan01 6:9b34fadd6628 199 {
madjan01 8:2ae54c6a9dc6 200 CZustand = 6;
madjan01 6:9b34fadd6628 201 }
madjan01 6:9b34fadd6628 202 else{
madjan01 8:2ae54c6a9dc6 203 CZustand = 0;
madjan01 8:2ae54c6a9dc6 204 //thread_sleep_for(100);
madjan01 6:9b34fadd6628 205 }
madjan01 6:9b34fadd6628 206
madjan01 6:9b34fadd6628 207
madjan01 6:9b34fadd6628 208 thread_sleep_for(100);
madjan01 6:9b34fadd6628 209 }
pmic 1:93d997d6b232 210
madjan01 6:9b34fadd6628 211 if(CZustand == 5)//hinten erkennt
madjan01 6:9b34fadd6628 212 {
madjan01 6:9b34fadd6628 213 printf("hinten\n");
madjan01 8:2ae54c6a9dc6 214 servo_0.SetPosition(2400);
madjan01 8:2ae54c6a9dc6 215 if((30 < ultrahinten.read_cm()) & (ultrahinten.read_cm() < 200))
madjan01 6:9b34fadd6628 216 {
madjan01 8:2ae54c6a9dc6 217 CZustand = 6;
madjan01 6:9b34fadd6628 218 }
madjan01 6:9b34fadd6628 219 else{
madjan01 8:2ae54c6a9dc6 220 CZustand = 0;
madjan01 6:9b34fadd6628 221
madjan01 6:9b34fadd6628 222 }
madjan01 6:9b34fadd6628 223
madjan01 6:9b34fadd6628 224 thread_sleep_for(100);
pmic 1:93d997d6b232 225 }
madjan01 6:9b34fadd6628 226 if(CZustand == 6)//spritzen
madjan01 6:9b34fadd6628 227 {
madjan01 8:2ae54c6a9dc6 228 printf("spritzen\n");
madjan01 6:9b34fadd6628 229 summer = 1;
madjan01 8:2ae54c6a9dc6 230 thread_sleep_for(spritz);
madjan01 6:9b34fadd6628 231 pumpe = 1;
madjan01 6:9b34fadd6628 232 thread_sleep_for(spritz); // Spritzzeit
madjan01 8:2ae54c6a9dc6 233 pumpe = 0;
madjan01 6:9b34fadd6628 234 summer = 0;
madjan01 6:9b34fadd6628 235 CZustand = 0;
madjan01 8:2ae54c6a9dc6 236 servo_0.SetPosition(1400);
madjan01 6:9b34fadd6628 237
madjan01 6:9b34fadd6628 238
madjan01 6:9b34fadd6628 239 }
madjan01 6:9b34fadd6628 240
madjan01 6:9b34fadd6628 241 /* ------------- stop hacking ------------- -------------*/
madjan01 6:9b34fadd6628 242
madjan01 6:9b34fadd6628 243 int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
madjan01 6:9b34fadd6628 244 int dT_loop_ms = Ts_ms - T_loop_ms;
madjan01 6:9b34fadd6628 245 thread_sleep_for(dT_loop_ms);
madjan01 6:9b34fadd6628 246
madjan01 6:9b34fadd6628 247 }
madjan01 6:9b34fadd6628 248
madjan01 6:9b34fadd6628 249
madjan01 6:9b34fadd6628 250
madjan01 6:9b34fadd6628 251
pmic 1:93d997d6b232 252 }
madjan01 6:9b34fadd6628 253
madjan01 6:9b34fadd6628 254