---
Dependencies: mbed
Fork of MicroMouse_MASTER_FIVE by
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
- Child:
- 9:ab19796bf14a
mit pathfinding
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), |
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 | } |