Jan Mader / Mbed OS PM2_Distanzhelm

Distanzhelm

Dependencies:   PM2_Libary

main.cpp@7:3b3a2c158803, 2021-05-12 (annotated)

Committer:
madjan01
Date:
Wed May 12 23:35:22 2021 +0000
Revision:
7:3b3a2c158803
Parent:
6:9b34fadd6628
Child:
8:2ae54c6a9dc6
mit range finder

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 6:9b34fadd6628 17 Servo servo_0(PA_6);
pmic 1:93d997d6b232 18
pmic 1:93d997d6b232 19
madjan01 7:3b3a2c158803 20 //Infrarotsensor
madjan01 7:3b3a2c158803 21 DigitalIn pirlinks(PB_12);
madjan01 7:3b3a2c158803 22 DigitalIn pirhinten(PC_7); //pc_6 puuty besetzt
madjan01 7:3b3a2c158803 23 DigitalIn pirrechts(PB_7); //pc_8
madjan01 7:3b3a2c158803 24 DigitalIn pirvorne(PA_1); //pc-9
madjan01 7:3b3a2c158803 25 //im moment nich eingebaut
madjan01 7:3b3a2c158803 26 // DigitalIn ultrarechts(PB_1);
madjan01 7:3b3a2c158803 27 // DigitalIn ultralinks(PB_13); //immer eins behoben
madjan01 7:3b3a2c158803 28 // DigitalIn ultravorne(PC_3);
madjan01 7:3b3a2c158803 29 //DigitalIn ultrahinten(PA_9);// immer eins behoben
pmic 1:93d997d6b232 30
madjan01 6:9b34fadd6628 31 //Output
madjan01 6:9b34fadd6628 32 //Servo
madjan01 6:9b34fadd6628 33 DigitalOut pumpe(PB_6);
madjan01 6:9b34fadd6628 34 DigitalOut summer(PA_0);
pmic 2:7c03fefb77ef 35
madjan01 7:3b3a2c158803 36 // Groove Ultrasonic Ranger V2.0
madjan01 7:3b3a2c158803 37 // 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 7:3b3a2c158803 38 /* create range finder object (ultra sonic distance sensor) */
madjan01 7:3b3a2c158803 39 RangeFinder ultrarechts(PB_1, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 7:3b3a2c158803 40 RangeFinder ultralinks(PB_13, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 7:3b3a2c158803 41 RangeFinder ultravorne(PC_3, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 7:3b3a2c158803 42 RangeFinder ultrahinten(PA_9, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
madjan01 7:3b3a2c158803 43
madjan01 7:3b3a2c158803 44
pmic 4:67506e285ad0 45 /* input your stuff here */
madjan01 6:9b34fadd6628 46
madjan01 6:9b34fadd6628 47 Timer loop_timer;
madjan01 6:9b34fadd6628 48 int Ts_ms = 500;
madjan01 7:3b3a2c158803 49 char CZustand = 0;
madjan01 6:9b34fadd6628 50 int spritz = 3000; //spritzzeit
madjan01 7:3b3a2c158803 51 float fhelp = 0.0f;
madjan01 6:9b34fadd6628 52
madjan01 6:9b34fadd6628 53
madjan01 6:9b34fadd6628 54
madjan01 6:9b34fadd6628 55 /*Interrupt*/
madjan01 6:9b34fadd6628 56
madjan01 6:9b34fadd6628 57 /*
madjan01 6:9b34fadd6628 58 class Counter {
madjan01 6:9b34fadd6628 59 public:
madjan01 6:9b34fadd6628 60 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
madjan01 6:9b34fadd6628 61 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
madjan01 6:9b34fadd6628 62 }
madjan01 6:9b34fadd6628 63
madjan01 6:9b34fadd6628 64 void increment() {
madjan01 6:9b34fadd6628 65 _count++;
madjan01 6:9b34fadd6628 66 }
madjan01 6:9b34fadd6628 67
madjan01 6:9b34fadd6628 68 int read() {
madjan01 6:9b34fadd6628 69 return _count;
madjan01 6:9b34fadd6628 70 }
madjan01 6:9b34fadd6628 71
madjan01 6:9b34fadd6628 72 private:
madjan01 6:9b34fadd6628 73 InterruptIn _interrupt;
madjan01 6:9b34fadd6628 74 volatile int _count;
madjan01 6:9b34fadd6628 75 };
madjan01 6:9b34fadd6628 76
madjan01 6:9b34fadd6628 77 */
madjan01 6:9b34fadd6628 78
madjan01 6:9b34fadd6628 79
madjan01 6:9b34fadd6628 80 int main()
madjan01 6:9b34fadd6628 81 {
madjan01 6:9b34fadd6628 82
madjan01 6:9b34fadd6628 83 servo_0.Enable(1500,20000);
madjan01 6:9b34fadd6628 84
pmic 4:67506e285ad0 85
madjan01 6:9b34fadd6628 86
pmic 1:93d997d6b232 87
pmic 1:93d997d6b232 88 while (true) {
madjan01 6:9b34fadd6628 89
madjan01 6:9b34fadd6628 90 loop_timer.reset();
madjan01 7:3b3a2c158803 91 /* if(user_button == 0)
madjan01 6:9b34fadd6628 92 {
madjan01 6:9b34fadd6628 93 printf("nicht aktiv");
madjan01 6:9b34fadd6628 94 CZustand = 0;
madjan01 6:9b34fadd6628 95
madjan01 7:3b3a2c158803 96 }*/
madjan01 6:9b34fadd6628 97
madjan01 6:9b34fadd6628 98 /* ------------- start hacking ------------- -------------*/
madjan01 6:9b34fadd6628 99 if(CZustand == 0) // Aufstarten
madjan01 6:9b34fadd6628 100 {
madjan01 6:9b34fadd6628 101 //servo_0.SetPosition(1000);
madjan01 6:9b34fadd6628 102 printf("Grundposition\n");
pmic 1:93d997d6b232 103
madjan01 6:9b34fadd6628 104 thread_sleep_for(100);
madjan01 6:9b34fadd6628 105 CZustand = 1;
madjan01 6:9b34fadd6628 106 }
madjan01 6:9b34fadd6628 107 if(CZustand == 1)// Warten
madjan01 6:9b34fadd6628 108 {
madjan01 6:9b34fadd6628 109 led = 0;
madjan01 6:9b34fadd6628 110 //printf("Warten\n");
madjan01 6:9b34fadd6628 111 if(pirrechts == 1)
madjan01 6:9b34fadd6628 112 {
madjan01 6:9b34fadd6628 113 CZustand = 2;
madjan01 6:9b34fadd6628 114 printf("erkennt\n");
madjan01 6:9b34fadd6628 115 thread_sleep_for(100);
madjan01 6:9b34fadd6628 116 //servo_0.SetPosition(2000);
pmic 4:67506e285ad0 117
madjan01 6:9b34fadd6628 118 }
madjan01 6:9b34fadd6628 119 if(pirvorne == 1)
madjan01 6:9b34fadd6628 120 {
madjan01 6:9b34fadd6628 121 CZustand = 4;
madjan01 6:9b34fadd6628 122 }
madjan01 6:9b34fadd6628 123 if(pirlinks == 1)
madjan01 6:9b34fadd6628 124 {
madjan01 6:9b34fadd6628 125 CZustand = 3;
madjan01 6:9b34fadd6628 126 }
madjan01 6:9b34fadd6628 127 if(pirhinten == 1)
madjan01 6:9b34fadd6628 128 {
madjan01 6:9b34fadd6628 129 printf("pirhinten\n");
madjan01 6:9b34fadd6628 130 CZustand = 5;
madjan01 6:9b34fadd6628 131 }
madjan01 6:9b34fadd6628 132
madjan01 6:9b34fadd6628 133
madjan01 6:9b34fadd6628 134
madjan01 6:9b34fadd6628 135
madjan01 6:9b34fadd6628 136
madjan01 6:9b34fadd6628 137 }
madjan01 6:9b34fadd6628 138 else{
madjan01 6:9b34fadd6628 139
madjan01 6:9b34fadd6628 140 }
madjan01 6:9b34fadd6628 141
madjan01 6:9b34fadd6628 142 if(CZustand == 2)//rechts erkennt
madjan01 6:9b34fadd6628 143 {
madjan01 6:9b34fadd6628 144 led = 1;
madjan01 6:9b34fadd6628 145 printf("rechts\n");
madjan01 6:9b34fadd6628 146
madjan01 7:3b3a2c158803 147 servo_0.SetPosition(2100); //1250
madjan01 7:3b3a2c158803 148 if(ultrarechts.read_cm() < 200)
madjan01 6:9b34fadd6628 149 {
madjan01 7:3b3a2c158803 150 fhelp = ultrarechts.read_cm();
madjan01 7:3b3a2c158803 151 printf("%f;",fhelp);
madjan01 6:9b34fadd6628 152 CZustand = 6;
madjan01 6:9b34fadd6628 153 }
madjan01 6:9b34fadd6628 154 else{
madjan01 6:9b34fadd6628 155 CZustand = 0;
madjan01 6:9b34fadd6628 156
pmic 1:93d997d6b232 157 }
pmic 1:93d997d6b232 158
madjan01 6:9b34fadd6628 159
madjan01 6:9b34fadd6628 160 thread_sleep_for(100);
madjan01 6:9b34fadd6628 161 }
madjan01 6:9b34fadd6628 162 if(CZustand == 3)//links erkennt
madjan01 6:9b34fadd6628 163 {
madjan01 6:9b34fadd6628 164 printf("links\n");
madjan01 7:3b3a2c158803 165 servo_0.SetPosition(1000);
madjan01 7:3b3a2c158803 166 if(ultralinks.read_cm() < 200)
madjan01 6:9b34fadd6628 167 {
madjan01 7:3b3a2c158803 168 fhelp = ultralinks.read_cm();
madjan01 7:3b3a2c158803 169 printf("%3.3f;\r\n",fhelp);
madjan01 6:9b34fadd6628 170 CZustand = 6;
madjan01 6:9b34fadd6628 171 }
madjan01 6:9b34fadd6628 172 else{
madjan01 6:9b34fadd6628 173 CZustand = 0;
madjan01 6:9b34fadd6628 174 //thread_sleep_for(100);
madjan01 6:9b34fadd6628 175 }
madjan01 6:9b34fadd6628 176
madjan01 6:9b34fadd6628 177
madjan01 6:9b34fadd6628 178 thread_sleep_for(100);
madjan01 6:9b34fadd6628 179 }
madjan01 6:9b34fadd6628 180 if(CZustand == 4)//vorne erkennt
madjan01 6:9b34fadd6628 181 {
madjan01 6:9b34fadd6628 182 printf("vorne\n");
madjan01 6:9b34fadd6628 183 servo_0.SetPosition(1500);
madjan01 7:3b3a2c158803 184 if(ultravorne.read_cm() < 200)
madjan01 6:9b34fadd6628 185 {
madjan01 6:9b34fadd6628 186 CZustand = 6;
madjan01 6:9b34fadd6628 187 }
madjan01 6:9b34fadd6628 188 else{
madjan01 6:9b34fadd6628 189 CZustand = 0;
madjan01 6:9b34fadd6628 190 thread_sleep_for(100);
madjan01 6:9b34fadd6628 191 }
madjan01 6:9b34fadd6628 192
madjan01 6:9b34fadd6628 193
madjan01 6:9b34fadd6628 194 thread_sleep_for(100);
madjan01 6:9b34fadd6628 195 }
pmic 1:93d997d6b232 196
madjan01 6:9b34fadd6628 197 if(CZustand == 5)//hinten erkennt
madjan01 6:9b34fadd6628 198 {
madjan01 6:9b34fadd6628 199 printf("hinten\n");
madjan01 7:3b3a2c158803 200 servo_0.SetPosition(2450);
madjan01 7:3b3a2c158803 201 if(ultrahinten.read_cm() < 200)
madjan01 6:9b34fadd6628 202 {
madjan01 6:9b34fadd6628 203 CZustand = 6;
madjan01 6:9b34fadd6628 204 }
madjan01 6:9b34fadd6628 205 else{
madjan01 6:9b34fadd6628 206 CZustand = 0;
madjan01 6:9b34fadd6628 207
madjan01 6:9b34fadd6628 208 }
madjan01 6:9b34fadd6628 209
madjan01 6:9b34fadd6628 210 thread_sleep_for(100);
pmic 1:93d997d6b232 211 }
madjan01 6:9b34fadd6628 212 if(CZustand == 6)//spritzen
madjan01 6:9b34fadd6628 213 {
madjan01 6:9b34fadd6628 214 printf("spritzen");
madjan01 6:9b34fadd6628 215 summer = 1;
madjan01 6:9b34fadd6628 216 pumpe = 1;
madjan01 6:9b34fadd6628 217 thread_sleep_for(spritz); // Spritzzeit
madjan01 6:9b34fadd6628 218 summer = 0;
madjan01 6:9b34fadd6628 219 pumpe = 0;
madjan01 6:9b34fadd6628 220 CZustand = 0;
madjan01 6:9b34fadd6628 221 servo_0.SetPosition(1500);
madjan01 6:9b34fadd6628 222
madjan01 6:9b34fadd6628 223
madjan01 6:9b34fadd6628 224 }
madjan01 6:9b34fadd6628 225
madjan01 6:9b34fadd6628 226 /* ------------- stop hacking ------------- -------------*/
madjan01 6:9b34fadd6628 227
madjan01 6:9b34fadd6628 228 int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
madjan01 6:9b34fadd6628 229 int dT_loop_ms = Ts_ms - T_loop_ms;
madjan01 6:9b34fadd6628 230 thread_sleep_for(dT_loop_ms);
madjan01 6:9b34fadd6628 231
madjan01 6:9b34fadd6628 232 }
madjan01 6:9b34fadd6628 233
madjan01 6:9b34fadd6628 234
madjan01 6:9b34fadd6628 235
madjan01 6:9b34fadd6628 236
pmic 1:93d997d6b232 237 }
madjan01 6:9b34fadd6628 238
madjan01 6:9b34fadd6628 239