Gruppe 3
/
PES4
xx
Fork of PES1 by
Diff: main.cpp
- Revision:
- 9:d9e46f9c9e40
- Parent:
- 8:6d9cd5ad332d
- Child:
- 12:b9faf8637183
diff -r 6d9cd5ad332d -r d9e46f9c9e40 main.cpp --- a/main.cpp Tue Apr 18 12:09:46 2017 +0000 +++ b/main.cpp Wed Apr 19 10:07:55 2017 +0000 @@ -40,78 +40,96 @@ pwmL.period(0.00005f); // Setzt die Periode auf 50 μs pwmR.period(0.00005f); pwmL = 0.5f; - pwmR = 0.5f; - + pwmR = 0.5f; + int state = 1; // Diese Variable gibt an in welchem State man sich befindet - /*if ( kamera() == 1) { - state = 4; - } // Die Kamera erkennt ein grünen Klotz*/ - while(1) { + + while(1) { + + if (kamera() == 1 || kamera() == 2 || kamera() == 3) { // Die Kamera erkennt ein grünen Klotz + state = 4; + } + if(kamera() == 4) { // Roboter in Position + state = 5; + } + switch(state) { - case 1: // Roboter Anschalten mit Knopf + case 1: // Roboter Anschalten mit Knopf - if (switchOnOff== 0){ - state = 5; - } + if (switchOnOff== 0) { + state = 2; + } - break; + break; - case 2: - roboter1.bandeAusweichen(); - static int time1 = 0; - if(time1 < 10) { // Im Kreis drehen für 1s - pwmL = 0.4f; - pwmR = 0.4f; - time1 ++; - } else { - time1 = 0; - pwmL = 0.5f; - pwmR = 0.5f; - state = 3; - } - break; + case 2: + static int time1 = 0; + if(time1 < 20) { // Im Kreis drehen für 1s + pwmL = 0.4f; + pwmR = 0.4f; + time1 ++; + } else { + time1 = 0; + pwmL = 0.5f; + pwmR = 0.5f; + state = 3; + } + break; - case 3: - roboter1.bandeAusweichen(); - static int time2 = 0; - if(time2 < 10) { // gerade aus fahren - pwmL = 0.6f; - pwmR = 0.4f; - time2 ++; - } else { - time2 = 0; - pwmL = 0.5f; - pwmR = 0.5f; - state = 2; - } - break; + case 3: + roboter1.bandeAusweichen(); + static int time2 = 0; + if(time2 < 40) { // gerade aus fahren + pwmL = 0.6f; + pwmR = 0.4f; + time2 ++; + } else { + time2 = 0; + pwmL = 0.5f; + pwmR = 0.5f; + state = 2; + } + break; - case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren - - - break; - - case 5: // Aufnehmen des Klotzes + case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren + roboter1.bandeAusweichen(); + if(kamera() == 1) { // links fahren + pwmL = 0.45f; + pwmR = 0.45f; + } + if(kamera() == 2) { // rechts fahren + pwmL = 0.55f; + pwmR = 0.55f; + } + if(kamera() == 3) { // gerade fahren + pwmL = 0.55f; + pwmR = 0.45f; + } - roboter1.bandeAusweichen(); - - break; - - - default: break; + break; + + case 5: // Aufnehmen des Klotzes + + - - } + break; + - wait(0.1f); + default: + break; + } + wait(0.1f); + } +} +