Fertige Version mit geschwindigkeit 100
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Turn.cpp@5:b8b1a979b0d5, 2018-04-25 (annotated)
- Committer:
- ruesipat
- Date:
- Wed Apr 25 12:07:03 2018 +0000
- Revision:
- 5:b8b1a979b0d5
- Parent:
- 4:e3f388933954
- Child:
- 6:a09d2ee3b82e
25.04.2018_FINDET ZIEL
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 | 5:b8b1a979b0d5 | 6 | const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm] |
ruesipat | 5:b8b1a979b0d5 | 7 | const int Turn::TURNINGCOUNTS = 868; //Entspricht Drehung um 90Grad //DONT TOUCH |
ruesipat | 1:d9e840c48b1e | 8 | |
ruesipat | 1:d9e840c48b1e | 9 | Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft): |
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 | 1:d9e840c48b1e | 15 | wallLeft(wallLeft) |
ruesipat | 1:d9e840c48b1e | 16 | {} |
ruesipat | 1:d9e840c48b1e | 17 | |
ruesipat | 1:d9e840c48b1e | 18 | Turn::~Turn() {} |
ruesipat | 1:d9e840c48b1e | 19 | |
ruesipat | 1:d9e840c48b1e | 20 | |
ruesipat | 1:d9e840c48b1e | 21 | void Turn::turning() |
ruesipat | 1:d9e840c48b1e | 22 | { |
ruesipat | 1:d9e840c48b1e | 23 | int countsRight = counterRight.read(); //EncoderCounts auslesen |
ruesipat | 1:d9e840c48b1e | 24 | int countsRight0 = countsRight; //ReferenzCounts setzten |
ruesipat | 1:d9e840c48b1e | 25 | int countsLeft = counterLeft.read(); |
ruesipat | 1:d9e840c48b1e | 26 | int countsLeft0 = countsLeft; |
ruesipat | 1:d9e840c48b1e | 27 | |
ruesipat | 1:d9e840c48b1e | 28 | //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind |
ruesipat | 1:d9e840c48b1e | 29 | |
ruesipat | 1:d9e840c48b1e | 30 | if (wallLeft == 0){ //Nach Links Drehen |
ruesipat | 1:d9e840c48b1e | 31 | |
ruesipat | 4:e3f388933954 | 32 | //printf("Links ist frei\n"); |
ruesipat | 1:d9e840c48b1e | 33 | |
ruesipat | 2:592f01278db4 | 34 | while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){ |
ruesipat | 1:d9e840c48b1e | 35 | controller.setDesiredSpeedRight(TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 36 | controller.setDesiredSpeedLeft(TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 37 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 38 | countsLeft = counterLeft.read(); |
ruesipat | 4:e3f388933954 | 39 | //printf("%d\n\r", countsRight); |
ruesipat | 4:e3f388933954 | 40 | //printf("%d\n\r", countsLeft); |
ruesipat | 1:d9e840c48b1e | 41 | } |
ruesipat | 1:d9e840c48b1e | 42 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 1:d9e840c48b1e | 43 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 44 | |
ruesipat | 1:d9e840c48b1e | 45 | }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus |
ruesipat | 1:d9e840c48b1e | 46 | |
ruesipat | 4:e3f388933954 | 47 | //printf("Vorne ist frei\n"); |
ruesipat | 1:d9e840c48b1e | 48 | |
ruesipat | 1:d9e840c48b1e | 49 | }else if (wallRight == 0) { //Nach Rechts Drehen |
ruesipat | 1:d9e840c48b1e | 50 | |
ruesipat | 4:e3f388933954 | 51 | //printf("Rechts ist frei\n"); |
ruesipat | 1:d9e840c48b1e | 52 | |
ruesipat | 2:592f01278db4 | 53 | while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){ |
ruesipat | 1:d9e840c48b1e | 54 | controller.setDesiredSpeedRight(-TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 55 | controller.setDesiredSpeedLeft(-TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 56 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 57 | countsLeft = counterLeft.read(); |
ruesipat | 2:592f01278db4 | 58 | //printf("%d\n", countsRight); |
ruesipat | 2:592f01278db4 | 59 | //printf("%d\n", countsLeft); |
ruesipat | 1:d9e840c48b1e | 60 | } |
ruesipat | 1:d9e840c48b1e | 61 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 1:d9e840c48b1e | 62 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 63 | |
ruesipat | 1:d9e840c48b1e | 64 | |
ruesipat | 1:d9e840c48b1e | 65 | }else{ //Alle Wege versperrt-> Wenden |
ruesipat | 1:d9e840c48b1e | 66 | |
ruesipat | 4:e3f388933954 | 67 | //printf("Alles versperrt...zurueck\n"); |
ruesipat | 1:d9e840c48b1e | 68 | |
ruesipat | 2:592f01278db4 | 69 | while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){ |
ruesipat | 1:d9e840c48b1e | 70 | controller.setDesiredSpeedRight(TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 71 | controller.setDesiredSpeedLeft(TURNINGSPEED); |
ruesipat | 1:d9e840c48b1e | 72 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 73 | countsLeft = counterLeft.read(); |
ruesipat | 2:592f01278db4 | 74 | //printf("%d\n", countsRight); |
ruesipat | 2:592f01278db4 | 75 | //printf("%d\n", countsLeft); |
ruesipat | 1:d9e840c48b1e | 76 | } |
ruesipat | 1:d9e840c48b1e | 77 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 1:d9e840c48b1e | 78 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 79 | } |
ruesipat | 1:d9e840c48b1e | 80 | } |