READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Drive.cpp@3:2ec7cf8bc3fc, 2018-04-08 (annotated)
- Committer:
- TheDarkDurzo
- Date:
- Sun Apr 08 19:17:01 2018 +0000
- Revision:
- 3:2ec7cf8bc3fc
- Parent:
- 2:592f01278db4
- Child:
- 4:e3f388933954
dunno
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 | 2:592f01278db4 | 10 | const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm |
ruesipat | 1:d9e840c48b1e | 11 | |
ruesipat | 2:592f01278db4 | 12 | Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack): |
ruesipat | 1:d9e840c48b1e | 13 | kontrastSensor(kontrastSensor), |
ruesipat | 1:d9e840c48b1e | 14 | counterLeft(counterLeft), |
ruesipat | 1:d9e840c48b1e | 15 | counterRight(counterRight), |
ruesipat | 1:d9e840c48b1e | 16 | controller(controller) |
ruesipat | 1:d9e840c48b1e | 17 | { |
ruesipat | 1:d9e840c48b1e | 18 | this->distanceRight = distanceRight; |
ruesipat | 1:d9e840c48b1e | 19 | this->distanceFront = distanceFront; |
ruesipat | 1:d9e840c48b1e | 20 | this->distanceLeftFront = distanceLeftFront; |
ruesipat | 2:592f01278db4 | 21 | this->distanceLeftBack = distanceLeftBack; |
ruesipat | 1:d9e840c48b1e | 22 | } |
ruesipat | 1:d9e840c48b1e | 23 | |
ruesipat | 1:d9e840c48b1e | 24 | Drive::~Drive() {} |
ruesipat | 1:d9e840c48b1e | 25 | |
ruesipat | 1:d9e840c48b1e | 26 | |
ruesipat | 1:d9e840c48b1e | 27 | void Drive::driving() |
ruesipat | 1:d9e840c48b1e | 28 | { |
ruesipat | 1:d9e840c48b1e | 29 | |
ruesipat | 1:d9e840c48b1e | 30 | int countsRight = counterRight.read(); //EncoderCounts auslesen |
ruesipat | 1:d9e840c48b1e | 31 | int countsRight0 = countsRight; //ReferenzCounts setzten |
ruesipat | 1:d9e840c48b1e | 32 | int countsLeft = counterLeft.read(); |
ruesipat | 1:d9e840c48b1e | 33 | int countsLeft0 = countsLeft; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 34 | int totCLeft, totCRight, counter, sumCorrection; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 35 | int corLeft = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 36 | int corRight = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 37 | |
ruesipat | 1:d9e840c48b1e | 38 | |
ruesipat | 1:d9e840c48b1e | 39 | printf("Los gehts\n"); |
ruesipat | 1:d9e840c48b1e | 40 | |
TheDarkDurzo | 3:2ec7cf8bc3fc | 41 | while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! |
ruesipat | 1:d9e840c48b1e | 42 | |
ruesipat | 1:d9e840c48b1e | 43 | //kontrastSensor.check(); |
TheDarkDurzo | 3:2ec7cf8bc3fc | 44 | controller.setDesiredSpeedRight(DRIVINGSPEED + corRight); |
TheDarkDurzo | 3:2ec7cf8bc3fc | 45 | controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft); |
ruesipat | 1:d9e840c48b1e | 46 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 47 | countsLeft = counterLeft.read(); |
ruesipat | 2:592f01278db4 | 48 | //printf("CountsRight%d\n\r", countsRight); |
ruesipat | 2:592f01278db4 | 49 | //printf("CountsLeft%d\n\r", countsLeft); |
ruesipat | 1:d9e840c48b1e | 50 | |
TheDarkDurzo | 3:2ec7cf8bc3fc | 51 | totCLeft =+countsLeft; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 52 | totCRight =+countsRight; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 53 | counter++; |
ruesipat | 2:592f01278db4 | 54 | |
TheDarkDurzo | 3:2ec7cf8bc3fc | 55 | if(counter < 100){ //Wait Time until corretion value added |
TheDarkDurzo | 3:2ec7cf8bc3fc | 56 | sumCorrection = totCLeft-totCRight; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 57 | if(sumCorrection <0){ |
TheDarkDurzo | 3:2ec7cf8bc3fc | 58 | corLeft =+1; //Driving Speed Left Tire Addition |
TheDarkDurzo | 3:2ec7cf8bc3fc | 59 | corRight=0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 60 | }else if(sumCorrection >0){ |
TheDarkDurzo | 3:2ec7cf8bc3fc | 61 | corLeft=0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 62 | corRight=+1; //Driving Speed Right Tire Addition |
TheDarkDurzo | 3:2ec7cf8bc3fc | 63 | } |
TheDarkDurzo | 3:2ec7cf8bc3fc | 64 | counter = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 65 | totCLeft = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 66 | totCRight = 0; |
TheDarkDurzo | 3:2ec7cf8bc3fc | 67 | } |
ruesipat | 1:d9e840c48b1e | 68 | } |
ruesipat | 1:d9e840c48b1e | 69 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 1:d9e840c48b1e | 70 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 71 | |
ruesipat | 1:d9e840c48b1e | 72 | } |