File content as of revision 5:b8b1a979b0d5:

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




using namespace std;

const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH
const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]
const int Drive::DRIVINGCOUNTS = 1773;  //Entspricht Strecke von 20cm  //DONT TOUCH

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

{

}

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;
    float rightLeftDif = 0;

    float correction = 0;
    float slowdown = 0;

    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) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) {

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

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


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


        //Bereit fuer neuen Durchgang
        correction = 0;
        slowdown = 0;


        //Ermittlung der Differenz Hinten-Vorne
        if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.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
        if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.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;

        }



        //Berechung Korrektur
        correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH




        //Kontrolle ob vorne Wand...Verlangsamen und Anhalten

        if(irSensor1.read() < 150.0f) { //slow down

            slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand

            if ( slowdown > DRIVINGSPEED) {

                drive = 0;

            }
        }


    }//Ende Whileschleife Drive...

    controller.setDesiredSpeedRight(0.0f);
    controller.setDesiredSpeedLeft(0.0f);

}