File content as of revision 7:5ef09519a6e9:

#include <cmath>
#include "mbed.h"
#include "Drive.h"




using namespace std;

const float Drive::FRONTDISTANCE = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH  //62.0f //55.0 110.0
const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]       //130.0f
const int Drive::DRIVINGCOUNTS = 1390;  //Entspricht Strecke von 20cm  //DONT TOUCH /1773 //1800 //1350 /1370

Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop):
    kontrastSensor(kontrastSensor),
    counterLeft(counterLeft),
    counterRight(counterRight),
    controller(controller),
    irSensor0(irSensor0),
    irSensor1(irSensor1),
    irSensor2(irSensor2),
    irSensor3(irSensor3),
    dontStop(dontStop)

{

}

Drive::~Drive() {}


void Drive::driving()
{

    controller.reset();

    int countsRight = counterRight.read();  //EncoderCounts auslesen
    int countsLeft = counterLeft.read();        //0 - 32767

    //printf("CountsRight%d\n\r", countsRight);
    //printf("           CountsLeft%d\n\r", countsLeft);


    int countsRight0 = countsRight;         //ReferenzCounts setzten
    int countsLeft0 = countsLeft;

    float parallelDif = 0.0f;
    float rightLeftDif = 0.0f;
    float leftDif = 0.0f;
    float rightDif = 0.0f;

    float speedCorrection = 0.0f;
    int countCorrection = 0;
    
    
    float softStart = DRIVINGSPEED;
    float slowdown = 0.0f;

    
    if(dontStop == 2){ //geradeaus
        softStart =0.0f;
    }
    

    int drive;


    //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird

    if(irSensor1.read() < FRONTDISTANCE) {
        drive = 0;
    } else {
        drive = 1;
    }


    //printf("Los gehts\n");

    while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {

        kontrastSensor.check();
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
        controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
        controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown);  //slowdown

        //printf("CountsRight%d\n\r", countsRight);
        //printf("           CountsLeft%d\n\r", countsLeft);


        //printf("correction:  %.0f\n\r", correction);



        //Bereit fuer neuen Durchgang
        //speedCorrection = 0.0f;
        //leftDif = 0.0f;
        //rightDif = 0.0f;
        //slowdown = 0.0f;


        //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
        if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront

            parallelDif = irSensor3.read()-irSensor2.read();    //differenz hinten vorne bestimmen
            //printf("        DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
            //printf("        DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
            //printf("            parallelDif:  %.0f \n\r", parallelDif);

        } else { //ist wand eine wand nicht vorhanden => keine korrektur

            parallelDif = 0;
        }


        //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
        if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) {  //irSensor0 => sensorRight irSensor2 => sensorLeftFornt

            rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
            //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
            //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
            //printf(" rightLeftDif:  %.0f \n\r", rightLeftDif);

        } else { //ist wand eine wand nicht vorhanden => keine korrektur

            rightLeftDif = 0;

            //Ermittlung der Differenz links auf ReferenzWert
            if(irSensor2.read() < 120.0f) {  //irSensor2 => sensorLeftFornt

                leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
                //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
                //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
                //printf(" leftDif:  %.0f \n\r", leftDif);

            } else { //ist wand eine wand nicht vorhanden => keine korrektur

                leftDif = 0;

            }


            //Ermittlung der Differenz rechts auf Referenzwert
            if(irSensor0.read() < 120.0f) {  //irSensor0 => sensorRight

                rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
                //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
                //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
                //printf(" rightDif:  %.0f \n\r", rightDif);

            } else { //ist wand eine wand nicht vorhanden => keine korrektur

                rightDif = 0;

            }




        }



        //Berechung Korrektur
        speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5



        //Anfahrrampe damit die Raeder nicht durchdrehen
        softStart = softStart - 3.5f;
        if (softStart <= 0.0f) {
            softStart = 0.0f;
        }
        //printf("           softStart: %.0f\n\r", softStart);







        //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
        if(irSensor1.read() < 150.0f) { //slow down

            countCorrection = 5000;
            //slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand
            slowdown = slowdown + 5.0f;
            if (slowdown >= DRIVINGSPEED - 20.0f) {
                slowdown = DRIVINGSPEED - 20.0f;
            }
            if (irSensor1.read() < FRONTDISTANCE) {

                drive = 0;

            }
            
        } else if(((DRIVINGCOUNTS - countsRight) < 300 || (countsLeft + DRIVINGCOUNTS) < 300) && (irSensor2.read() > 120.0f)) { //Anhaltrampe wenn nach counts gefahren
            slowdown = slowdown + 5.0f;
            if (slowdown >= DRIVINGSPEED - 20.0f) {
                slowdown = DRIVINGSPEED - 20.0f;
            }
            
        } else{
            slowdown = 0.0f;
        }
        
        
        //if(dontStop == 2){ //geradeaus
        //    slowdown = 0.0f;
        //}



        wait(0.01f);
    }//Ende Whileschleife Drive...

    //controller.setDesiredSpeedRight(0.5f);  //0.5f
    //controller.setDesiredSpeedLeft(-0.5f);   //-0.5f

}