File content as of revision 2:592f01278db4:

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



using namespace std;

const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
const int Drive::DRIVINGCOUNTS = 1712;  //Entspricht Strecke von 20cm

Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack):
    kontrastSensor(kontrastSensor),
    counterLeft(counterLeft), 
    counterRight(counterRight),
    controller(controller)
{
    this->distanceRight = distanceRight;
    this->distanceFront = distanceFront; 
    this->distanceLeftFront = distanceLeftFront;
    this->distanceLeftBack = distanceLeftBack;
}

Drive::~Drive() {}


void Drive::driving()
{

    int countsRight = counterRight.read();  //EncoderCounts auslesen  
    int countsRight0 = countsRight;         //ReferenzCounts setzten
    int countsLeft = counterLeft.read();
    int countsLeft0 = countsLeft;
    
    printf("Los gehts\n");
        
    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS)){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
    
        //kontrastSensor.check();
        controller.setDesiredSpeedRight(DRIVINGSPEED);
        controller.setDesiredSpeedLeft(-DRIVINGSPEED);
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
        //printf("CountsRight%d\n\r", countsRight);
        //printf("CountsLeft%d\n\r", countsLeft);
        
        

        //(distanceLeftFront - distanceRight)  /rotationsgeschwindigkeit und allenfalls korrektur!!!!!!!!!!!!!!
        
    }
    controller.setDesiredSpeedRight(0.0f);
    controller.setDesiredSpeedLeft(0.0f);

}