Gruppe 3
/
PES4
xx
Fork of PES1 by
Diff: main.cpp
- Revision:
- 18:6547e54ac803
- Parent:
- 17:caf5ae550f2e
diff -r caf5ae550f2e -r 6547e54ac803 main.cpp --- a/main.cpp Tue Apr 25 12:26:04 2017 +0000 +++ b/main.cpp Tue May 09 13:21:49 2017 +0000 @@ -1,7 +1,11 @@ #include <mbed.h> #include "Roboter.h" -DigitalOut led(LED1); + +//Periphery for Encoder +EncoderCounter counterLeft(PB_6, PB_7); +EncoderCounter counterRight(PA_6, PC_7); + //Periphery for distance sensors AnalogIn distance(PB_1); @@ -12,11 +16,10 @@ IRSensor sensors[6]; // Periphery for servos -Servo servos1(PB_7); -Servo servos2(PA_6); +Servo servo1(PB_13); //Greiferservo Anschluss D2 +Servo servo2(PA_10); //Armservo Anschluss D0 //Periphery for Switch ON/OFF - DigitalIn switchOnOff(USER_BUTTON); //motor stuff @@ -27,124 +30,74 @@ //indicator leds arround robot DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; -Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2); +// Erstellen eines Roboterobjekts +Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servo1, &servo2, &counterLeft, &counterRight, &enableMotorDriver); int main() { - enable = 1; // Sensoren einschalten - enableMotorDriver = 1; // Schaltet die Motoren ein - pwmL.period(0.00005f); // Setzt die Periode auf 50 μs - pwmR.period(0.00005f); - pwmL = 0.5f; - pwmR = 0.5f; - - - int state = 0; // Diese Variable gibt an in welchem State man sich befindet - int tempState = 2; - int time1 = 0; - int time2 = 0; + int state = 0; // Diese Variable gibt an in welchem State man sich befindet + int tempState = 2; // Wird benötig um zwischen Kameraauswertung und fahren zu wechseln + enable = 1; // Sensoren einschalten + float camValue; + servo1 = 0.0f; // Startpos. Greifer + servo2 = 0.0f; // Startpos. Arm + //wait(1); while(1) { - int camValue = readCamera(); +/* + + //printf("%d\n", state); switch(state) { case 0: // Roboter Anschalten mit Knopf printf("0"); - if (switchOnOff == 0) { - state = 1; + if (switchOnOff == 0) { // Bei Betätigung des Userbuttons wird das Programm gestartet + state = 5; } break; case 1: - printf("1"); - if(camValue == 1 || camValue == 2 || camValue == 3) { // Die Kamera erkennt ein grünen Klotz - state = 4; - } - if(camValue == 4) { // Roboter in Position - state = 5; - } - if(camValue == 0){ - state = tempState; - } + //printf("1"); + camValue = readCamera(); + state = roboter1.stateAuswahl(camValue, tempState); // Case-Auswahl aufgrund von der Kamera break; case 2: - printf("2"); - if(time1 < 20) { // Im Kreis drehen für 1s - pwmL = 0.4f; - pwmR = 0.4f; - time1 ++; - state = 1; - tempState = 2; - } else { - time1 = 0; - pwmL = 0.5f; - pwmR = 0.5f; - state = 3; - } + //printf("2"); + state = roboter1.kreisFahren(); // Im Kreis fahren + tempState = 2; break; case 3: - printf("3"); - roboter1.bandeAusweichen(); - if(time2 < 40) { // gerade aus fahren - pwmL = 0.6f; - pwmR = 0.4f; - time2 ++; - state = 1; - tempState = 3; - } else { - time2 = 0; - pwmL = 0.5f; - pwmR = 0.5f; - state = 2; - } + //printf("3"); + roboter1.bandeAusweichen(); // Hindernissen ausweichen + state = roboter1.geradeFahren(); // Gerade Fahren + tempState = 3; break; - case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren - printf("4"); - roboter1.bandeAusweichen(); - if(camValue == 1) { // links fahren - pwmL = 0.45f; - pwmR = 0.45f; - } - if(camValue == 2) { // rechts fahren - pwmL = 0.55f; - pwmR = 0.55f; - } - if(camValue == 3) { // gerade fahren - pwmL = 0.55f; - pwmR = 0.45f; - } + case 4: + //printf("4"); + roboter1.bandeAusweichen(); // Hindernissen ausweichen + roboter1.anfahren(camValue); // Roboter erkennt gründer Klotz, Klotz wird angefahren state = 1; - break; - case 5: // Aufnehmen des Klotzes - printf("5"); - pwmL = 0.5f; - pwmR = 0.5f; - state = 1; - time1 = 0; - time2 = 0; - + case 5: + //printf("5"); + state = roboter1.pickup(); // Aufnehmen des Klotzes break; - default: break; - } - - //wait(0.1f); + */ + wait(0.01f); } - } -