Drive.cpp

00001 #include <cmath>
00002 #include "mbed.h"
00003 #include "Drive.h"
00004 
00005 
00006 
00007 
00008 using namespace std;
00009 
00010 const float Drive::FRONTDISTANCE = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH  //62.0f //55.0 110.0
00011 const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]       //130.0f
00012 const int Drive::DRIVINGCOUNTS = 1380;  //Entspricht Strecke von 20cm  //DONT TOUCH /1773 //1800 //1350 /////1390
00013 
00014 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop, int& modeStart, int& path, int& pathNext):
00015     kontrastSensor(kontrastSensor),
00016     counterLeft(counterLeft),
00017     counterRight(counterRight),
00018     controller(controller),
00019     irSensor0(irSensor0),
00020     irSensor1(irSensor1),
00021     irSensor2(irSensor2),
00022     irSensor3(irSensor3),
00023     dontStop(dontStop),
00024     modeStart(modeStart),
00025     path(path),
00026     pathNext(pathNext)
00027 
00028 {
00029 
00030 }
00031 
00032 Drive::~Drive() {}
00033 
00034 
00035 void Drive::driving()
00036 {
00037 
00038     controller.reset();
00039 
00040     int countsRight = counterRight.read();  //EncoderCounts auslesen
00041     int countsLeft = counterLeft.read();
00042     int countsRight0 = countsRight;         //ReferenzCounts setzten
00043     int countsLeft0 = countsLeft;
00044 
00045     float parallelDif = 0.0f;
00046     float rightLeftDif = 0.0f;
00047     float leftDif = 0.0f;
00048     float rightDif = 0.0f;
00049 
00050     float speedCorrection = 0.0f;
00051     int countCorrection = 0;
00052 
00053     float softStart = DRIVINGSPEED;
00054     float slowdown = 0.0f;
00055 
00056     if(dontStop == 2) { //geradeaus
00057         softStart =0.0f;
00058     }
00059 
00060     int drive;
00061 
00062 
00063     //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
00064 
00065     if(irSensor1.read() < FRONTDISTANCE) {
00066         drive = 0;
00067     } else {
00068         drive = 1;
00069     }
00070 
00071     while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
00072 
00073         kontrastSensor.check();
00074         countsRight = counterRight.read();
00075         countsLeft = counterLeft.read();
00076         controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
00077         controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown);  //slowdown
00078 
00079 
00080         //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
00081 
00082         if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
00083 
00084             parallelDif = irSensor3.read()-irSensor2.read();    //differenz hinten vorne bestimmen
00085 
00086         } else { //ist wand eine wand nicht vorhanden => keine korrektur
00087 
00088             parallelDif = 0;
00089         }
00090 
00091 
00092         //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
00093 
00094         if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) {  //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
00095 
00096             rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
00097 
00098         } else { //ist wand eine wand nicht vorhanden => keine korrektur
00099 
00100             rightLeftDif = 0;
00101 
00102             //Ermittlung der Differenz links auf ReferenzWert
00103             if(irSensor2.read() < 120.0f) {  //irSensor2 => sensorLeftFornt
00104 
00105                 leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
00106 
00107             } else { //ist wand eine wand nicht vorhanden => keine korrektur
00108 
00109                 leftDif = 0;
00110             }
00111 
00112 
00113             //Ermittlung der Differenz rechts auf Referenzwert
00114 
00115             if(irSensor0.read() < 120.0f) {  //irSensor0 => sensorRight
00116 
00117                 rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
00118 
00119             } else { //ist wand eine wand nicht vorhanden => keine korrektur
00120 
00121                 rightDif = 0;
00122             }
00123         }
00124 
00125 
00126         //Berechung Korrektur
00127 
00128         speedCorrection = ((0.3f * rightLeftDif) + (0.6f * parallelDif) + (0.6f * leftDif) + (0.6f * rightDif)); //DONT TOUCH 0.35 0.7 0.7 0.7
00129 
00130 
00131         //Anfahrrampe damit die Raeder nicht durchdrehen
00132 
00133         softStart = softStart - 3.5f;
00134         if (softStart <= 0.0f) {
00135             softStart = 0.0f;
00136         }
00137 
00138 
00139         //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
00140 
00141         if(irSensor1.read() < 150.0f) { //slow down
00142 
00143             countCorrection = 5000;
00144             slowdown = slowdown + 5.0f;
00145             if (slowdown >= DRIVINGSPEED - 20.0f) {
00146 
00147                 slowdown = DRIVINGSPEED - 20.0f;
00148             }
00149             if (irSensor1.read() < FRONTDISTANCE) {
00150 
00151                 drive = 0;
00152             }
00153 
00154         } else if((((DRIVINGCOUNTS - countsRight) < 220) || ((countsLeft + DRIVINGCOUNTS) < 220)) && ((irSensor2.read() > 120.0f) && ((modeStart != 1) || (pathNext == 2)))) { //Anhaltrampe wenn nach counts gefahren
00155             slowdown = slowdown + 5.0f;
00156             if (slowdown >= DRIVINGSPEED - 20.0f) {
00157 
00158                 slowdown = DRIVINGSPEED - 20.0f;
00159 }
00160             } else if((((DRIVINGCOUNTS - countsRight) < 220) || ((countsLeft + DRIVINGCOUNTS) < 220)) && ((irSensor0.read() > 120.0f) && ((modeStart == 1) && (pathNext == 3)))) { //Anhaltrampe wenn nach counts gefahren
00161                 slowdown = slowdown + 5.0f;
00162                 if (slowdown >= DRIVINGSPEED - 20.0f) {
00163 
00164                     slowdown = DRIVINGSPEED - 20.0f;
00165                 }
00166             } else {
00167 
00168                 slowdown = 0.0f;
00169             }
00170 
00171             wait(0.01f);
00172         }//Ende Whileschleife Drive...
00173     }