![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
Diff: Turn.cpp
- Revision:
- 7:5ef09519a6e9
- Parent:
- 6:a09d2ee3b82e
- Child:
- 8:1c8a747c49c8
--- a/Turn.cpp Wed Apr 25 12:49:48 2018 +0000 +++ b/Turn.cpp Mon May 07 18:52:04 2018 +0000 @@ -3,16 +3,17 @@ using namespace std; -const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm] -const int Turn::TURNINGCOUNTS = 868; //Entspricht Drehung um 90Grad //DONT TOUCH +const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm] +const int Turn::TURNINGCOUNTS = 950; //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070 -Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft): +Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop): counterLeft(counterLeft), counterRight(counterRight), controller(controller), wallRight(wallRight), wallFront(wallFront), - wallLeft(wallLeft) + wallLeft(wallLeft), + dontStop(dontStop) {} Turn::~Turn() {} @@ -25,6 +26,13 @@ int countsLeft = counterLeft.read(); int countsLeft0 = countsLeft; + //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 (wallLeft == 0){ //Nach Links Drehen @@ -40,12 +48,16 @@ //printf("%d\n\r", countsLeft); } controller.setDesiredSpeedRight(0.5f); - controller.setDesiredSpeedLeft(0.5f); + controller.setDesiredSpeedLeft(-0.5f); + + dontStop = 1; }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus //printf("Vorne ist frei\n"); + dontStop = 2; + }else if (wallRight == 0) { //Nach Rechts Drehen //printf("Rechts ist frei\n"); @@ -59,14 +71,16 @@ //printf("%d\n", countsLeft); } controller.setDesiredSpeedRight(0.5f); - controller.setDesiredSpeedLeft(0.5f); + controller.setDesiredSpeedLeft(-0.5f); + + dontStop = 3; }else{ //Alle Wege versperrt-> Wenden //printf("Alles versperrt...zurueck\n"); - while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){ + while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){ controller.setDesiredSpeedRight(TURNINGSPEED); controller.setDesiredSpeedLeft(TURNINGSPEED); countsRight = counterRight.read(); @@ -75,6 +89,8 @@ //printf("%d\n", countsLeft); } controller.setDesiredSpeedRight(0.5f); //0.0f - controller.setDesiredSpeedLeft(0.5f); //0.0f + controller.setDesiredSpeedLeft(-0.5f); //0.0f + + dontStop = 4; } }