Jan Mader / Mbed OS PM2_Distanzhelm

Distanzhelm

Dependencies:   PM2_Libary

main.cpp@6:9b34fadd6628, 2021-04-26 (annotated)

Committer:
madjan01
Date:
Mon Apr 26 08:33:06 2021 +0000
Revision:
6:9b34fadd6628
Parent:
4:67506e285ad0
Child:
7:3b3a2c158803
Erster Entwurft Baud 9600

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