PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_THREE

mit pathdingsbums

Dependencies:   mbed

Fork of MicroMouse_MASTER_TWO by PES2_R2D2.0

Turn.cpp@8:1c8a747c49c8, 2018-05-16 (annotated)

Committer:
TheDarkDurzo
Date:
Wed May 16 12:15:23 2018 +0000
Revision:
8:1c8a747c49c8
Parent:
7:5ef09519a6e9 
mit pathfinding

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruesipat 1:d9e840c48b1e 1 #include <cmath>
ruesipat 1:d9e840c48b1e 2 #include "Turn.h"
TheDarkDurzo 8:1c8a747c49c8 3 //ausr startx und starty kann aus turn.cpp/turn.h entfernt werden
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
TheDarkDurzo 8:1c8a747c49c8 9 Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path):
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),
TheDarkDurzo 8:1c8a747c49c8 16 dontStop(dontStop),
TheDarkDurzo 8:1c8a747c49c8 17 path(path),
TheDarkDurzo 8:1c8a747c49c8 18 modeStart(modeStart)
ruesipat 1:d9e840c48b1e 19 {}
ruesipat 1:d9e840c48b1e 20
ruesipat 1:d9e840c48b1e 21 Turn::~Turn() {}
ruesipat 1:d9e840c48b1e 22
ruesipat 1:d9e840c48b1e 23
ruesipat 1:d9e840c48b1e 24 void Turn::turning()
ruesipat 1:d9e840c48b1e 25 {
ruesipat 1:d9e840c48b1e 26 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 27 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 28 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 29 int countsLeft0 = countsLeft;
ruesipat 1:d9e840c48b1e 30
ruesipat 7:5ef09519a6e9 31 //vor dem abbiegen halten
ruesipat 7:5ef09519a6e9 32 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 33 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 34 //wait(0.1f);
ruesipat 7:5ef09519a6e9 35
ruesipat 7:5ef09519a6e9 36
ruesipat 7:5ef09519a6e9 37
ruesipat 1:d9e840c48b1e 38 //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
ruesipat 1:d9e840c48b1e 39
TheDarkDurzo 8:1c8a747c49c8 40 if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
ruesipat 1:d9e840c48b1e 41
ruesipat 4:e3f388933954 42 //printf("Links ist frei\n");
ruesipat 1:d9e840c48b1e 43
ruesipat 2:592f01278db4 44 while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 45 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 46 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 47 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 48 countsLeft = counterLeft.read();
ruesipat 4:e3f388933954 49 //printf("%d\n\r", countsRight);
ruesipat 4:e3f388933954 50 //printf("%d\n\r", countsLeft);
ruesipat 1:d9e840c48b1e 51 }
ruesipat 6:a09d2ee3b82e 52 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 53 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 54
TheDarkDurzo 8:1c8a747c49c8 55
TheDarkDurzo 8:1c8a747c49c8 56
ruesipat 7:5ef09519a6e9 57 dontStop = 1;
ruesipat 1:d9e840c48b1e 58
TheDarkDurzo 8:1c8a747c49c8 59 }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
ruesipat 1:d9e840c48b1e 60
ruesipat 4:e3f388933954 61 //printf("Vorne ist frei\n");
ruesipat 1:d9e840c48b1e 62
ruesipat 7:5ef09519a6e9 63 dontStop = 2;
ruesipat 7:5ef09519a6e9 64
TheDarkDurzo 8:1c8a747c49c8 65
TheDarkDurzo 8:1c8a747c49c8 66
TheDarkDurzo 8:1c8a747c49c8 67 }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
ruesipat 1:d9e840c48b1e 68
ruesipat 4:e3f388933954 69 //printf("Rechts ist frei\n");
ruesipat 1:d9e840c48b1e 70
ruesipat 2:592f01278db4 71 while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 72 controller.setDesiredSpeedRight(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 73 controller.setDesiredSpeedLeft(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 74 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 75 countsLeft = counterLeft.read();
ruesipat 2:592f01278db4 76 //printf("%d\n", countsRight);
ruesipat 2:592f01278db4 77 //printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 78 }
ruesipat 6:a09d2ee3b82e 79 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 80 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 81
TheDarkDurzo 8:1c8a747c49c8 82
TheDarkDurzo 8:1c8a747c49c8 83
ruesipat 7:5ef09519a6e9 84 dontStop = 3;
ruesipat 1:d9e840c48b1e 85
ruesipat 1:d9e840c48b1e 86
TheDarkDurzo 8:1c8a747c49c8 87 }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
ruesipat 1:d9e840c48b1e 88
ruesipat 4:e3f388933954 89 //printf("Alles versperrt...zurueck\n");
ruesipat 1:d9e840c48b1e 90
ruesipat 7:5ef09519a6e9 91 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
ruesipat 1:d9e840c48b1e 92 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 93 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 94 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 95 countsLeft = counterLeft.read();
ruesipat 2:592f01278db4 96 //printf("%d\n", countsRight);
ruesipat 2:592f01278db4 97 //printf("%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 98 }
ruesipat 6:a09d2ee3b82e 99 controller.setDesiredSpeedRight(0.5f); //0.0f
ruesipat 7:5ef09519a6e9 100 controller.setDesiredSpeedLeft(-0.5f); //0.0f
ruesipat 7:5ef09519a6e9 101
TheDarkDurzo 8:1c8a747c49c8 102
TheDarkDurzo 8:1c8a747c49c8 103
ruesipat 7:5ef09519a6e9 104 dontStop = 4;
ruesipat 1:d9e840c48b1e 105 }
ruesipat 1:d9e840c48b1e 106 }