mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
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?
User | Revision | Line number | New 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 | } |