PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

Turn.cpp@1:d9e840c48b1e, 2018-03-31 (annotated)

Committer:
ruesipat
Date:
Sat Mar 31 16:45:57 2018 +0000
Revision:
1:d9e840c48b1e
Child:
2:592f01278db4
j

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruesipat 1:d9e840c48b1e 1 #include <cmath>
ruesipat 1:d9e840c48b1e 2 #include "Turn.h"
ruesipat 1:d9e840c48b1e 3
ruesipat 1:d9e840c48b1e 4 using namespace std;
ruesipat 1:d9e840c48b1e 5
ruesipat 1:d9e840c48b1e 6 const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit
ruesipat 1:d9e840c48b1e 7 const int Turn::TURNINGCOUNTS = 2000; //Entspricht Drehung um 90Grad
ruesipat 1:d9e840c48b1e 8
ruesipat 1:d9e840c48b1e 9 Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
ruesipat 1:d9e840c48b1e 10 counterLeft(counterLeft),
ruesipat 1:d9e840c48b1e 11 counterRight(counterRight),
ruesipat 1:d9e840c48b1e 12 controller(controller),
ruesipat 1:d9e840c48b1e 13 wallRight(wallRight),
ruesipat 1:d9e840c48b1e 14 wallFront(wallFront),
ruesipat 1:d9e840c48b1e 15 wallLeft(wallLeft)
ruesipat 1:d9e840c48b1e 16 {}
ruesipat 1:d9e840c48b1e 17
ruesipat 1:d9e840c48b1e 18 Turn::~Turn() {}
ruesipat 1:d9e840c48b1e 19
ruesipat 1:d9e840c48b1e 20
ruesipat 1:d9e840c48b1e 21 void Turn::turning()
ruesipat 1:d9e840c48b1e 22 {
ruesipat 1:d9e840c48b1e 23 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 24 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 25 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 26 int countsLeft0 = countsLeft;
ruesipat 1:d9e840c48b1e 27
ruesipat 1:d9e840c48b1e 28 //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
ruesipat 1:d9e840c48b1e 29
ruesipat 1:d9e840c48b1e 30 if (wallLeft == 0){ //Nach Links Drehen
ruesipat 1:d9e840c48b1e 31
ruesipat 1:d9e840c48b1e 32 printf("Links ist frei\n");
ruesipat 1:d9e840c48b1e 33
ruesipat 1:d9e840c48b1e 34 while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 35 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 36 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 37 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 38 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 39 printf("%d\n", countsRight);
ruesipat 1:d9e840c48b1e 40 printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 41 }
ruesipat 1:d9e840c48b1e 42 controller.setDesiredSpeedRight(0.0f);
ruesipat 1:d9e840c48b1e 43 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 44
ruesipat 1:d9e840c48b1e 45 }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
ruesipat 1:d9e840c48b1e 46
ruesipat 1:d9e840c48b1e 47 printf("Vorne ist frei\n");
ruesipat 1:d9e840c48b1e 48
ruesipat 1:d9e840c48b1e 49 }else if (wallRight == 0) { //Nach Rechts Drehen
ruesipat 1:d9e840c48b1e 50
ruesipat 1:d9e840c48b1e 51 printf("Rechts ist frei\n");
ruesipat 1:d9e840c48b1e 52
ruesipat 1:d9e840c48b1e 53 while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 54 controller.setDesiredSpeedRight(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 55 controller.setDesiredSpeedLeft(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 56 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 57 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 58 printf("%d\n", countsRight);
ruesipat 1:d9e840c48b1e 59 printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 60 }
ruesipat 1:d9e840c48b1e 61 controller.setDesiredSpeedRight(0.0f);
ruesipat 1:d9e840c48b1e 62 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 63
ruesipat 1:d9e840c48b1e 64
ruesipat 1:d9e840c48b1e 65 }else{ //Alle Wege versperrt-> Wenden
ruesipat 1:d9e840c48b1e 66
ruesipat 1:d9e840c48b1e 67 printf("Alles versperrt...zurueck\n");
ruesipat 1:d9e840c48b1e 68
ruesipat 1:d9e840c48b1e 69 while((countsRight >= countsRight0 - 2*TURNINGCOUNTS) && (countsLeft >= countsLeft0 - 2*TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 70 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 71 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 72 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 73 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 74 printf("%d\n", countsRight);
ruesipat 1:d9e840c48b1e 75 printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 76 }
ruesipat 1:d9e840c48b1e 77 controller.setDesiredSpeedRight(0.0f);
ruesipat 1:d9e840c48b1e 78 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 79 }
ruesipat 1:d9e840c48b1e 80 }