![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertige Version mit geschwindigkeit 100
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Turn.cpp
- Committer:
- ruesipat
- Date:
- 2018-03-31
- Revision:
- 1:d9e840c48b1e
- Child:
- 2:592f01278db4
File content as of revision 1:d9e840c48b1e:
#include <cmath> #include "Turn.h" using namespace std; const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit const int Turn::TURNINGCOUNTS = 2000; //Entspricht Drehung um 90Grad 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", countsRight); printf("%d\n", countsLeft); } controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); }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.0f); controller.setDesiredSpeedLeft(0.0f); }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.0f); controller.setDesiredSpeedLeft(0.0f); } }