Patrick Ruesi
/
Micromouse_alpha_copy_copy
Version12.04.18
Fork of Micromouse_beta by
Diff: Turn.cpp
- Revision:
- 1:d9e840c48b1e
- Child:
- 2:592f01278db4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Turn.cpp Sat Mar 31 16:45:57 2018 +0000 @@ -0,0 +1,80 @@ +#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); + } +}