Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
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?
User | Revision | Line number | New 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 | } |