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@9:ab19796bf14a, 2018-05-16 (annotated)

Committer:
ruesipat
Date:
Wed May 16 16:41:44 2018 +0000
Revision:
9:ab19796bf14a
Parent:
8:1c8a747c49c8 
;

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),
ruesipat 9:ab19796bf14a 17 modeStart(modeStart),
ruesipat 9:ab19796bf14a 18 path(path)
ruesipat 9:ab19796bf14a 19
ruesipat 1:d9e840c48b1e 20 {}
ruesipat 1:d9e840c48b1e 21
ruesipat 1:d9e840c48b1e 22 Turn::~Turn() {}
ruesipat 1:d9e840c48b1e 23
ruesipat 1:d9e840c48b1e 24
ruesipat 1:d9e840c48b1e 25 void Turn::turning()
ruesipat 1:d9e840c48b1e 26 {
ruesipat 1:d9e840c48b1e 27 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 28 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 29 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 30 int countsLeft0 = countsLeft;
ruesipat 1:d9e840c48b1e 31
ruesipat 9:ab19796bf14a 32 //Vor dem abbiegen halten
ruesipat 9:ab19796bf14a 33
ruesipat 7:5ef09519a6e9 34 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 35 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 1:d9e840c48b1e 36
TheDarkDurzo 8:1c8a747c49c8 37 if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
ruesipat 2:592f01278db4 38 while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 39 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 40 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 41 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 42 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 43 }
ruesipat 9:ab19796bf14a 44
ruesipat 6:a09d2ee3b82e 45 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 46 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 47
ruesipat 7:5ef09519a6e9 48 dontStop = 1;
ruesipat 1:d9e840c48b1e 49
TheDarkDurzo 8:1c8a747c49c8 50 }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
ruesipat 1:d9e840c48b1e 51
ruesipat 7:5ef09519a6e9 52 dontStop = 2;
ruesipat 7:5ef09519a6e9 53
TheDarkDurzo 8:1c8a747c49c8 54 }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
ruesipat 1:d9e840c48b1e 55
ruesipat 2:592f01278db4 56 while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
ruesipat 1:d9e840c48b1e 57 controller.setDesiredSpeedRight(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 58 controller.setDesiredSpeedLeft(-TURNINGSPEED);
ruesipat 1:d9e840c48b1e 59 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 60 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 61 }
ruesipat 6:a09d2ee3b82e 62 controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 63 controller.setDesiredSpeedLeft(-0.5f);
ruesipat 7:5ef09519a6e9 64 dontStop = 3;
ruesipat 1:d9e840c48b1e 65
ruesipat 1:d9e840c48b1e 66
TheDarkDurzo 8:1c8a747c49c8 67 }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
ruesipat 7:5ef09519a6e9 68 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
ruesipat 1:d9e840c48b1e 69 controller.setDesiredSpeedRight(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 70 controller.setDesiredSpeedLeft(TURNINGSPEED);
ruesipat 1:d9e840c48b1e 71 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 72 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 73 }
ruesipat 6:a09d2ee3b82e 74 controller.setDesiredSpeedRight(0.5f); //0.0f
ruesipat 7:5ef09519a6e9 75 controller.setDesiredSpeedLeft(-0.5f); //0.0f
ruesipat 7:5ef09519a6e9 76
ruesipat 7:5ef09519a6e9 77 dontStop = 4;
ruesipat 1:d9e840c48b1e 78 }
ruesipat 1:d9e840c48b1e 79 }