---
Dependencies: mbed
Fork of MicroMouse_MASTER_FIVE by
Diff: Turn.cpp
- Revision:
- 8:1c8a747c49c8
- Parent:
- 7:5ef09519a6e9
- Child:
- 9:ab19796bf14a
diff -r 5ef09519a6e9 -r 1c8a747c49c8 Turn.cpp --- a/Turn.cpp Mon May 07 18:52:04 2018 +0000 +++ b/Turn.cpp Wed May 16 12:15:23 2018 +0000 @@ -1,19 +1,21 @@ #include <cmath> #include "Turn.h" - +//ausr startx und starty kann aus turn.cpp/turn.h entfernt werden using namespace std; 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, int& dontStop): +Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path): counterLeft(counterLeft), counterRight(counterRight), controller(controller), wallRight(wallRight), wallFront(wallFront), wallLeft(wallLeft), - dontStop(dontStop) + dontStop(dontStop), + path(path), + modeStart(modeStart) {} Turn::~Turn() {} @@ -35,7 +37,7 @@ //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind - if (wallLeft == 0){ //Nach Links Drehen + if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen //printf("Links ist frei\n"); @@ -50,15 +52,19 @@ controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); + + dontStop = 1; - }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus + }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus //printf("Vorne ist frei\n"); dontStop = 2; - }else if (wallRight == 0) { //Nach Rechts Drehen + + + }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen //printf("Rechts ist frei\n"); @@ -73,10 +79,12 @@ controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); + + dontStop = 3; - }else{ //Alle Wege versperrt-> Wenden + }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden //printf("Alles versperrt...zurueck\n"); @@ -91,6 +99,8 @@ controller.setDesiredSpeedRight(0.5f); //0.0f controller.setDesiredSpeedLeft(-0.5f); //0.0f + + dontStop = 4; } }