kings / Mbed 2 deprecated DrehungMitStopp

sef

Dependencies:   mbed

Fork of Bewegungen_mit_Sensor by kings

main.cpp@2:365bf16abbf6, 2017-05-16 (annotated)

Committer:
EHess
Date:
Tue May 16 14:14:08 2017 +0000
Revision:
2:365bf16abbf6
Parent:
1:d40ff07e2fe0 
zdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EHess 0:96f88638114b 1 #include "mbed.h"
EHess 0:96f88638114b 2 #include "IRSensor.h"
EHess 1:d40ff07e2fe0 3 #include "MotorEncoder.h"
EHess 1:d40ff07e2fe0 4 #include "LowpassFilter.h"
EHess 2:365bf16abbf6 5 #include <cmath>
EHess 0:96f88638114b 6
EHess 1:d40ff07e2fe0 7 DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer
EHess 0:96f88638114b 8
EHess 1:d40ff07e2fe0 9 AnalogIn distance(PB_1); //Input der Distanzsensoren
EHess 1:d40ff07e2fe0 10 DigitalOut enableSensor(PC_1); //Aktivierung der IRSensoren
EHess 1:d40ff07e2fe0 11 DigitalOut bit0(PH_1); //Ansteuerung der Sensoren 0-5 mit 3 Bits
EHess 0:96f88638114b 12 DigitalOut bit1(PC_2);
EHess 0:96f88638114b 13 DigitalOut bit2(PC_3);
EHess 1:d40ff07e2fe0 14 IRSensor sensors[6]; //Erstellt 6 IRSensor-Objekte als Array
EHess 0:96f88638114b 15
EHess 1:d40ff07e2fe0 16 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //LED-Outputs der Sensoren
EHess 0:96f88638114b 17
EHess 2:365bf16abbf6 18 float sensorMittelwert[6]; //Array der 6 Sensorenwerte
EHess 2:365bf16abbf6 19 float sensorTiefbass[6];
EHess 2:365bf16abbf6 20 float sensorZentimeter[6];
EHess 2:365bf16abbf6 21 int printfZaehler = 0;
EHess 0:96f88638114b 22
EHess 1:d40ff07e2fe0 23 //Titel printf()
EHess 2:365bf16abbf6 24 void title()
EHess 2:365bf16abbf6 25 {
EHess 2:365bf16abbf6 26 printf("\f"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
EHess 2:365bf16abbf6 27 }
EHess 2:365bf16abbf6 28
EHess 2:365bf16abbf6 29 void sensorWerte() //Rechnet Sensorwerte aus und speichert sie unter sensorZentimeter[]
EHess 2:365bf16abbf6 30 {
EHess 2:365bf16abbf6 31 for(int j = 0; j < 25; j++) { //Zählt 25 Sensorwerten pro Sensor zusammen
EHess 2:365bf16abbf6 32 for(int i = 0; i < 6; i++) {
EHess 2:365bf16abbf6 33 sensorMittelwert[i] += sensors[i].read();
EHess 2:365bf16abbf6 34 }
EHess 2:365bf16abbf6 35 //wait( 0.001f );
EHess 2:365bf16abbf6 36 }
EHess 2:365bf16abbf6 37 for(int i = 0; i < 6; i++) {
EHess 2:365bf16abbf6 38 sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f; //Verrechnet den neuen Wert mit dem alten
EHess 2:365bf16abbf6 39 sensorMittelwert[i] = 0.0f; //Setzt die Sensorwerte auf NULL
EHess 2:365bf16abbf6 40 switch(i) { //Rechnet die Werte in Zentimeter
EHess 2:365bf16abbf6 41 case 0: //Mitte
EHess 2:365bf16abbf6 42 if(sensorTiefbass[i] < 2.97f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 43 else sensorZentimeter[i] = 110.0f/pow((sensorTiefbass[i]-1.9f),0.41f)-27.0f;
EHess 2:365bf16abbf6 44 break;
EHess 2:365bf16abbf6 45 case 1: //UntenRechts
EHess 2:365bf16abbf6 46 if(sensorTiefbass[i] < 3.2f) sensorZentimeter[i] = 69.0f; //Grenzwert
EHess 2:365bf16abbf6 47 else sensorZentimeter[i] = 87.0f/pow((sensorTiefbass[i]-2.2f),0.41f)-18.0f;
EHess 2:365bf16abbf6 48 break;
EHess 2:365bf16abbf6 49 case 3: //Links
EHess 2:365bf16abbf6 50 if(sensorTiefbass[i] < 3.67f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 51 sensorZentimeter[i] = 80.0f/pow((sensorTiefbass[i]-3.1f),0.38f)-19.0f;
EHess 2:365bf16abbf6 52 break;
EHess 2:365bf16abbf6 53 case 4: //Rechts
EHess 2:365bf16abbf6 54 if(sensorTiefbass[i] < 3.53f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 55 else sensorZentimeter[i] = 90.0f/pow((sensorTiefbass[i]-2.8f),0.4f)-22.0f;
EHess 2:365bf16abbf6 56 break;
EHess 2:365bf16abbf6 57 case 5: //UntenLinks
EHess 2:365bf16abbf6 58 if(sensorTiefbass[i] < 4.4f) sensorZentimeter[i] = 59.0f; //Grenzwert
EHess 2:365bf16abbf6 59 else sensorZentimeter[i] = 115.0f/pow((sensorTiefbass[i]-2.5f),0.4f)-30.0f;
EHess 2:365bf16abbf6 60 break;
EHess 2:365bf16abbf6 61 }
EHess 2:365bf16abbf6 62 }
EHess 2:365bf16abbf6 63
EHess 2:365bf16abbf6 64 printf("%f\t%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0], sensorZentimeter[1]); //Plottet die unteren Sensoren + Mitte-Oben
EHess 2:365bf16abbf6 65
EHess 2:365bf16abbf6 66 printfZaehler++;
EHess 2:365bf16abbf6 67 // if(printfZaehler % 120 == 0){
EHess 2:365bf16abbf6 68 // wait(3.5f);
EHess 2:365bf16abbf6 69 // }
EHess 2:365bf16abbf6 70 if(printfZaehler % 40 == 0) title(); //Erstellt nach 40 Zeilen eine neue Seite
EHess 0:96f88638114b 71 }
EHess 0:96f88638114b 72
EHess 1:d40ff07e2fe0 73
EHess 1:d40ff07e2fe0 74 const float PERIOD = 0.002f; // period of control task, given in [s]
EHess 1:d40ff07e2fe0 75 const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
EHess 1:d40ff07e2fe0 76 const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
EHess 1:d40ff07e2fe0 77 const float KN = 40.0f; // speed constant of motor, given in [rpm/V]
EHess 1:d40ff07e2fe0 78 const float KP = 0.2f; // speed controller gain, given in [V/rpm]
EHess 1:d40ff07e2fe0 79 const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
EHess 1:d40ff07e2fe0 80 const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
EHess 1:d40ff07e2fe0 81 const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
EHess 1:d40ff07e2fe0 82
EHess 1:d40ff07e2fe0 83 MotorEncoder counterLeft(PB_6, PB_7);
EHess 1:d40ff07e2fe0 84 MotorEncoder counterRight(PA_6, PC_7);
EHess 1:d40ff07e2fe0 85
EHess 1:d40ff07e2fe0 86 LowpassFilter speedLeftFilter;
EHess 1:d40ff07e2fe0 87 LowpassFilter speedRightFilter;
EHess 1:d40ff07e2fe0 88
EHess 1:d40ff07e2fe0 89 DigitalOut enableMotorDriver(PB_2);
EHess 1:d40ff07e2fe0 90 PwmOut pwmLeft(PA_8);
EHess 1:d40ff07e2fe0 91 PwmOut pwmRight(PA_9);
EHess 1:d40ff07e2fe0 92
EHess 1:d40ff07e2fe0 93 DigitalOut my_led(LED1);
EHess 1:d40ff07e2fe0 94
EHess 1:d40ff07e2fe0 95 short previousValueCounterRight = 0;
EHess 1:d40ff07e2fe0 96 short previousValueCounterLeft = 0;
EHess 1:d40ff07e2fe0 97
EHess 1:d40ff07e2fe0 98 float desiredSpeedLeft;
EHess 1:d40ff07e2fe0 99 float desiredSpeedRight;
EHess 0:96f88638114b 100
EHess 1:d40ff07e2fe0 101 float actualSpeedLeft;
EHess 1:d40ff07e2fe0 102 float actualSpeedRight;
EHess 1:d40ff07e2fe0 103
EHess 1:d40ff07e2fe0 104 void speedCtrl()
EHess 1:d40ff07e2fe0 105 {
EHess 1:d40ff07e2fe0 106 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
EHess 1:d40ff07e2fe0 107 short valueCounterLeft = counterLeft.read();
EHess 1:d40ff07e2fe0 108 short valueCounterRight = counterRight.read();
EHess 1:d40ff07e2fe0 109 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
EHess 1:d40ff07e2fe0 110 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
EHess 1:d40ff07e2fe0 111
EHess 1:d40ff07e2fe0 112 previousValueCounterLeft = valueCounterLeft;
EHess 1:d40ff07e2fe0 113 previousValueCounterRight = valueCounterRight;
EHess 1:d40ff07e2fe0 114 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 1:d40ff07e2fe0 115 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 1:d40ff07e2fe0 116
EHess 1:d40ff07e2fe0 117 // Berechnen der Motorspannungen Uout
EHess 1:d40ff07e2fe0 118 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
EHess 1:d40ff07e2fe0 119 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
EHess 1:d40ff07e2fe0 120
EHess 1:d40ff07e2fe0 121 // Berechnen, Limitieren und Setzen der Duty-Cycle
EHess 1:d40ff07e2fe0 122 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
EHess 1:d40ff07e2fe0 123 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 124 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 125
EHess 1:d40ff07e2fe0 126 pwmLeft = dutyCycleLeft;
EHess 1:d40ff07e2fe0 127 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
EHess 1:d40ff07e2fe0 128 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 129 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 130
EHess 1:d40ff07e2fe0 131 pwmRight = dutyCycleRight;
EHess 0:96f88638114b 132 }
EHess 0:96f88638114b 133
EHess 2:365bf16abbf6 134 enum RobotState {
EHess 2:365bf16abbf6 135 START = 0,
EHess 2:365bf16abbf6 136 VORSUCHEN,
EHess 2:365bf16abbf6 137 SUCHEN,
EHess 2:365bf16abbf6 138 LADEN,
EHess 2:365bf16abbf6 139 SONST
EHess 2:365bf16abbf6 140
EHess 2:365bf16abbf6 141 };
EHess 2:365bf16abbf6 142
EHess 2:365bf16abbf6 143
EHess 2:365bf16abbf6 144 int main()
EHess 2:365bf16abbf6 145 {
EHess 2:365bf16abbf6 146
EHess 1:d40ff07e2fe0 147 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
EHess 1:d40ff07e2fe0 148 pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
EHess 1:d40ff07e2fe0 149 pwmRight.period(0.00005f);
EHess 1:d40ff07e2fe0 150 pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
EHess 1:d40ff07e2fe0 151 pwmRight = 0.5f; // Duty-Cycle von 50%
EHess 1:d40ff07e2fe0 152
EHess 1:d40ff07e2fe0 153 // Initialisieren von lokalen Variabeln
EHess 1:d40ff07e2fe0 154 previousValueCounterLeft = counterLeft.read();
EHess 1:d40ff07e2fe0 155 previousValueCounterRight = counterRight.read();
EHess 1:d40ff07e2fe0 156 speedLeftFilter.setPeriod(PERIOD);
EHess 1:d40ff07e2fe0 157 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 1:d40ff07e2fe0 158 speedRightFilter.setPeriod(PERIOD);
EHess 1:d40ff07e2fe0 159 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 0:96f88638114b 160
EHess 1:d40ff07e2fe0 161 desiredSpeedLeft = 0.0f;
EHess 1:d40ff07e2fe0 162 desiredSpeedRight = 0.0f;
EHess 1:d40ff07e2fe0 163 actualSpeedLeft = 0.0f;
EHess 1:d40ff07e2fe0 164 actualSpeedRight = 0.0f;
EHess 1:d40ff07e2fe0 165
EHess 1:d40ff07e2fe0 166 Ticker t1;
EHess 1:d40ff07e2fe0 167 t1.attach( &speedCtrl, PERIOD);
EHess 0:96f88638114b 168
EHess 2:365bf16abbf6 169 int status = 0; //status definiert aktuellen Programmcode (0: Startprogramm, 1: Suchen, 2: Ausrichten, 3: Anfahren, 4: Aufnehmen)
EHess 2:365bf16abbf6 170
EHess 2:365bf16abbf6 171 //SensorArrays
EHess 2:365bf16abbf6 172 int z = 0; //Zähler
EHess 2:365bf16abbf6 173 float sensorLinks[100];
EHess 2:365bf16abbf6 174 float sensorOben[100];
EHess 2:365bf16abbf6 175 float sensorRechts[100];
EHess 2:365bf16abbf6 176
EHess 1:d40ff07e2fe0 177 enableMotorDriver = 1;
EHess 0:96f88638114b 178
EHess 1:d40ff07e2fe0 179 //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
EHess 1:d40ff07e2fe0 180 for( int i = 0; i < 6; i++) {
EHess 1:d40ff07e2fe0 181 sensors[i].init(&distance, &bit0, &bit1, &bit2, i);
EHess 1:d40ff07e2fe0 182 sensorMittelwert[i] = 0.0f;
EHess 1:d40ff07e2fe0 183 sensorTiefbass[i] = 0.0f;
EHess 1:d40ff07e2fe0 184 }
EHess 1:d40ff07e2fe0 185 enableSensor = 1; //Aktiviert die IRSensoren
EHess 0:96f88638114b 186
EHess 2:365bf16abbf6 187 status = START;
EHess 2:365bf16abbf6 188
EHess 2:365bf16abbf6 189 int timer = 0;
EHess 2:365bf16abbf6 190 my_led = 0;
EHess 2:365bf16abbf6 191
EHess 0:96f88638114b 192 while(1) {
EHess 2:365bf16abbf6 193 //lese sensorwerte, tiefpass initialisierung
EHess 2:365bf16abbf6 194 sensorWerte();
EHess 2:365bf16abbf6 195 ++timer;
EHess 2:365bf16abbf6 196 wait(0.025f);
EHess 2:365bf16abbf6 197
EHess 2:365bf16abbf6 198 //DISPLAY STATE
EHess 2:365bf16abbf6 199
EHess 2:365bf16abbf6 200 //while(status == 1){
EHess 2:365bf16abbf6 201 switch(status) {
EHess 2:365bf16abbf6 202
EHess 2:365bf16abbf6 203 //start sequenz
EHess 2:365bf16abbf6 204 case START:
EHess 2:365bf16abbf6 205 // desiredSpeedLeft = 15.0f; //Drehung
EHess 2:365bf16abbf6 206 // desiredSpeedRight = 15.0f;
EHess 2:365bf16abbf6 207
EHess 2:365bf16abbf6 208 //roboter dreh links
EHess 2:365bf16abbf6 209 if( timer > 40 )
EHess 2:365bf16abbf6 210 status = 1;
EHess 2:365bf16abbf6 211 break;
EHess 2:365bf16abbf6 212
EHess 2:365bf16abbf6 213 case VORSUCHEN: {
EHess 2:365bf16abbf6 214 my_led = 1;
EHess 2:365bf16abbf6 215 desiredSpeedLeft = 15.0f; //Drehung
EHess 2:365bf16abbf6 216 desiredSpeedRight = 15.0f;
EHess 2:365bf16abbf6 217 if(z<12) { //Schreibt die Sensorwerte in ein Array
EHess 2:365bf16abbf6 218 sensorOben[z] = sensorZentimeter[0];
EHess 2:365bf16abbf6 219 sensorLinks[z] = sensorZentimeter[5];
EHess 2:365bf16abbf6 220 sensorRechts[z] = sensorZentimeter[1];
EHess 2:365bf16abbf6 221 z++;
EHess 2:365bf16abbf6 222 } else status = SUCHEN;
EHess 2:365bf16abbf6 223 break;
EHess 1:d40ff07e2fe0 224 }
EHess 2:365bf16abbf6 225 case SUCHEN: {
EHess 2:365bf16abbf6 226 my_led = 1;
EHess 2:365bf16abbf6 227
EHess 2:365bf16abbf6 228 //for(int i = 0; i < 10; i++)
EHess 2:365bf16abbf6 229 desiredSpeedLeft = 15.0f; //Drehung
EHess 2:365bf16abbf6 230 desiredSpeedRight = 15.0f;
EHess 2:365bf16abbf6 231
EHess 2:365bf16abbf6 232 if(z < 100) {
EHess 2:365bf16abbf6 233 sensorOben[z] = sensorZentimeter[0];
EHess 2:365bf16abbf6 234 sensorLinks[z] = sensorZentimeter[5];
EHess 2:365bf16abbf6 235 sensorRechts[z] = sensorZentimeter[1];
EHess 2:365bf16abbf6 236 //Prüft Ausschlag && vergleicht ersten mit letztem Wert && prüft Zwischenwerte
EHess 2:365bf16abbf6 237 if(3.0f * sensorLinks[z] > sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] + 13.5f && sensorLinks[z] + 1 > sensorLinks[z-12] && sensorLinks[z] >= sensorLinks[z-2] && sensorLinks[z] >= sensorLinks[z-3] && sensorLinks[z] >= sensorLinks[z-4] && sensorLinks[z-12] >= sensorLinks[z-10] && sensorLinks[z-12] >= sensorLinks[z-9] && sensorLinks[z-12] >= sensorLinks[z-8]) {
EHess 2:365bf16abbf6 238 //Prüft Hindernis
EHess 2:365bf16abbf6 239 if(sensorLinks[z-5] + 3.5f < sensorOben[z-5] && sensorLinks[z-6] + 3.5f < sensorOben[z-6] && sensorLinks[z-7] + 3.5f < sensorOben[z-7]){
EHess 2:365bf16abbf6 240 status = 3;
EHess 2:365bf16abbf6 241 z=0;
EHess 2:365bf16abbf6 242 }
EHess 2:365bf16abbf6 243 }
EHess 2:365bf16abbf6 244 z++;
EHess 2:365bf16abbf6 245 }
EHess 2:365bf16abbf6 246 else{
EHess 2:365bf16abbf6 247 z = 0;
EHess 2:365bf16abbf6 248 status = 4;
EHess 2:365bf16abbf6 249 }
EHess 2:365bf16abbf6 250
EHess 2:365bf16abbf6 251 break;
EHess 2:365bf16abbf6 252 }
EHess 2:365bf16abbf6 253 case 3:
EHess 2:365bf16abbf6 254
EHess 2:365bf16abbf6 255 desiredSpeedLeft = -20.0f; //Stillstand
EHess 2:365bf16abbf6 256 desiredSpeedRight = 15.0f;
EHess 2:365bf16abbf6 257 wait(2.0f);
EHess 2:365bf16abbf6 258 status = VORSUCHEN;
EHess 2:365bf16abbf6 259 break;
EHess 2:365bf16abbf6 260 case 4:
EHess 2:365bf16abbf6 261
EHess 2:365bf16abbf6 262 desiredSpeedLeft = 15.0f; //Stillstand
EHess 2:365bf16abbf6 263 desiredSpeedRight = -20.0f;
EHess 2:365bf16abbf6 264 wait(2.0f);
EHess 2:365bf16abbf6 265 status = VORSUCHEN;
EHess 2:365bf16abbf6 266 break;
EHess 2:365bf16abbf6 267
EHess 1:d40ff07e2fe0 268 }
EHess 0:96f88638114b 269 }
EHess 1:d40ff07e2fe0 270 }