File content as of revision 1:d9e840c48b1e:

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



using namespace std;

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

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

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)){
    
        //kontrastSensor.check();
        controller.setDesiredSpeedRight(DRIVINGSPEED);
        controller.setDesiredSpeedLeft(-DRIVINGSPEED);
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
        //printf("CountsRight%d\n", countsRight);
        //printf("CountsLeft%d\n", countsLeft);
        
        //parallelitaet und center pruefen!!!!!!!!!!!!
        
    }
    controller.setDesiredSpeedRight(0.0f);
    controller.setDesiredSpeedLeft(0.0f);

}