Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Turn.cpp
- Committer:
- ruesipat
- Date:
- 2018-04-25
- Revision:
- 6:a09d2ee3b82e
- Parent:
- 5:b8b1a979b0d5
- Child:
- 7:5ef09519a6e9
File content as of revision 6:a09d2ee3b82e:
#include <cmath> #include "Turn.h" using namespace std; const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm] const int Turn::TURNINGCOUNTS = 868; //Entspricht Drehung um 90Grad //DONT TOUCH Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft): counterLeft(counterLeft), counterRight(counterRight), controller(controller), wallRight(wallRight), wallFront(wallFront), wallLeft(wallLeft) {} Turn::~Turn() {} void Turn::turning() { int countsRight = counterRight.read(); //EncoderCounts auslesen int countsRight0 = countsRight; //ReferenzCounts setzten int countsLeft = counterLeft.read(); int countsLeft0 = countsLeft; //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind if (wallLeft == 0){ //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); }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus //printf("Vorne ist frei\n"); }else if (wallRight == 0) { //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); }else{ //Alle Wege versperrt-> Wenden //printf("Alles versperrt...zurueck\n"); while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*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); //0.0f controller.setDesiredSpeedLeft(0.5f); //0.0f } }