---
Dependencies: mbed
Fork of MicroMouse_MASTER_FIVE by
Diff: Turn.cpp
- Revision:
- 9:ab19796bf14a
- Parent:
- 8:1c8a747c49c8
diff -r 1c8a747c49c8 -r ab19796bf14a Turn.cpp --- a/Turn.cpp Wed May 16 12:15:23 2018 +0000 +++ b/Turn.cpp Wed May 16 16:41:44 2018 +0000 @@ -14,8 +14,9 @@ wallFront(wallFront), wallLeft(wallLeft), dontStop(dontStop), - path(path), - modeStart(modeStart) + modeStart(modeStart), + path(path) + {} Turn::~Turn() {} @@ -28,79 +29,51 @@ int countsLeft = counterLeft.read(); int countsLeft0 = countsLeft; - //vor dem abbiegen halten + //Vor dem abbiegen halten + controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); - //wait(0.1f); - - - - //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen - - //printf("Links ist frei\n"); - while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){ controller.setDesiredSpeedRight(TURNINGSPEED); controller.setDesiredSpeedLeft(TURNINGSPEED); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - //printf("%d\n\r", countsRight); - //printf("%d\n\r", countsLeft); } + controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); - - dontStop = 1; }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus - //printf("Vorne ist frei\n"); - dontStop = 2; - - }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen - //printf("Rechts ist frei\n"); - while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){ controller.setDesiredSpeedRight(-TURNINGSPEED); controller.setDesiredSpeedLeft(-TURNINGSPEED); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - //printf("%d\n", countsRight); - //printf("%d\n", countsLeft); } controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); - - - dontStop = 3; }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden - - //printf("Alles versperrt...zurueck\n"); - while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){ controller.setDesiredSpeedRight(TURNINGSPEED); controller.setDesiredSpeedLeft(TURNINGSPEED); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - //printf("%d\n", countsRight); - //printf("%d\n", countsLeft); } controller.setDesiredSpeedRight(0.5f); //0.0f controller.setDesiredSpeedLeft(-0.5f); //0.0f - - dontStop = 4; } }