kings / Mbed 2 deprecated biszueimchlotz

Drehen mit Halt und offset um zum Klotz zurück drehen. (Kann nur ein klotz aufheben)

Dependencies:   Servo mbed

Fork of DrehungMitStopp by kings

main.cpp@4:85b8b4aa97a3, 2017-05-18 (annotated)

Committer:
freunjor
Date:
Thu May 18 13:25:31 2017 +0000
Revision:
4:85b8b4aa97a3
Parent:
3:5483d7c18c34 
fdasdf

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