PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

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?

UserRevisionLine numberNew 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 }