mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
Drive.cpp@4:e3f388933954, 2018-04-12 (annotated)
- Committer:
- ruesipat
- Date:
- Thu Apr 12 16:14:02 2018 +0000
- Revision:
- 4:e3f388933954
- Parent:
- 3:2ec7cf8bc3fc
- Child:
- 5:b8b1a979b0d5
Version12.04.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 "Drive.h" |
ruesipat | 1:d9e840c48b1e | 3 | |
ruesipat | 1:d9e840c48b1e | 4 | |
ruesipat | 1:d9e840c48b1e | 5 | |
TheDarkDurzo | 3:2ec7cf8bc3fc | 6 | |
ruesipat | 1:d9e840c48b1e | 7 | using namespace std; |
ruesipat | 1:d9e840c48b1e | 8 | |
ruesipat | 2:592f01278db4 | 9 | const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit |
ruesipat | 4:e3f388933954 | 10 | const int Drive::DRIVINGCOUNTS = 1712000; //Entspricht Strecke von 20cm |
ruesipat | 1:d9e840c48b1e | 11 | |
ruesipat | 4:e3f388933954 | 12 | Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3): |
ruesipat | 1:d9e840c48b1e | 13 | kontrastSensor(kontrastSensor), |
ruesipat | 1:d9e840c48b1e | 14 | counterLeft(counterLeft), |
ruesipat | 1:d9e840c48b1e | 15 | counterRight(counterRight), |
ruesipat | 4:e3f388933954 | 16 | controller(controller), |
ruesipat | 4:e3f388933954 | 17 | irSensor0(irSensor0), |
ruesipat | 4:e3f388933954 | 18 | irSensor1(irSensor1), |
ruesipat | 4:e3f388933954 | 19 | irSensor2(irSensor2), |
ruesipat | 4:e3f388933954 | 20 | irSensor3(irSensor3) |
ruesipat | 4:e3f388933954 | 21 | |
ruesipat | 1:d9e840c48b1e | 22 | { |
ruesipat | 4:e3f388933954 | 23 | |
ruesipat | 1:d9e840c48b1e | 24 | } |
ruesipat | 1:d9e840c48b1e | 25 | |
ruesipat | 1:d9e840c48b1e | 26 | Drive::~Drive() {} |
ruesipat | 1:d9e840c48b1e | 27 | |
ruesipat | 1:d9e840c48b1e | 28 | |
ruesipat | 1:d9e840c48b1e | 29 | void Drive::driving() |
ruesipat | 1:d9e840c48b1e | 30 | { |
ruesipat | 1:d9e840c48b1e | 31 | |
ruesipat | 1:d9e840c48b1e | 32 | int countsRight = counterRight.read(); //EncoderCounts auslesen |
ruesipat | 1:d9e840c48b1e | 33 | int countsRight0 = countsRight; //ReferenzCounts setzten |
ruesipat | 1:d9e840c48b1e | 34 | int countsLeft = counterLeft.read(); |
ruesipat | 1:d9e840c48b1e | 35 | int countsLeft0 = countsLeft; |
ruesipat | 4:e3f388933954 | 36 | //int totCLeft, totCRight, |
ruesipat | 4:e3f388933954 | 37 | int sumCorrection =0; |
ruesipat | 4:e3f388933954 | 38 | //int sumCor = 0; |
ruesipat | 4:e3f388933954 | 39 | float corLeft = 0; |
ruesipat | 4:e3f388933954 | 40 | float corRight = 0; |
ruesipat | 4:e3f388933954 | 41 | //int counter = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 42 | |
ruesipat | 1:d9e840c48b1e | 43 | |
ruesipat | 4:e3f388933954 | 44 | |
ruesipat | 4:e3f388933954 | 45 | |
ruesipat | 4:e3f388933954 | 46 | //printf("Los gehts\n"); |
ruesipat | 1:d9e840c48b1e | 47 | |
ruesipat | 4:e3f388933954 | 48 | while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f |
ruesipat | 1:d9e840c48b1e | 49 | |
ruesipat | 1:d9e840c48b1e | 50 | //kontrastSensor.check(); |
ruesipat | 4:e3f388933954 | 51 | controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit |
TheDarkDurzo | 3:2ec7cf8bc3fc | 52 | controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft); |
ruesipat | 1:d9e840c48b1e | 53 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 54 | countsLeft = counterLeft.read(); |
ruesipat | 2:592f01278db4 | 55 | //printf("CountsRight%d\n\r", countsRight); |
ruesipat | 4:e3f388933954 | 56 | //printf(" CountsLeft%d\n\r", countsLeft); |
ruesipat | 4:e3f388933954 | 57 | |
ruesipat | 4:e3f388933954 | 58 | //totCLeft =+countsLeft; |
ruesipat | 4:e3f388933954 | 59 | //totCRight =+countsRight; |
ruesipat | 1:d9e840c48b1e | 60 | |
ruesipat | 4:e3f388933954 | 61 | if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)){ // irSensor0 durch irSensor3 ersetzt |
ruesipat | 4:e3f388933954 | 62 | |
ruesipat | 4:e3f388933954 | 63 | // int i = 0; |
ruesipat | 4:e3f388933954 | 64 | // while (i < 10){ |
ruesipat | 4:e3f388933954 | 65 | // |
ruesipat | 4:e3f388933954 | 66 | // sumCorrection += irSensor3.read()-irSensor2.read(); //irSensor0.read() => sensorRight irSensor2.read() => sensorLeftFornt // irSensor0 durch irSensor3 ersetzt |
ruesipat | 4:e3f388933954 | 67 | // //sumCor = irSensor2.read()-irSensor3.read(); |
ruesipat | 4:e3f388933954 | 68 | // //double AvSensDist; |
ruesipat | 4:e3f388933954 | 69 | // i++; |
ruesipat | 4:e3f388933954 | 70 | // } |
ruesipat | 4:e3f388933954 | 71 | // sumCorrection = sumCorrection/(double)i; |
ruesipat | 4:e3f388933954 | 72 | sumCorrection = irSensor2.read()-irSensor3.read(); |
ruesipat | 4:e3f388933954 | 73 | printf(" SumCorrection: %d\n\r", sumCorrection); |
ruesipat | 4:e3f388933954 | 74 | printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read()); |
ruesipat | 4:e3f388933954 | 75 | printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read()); |
ruesipat | 4:e3f388933954 | 76 | }else{ |
ruesipat | 4:e3f388933954 | 77 | corRight=0; |
ruesipat | 4:e3f388933954 | 78 | corLeft=0; |
ruesipat | 4:e3f388933954 | 79 | } |
ruesipat | 2:592f01278db4 | 80 | |
ruesipat | 4:e3f388933954 | 81 | //if(counter > 10){ //Wait Time until corretion value added |
ruesipat | 4:e3f388933954 | 82 | |
ruesipat | 4:e3f388933954 | 83 | |
ruesipat | 4:e3f388933954 | 84 | |
ruesipat | 4:e3f388933954 | 85 | if(sumCorrection < -1){ // (steht naeher an rechter Wand) Arsch zu weit links |
ruesipat | 4:e3f388933954 | 86 | //corLeft = 1 * -sumCorrection; //Driving Speed Left Tire Addition |
ruesipat | 4:e3f388933954 | 87 | corLeft = 20.0f; // Correction neu 20 statt 1 |
TheDarkDurzo | 3:2ec7cf8bc3fc | 88 | corRight=0; |
ruesipat | 4:e3f388933954 | 89 | }else if(sumCorrection > 1){ // (steht naeher an linker Wand) Arsch zu weit rechts |
TheDarkDurzo | 3:2ec7cf8bc3fc | 90 | corLeft=0; |
ruesipat | 4:e3f388933954 | 91 | corRight = 20.0f; // Correction neu 20 statt 1 |
ruesipat | 4:e3f388933954 | 92 | //corRight = 1 * sumCorrection; //Driving Speed Right Tire Addition |
ruesipat | 4:e3f388933954 | 93 | }else{ |
ruesipat | 4:e3f388933954 | 94 | |
ruesipat | 4:e3f388933954 | 95 | |
ruesipat | 4:e3f388933954 | 96 | corLeft=0; |
ruesipat | 4:e3f388933954 | 97 | corRight=0; |
ruesipat | 4:e3f388933954 | 98 | |
ruesipat | 4:e3f388933954 | 99 | |
ruesipat | 4:e3f388933954 | 100 | // if(sumCor < -2){ |
ruesipat | 4:e3f388933954 | 101 | // corLeft=0; |
ruesipat | 4:e3f388933954 | 102 | // corRight = 1 * sumCor; //Driving Speed Right Tire Addition |
ruesipat | 4:e3f388933954 | 103 | |
ruesipat | 4:e3f388933954 | 104 | // }else if(sumCor > 2){ |
ruesipat | 4:e3f388933954 | 105 | // corLeft = 1 * -sumCor; //Driving Speed Left Tire Addition |
ruesipat | 4:e3f388933954 | 106 | // corRight=0; |
ruesipat | 4:e3f388933954 | 107 | // }else{ |
ruesipat | 4:e3f388933954 | 108 | |
ruesipat | 4:e3f388933954 | 109 | // corLeft=0; |
ruesipat | 4:e3f388933954 | 110 | // corRight=0; |
ruesipat | 4:e3f388933954 | 111 | // } |
TheDarkDurzo | 3:2ec7cf8bc3fc | 112 | } |
ruesipat | 4:e3f388933954 | 113 | |
ruesipat | 4:e3f388933954 | 114 | //counter = 0; |
ruesipat | 4:e3f388933954 | 115 | sumCorrection =0; |
ruesipat | 4:e3f388933954 | 116 | // totCLeft = 0; |
ruesipat | 4:e3f388933954 | 117 | //totCRight = 0; |
ruesipat | 4:e3f388933954 | 118 | //} |
ruesipat | 4:e3f388933954 | 119 | //counter++; |
ruesipat | 4:e3f388933954 | 120 | |
ruesipat | 4:e3f388933954 | 121 | |
ruesipat | 1:d9e840c48b1e | 122 | } |
ruesipat | 4:e3f388933954 | 123 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 4:e3f388933954 | 124 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 125 | |
ruesipat | 1:d9e840c48b1e | 126 | } |