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@7:5ef09519a6e9, 2018-05-07 (annotated)

Committer:
ruesipat
Date:
Mon May 07 18:52:04 2018 +0000
Revision:
7:5ef09519a6e9
Parent:
6:a09d2ee3b82e
Child:
8:1c8a747c49c8
VERSION 7.5.18

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 7:5ef09519a6e9 6 const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
ruesipat 7:5ef09519a6e9 7 const int Turn::TURNINGCOUNTS = 950; //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
ruesipat 1:d9e840c48b1e 8
ruesipat 7:5ef09519a6e9 9 Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop):
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 7:5ef09519a6e9 15 wallLeft(wallLeft),
ruesipat 7:5ef09519a6e9 16 dontStop(dontStop)
ruesipat 1:d9e840c48b1e 17 {}
ruesipat 1:d9e840c48b1e 18
ruesipat 1:d9e840c48b1e 19 Turn::~Turn() {}
ruesipat 1:d9e840c48b1e 20
ruesipat 1:d9e840c48b1e 21
ruesipat 1:d9e840c48b1e 22 void Turn::turning()
ruesipat 1:d9e840c48b1e 23 {
ruesipat 1:d9e840c48b1e 24 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 25 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 26 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 27 int countsLeft0 = countsLeft;
ruesipat 1:d9e840c48b1e 28
ruesipat 7:5ef09519a6e9 29 //vor dem abbiegen halten
ruesipat 7:5ef09519a6e9 30 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 31 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 32 //wait(0.1f);
ruesipat 7:5ef09519a6e9 33
ruesipat 7:5ef09519a6e9 34
ruesipat 7:5ef09519a6e9 35
ruesipat 1:d9e840c48b1e 36 //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
ruesipat 1:d9e840c48b1e 37
ruesipat 1:d9e840c48b1e 38 if (wallLeft == 0){ //Nach Links Drehen
ruesipat 1:d9e840c48b1e 39
ruesipat 4:e3f388933954 40 //printf("Links ist frei\n");
ruesipat 1:d9e840c48b1e 41
ruesipat 2:592f01278db4 42 while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 43 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 44 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 45 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 46 countsLeft = counterLeft.read();
ruesipat 4:e3f388933954 47 //printf("%d\n\r", countsRight);
ruesipat 4:e3f388933954 48 //printf("%d\n\r", countsLeft);
ruesipat 1:d9e840c48b1e 49 }
ruesipat 6:a09d2ee3b82e 50 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 51 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 52
ruesipat 7:5ef09519a6e9 53 dontStop = 1;
ruesipat 1:d9e840c48b1e 54
ruesipat 1:d9e840c48b1e 55 }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
ruesipat 1:d9e840c48b1e 56
ruesipat 4:e3f388933954 57 //printf("Vorne ist frei\n");
ruesipat 1:d9e840c48b1e 58
ruesipat 7:5ef09519a6e9 59 dontStop = 2;
ruesipat 7:5ef09519a6e9 60
ruesipat 1:d9e840c48b1e 61 }else if (wallRight == 0) { //Nach Rechts Drehen
ruesipat 1:d9e840c48b1e 62
ruesipat 4:e3f388933954 63 //printf("Rechts ist frei\n");
ruesipat 1:d9e840c48b1e 64
ruesipat 2:592f01278db4 65 while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 66 controller.setDesiredSpeedRight(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 67 controller.setDesiredSpeedLeft(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 68 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 69 countsLeft = counterLeft.read();
ruesipat 2:592f01278db4 70 //printf("%d\n", countsRight);
ruesipat 2:592f01278db4 71 //printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 72 }
ruesipat 6:a09d2ee3b82e 73 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 74 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 75
ruesipat 7:5ef09519a6e9 76 dontStop = 3;
ruesipat 1:d9e840c48b1e 77
ruesipat 1:d9e840c48b1e 78
ruesipat 1:d9e840c48b1e 79 }else{ //Alle Wege versperrt-> Wenden
ruesipat 1:d9e840c48b1e 80
ruesipat 4:e3f388933954 81 //printf("Alles versperrt...zurueck\n");
ruesipat 1:d9e840c48b1e 82
ruesipat 7:5ef09519a6e9 83 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
ruesipat 1:d9e840c48b1e 84 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 85 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 86 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 87 countsLeft = counterLeft.read();
ruesipat 2:592f01278db4 88 //printf("%d\n", countsRight);
ruesipat 2:592f01278db4 89 //printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 90 }
ruesipat 6:a09d2ee3b82e 91 controller.setDesiredSpeedRight(0.5f); //0.0f
ruesipat 7:5ef09519a6e9 92 controller.setDesiredSpeedLeft(-0.5f); //0.0f
ruesipat 7:5ef09519a6e9 93
ruesipat 7:5ef09519a6e9 94 dontStop = 4;
ruesipat 1:d9e840c48b1e 95 }
ruesipat 1:d9e840c48b1e 96 }