File content as of revision 3:2ec7cf8bc3fc:

#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;
    int totCLeft, totCRight, counter, sumCorrection;
    int corLeft = 0;
    int corRight = 0;
    
    
    printf("Los gehts\n");
        
    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
    
        //kontrastSensor.check();
        controller.setDesiredSpeedRight(DRIVINGSPEED + corRight);
        controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
        //printf("CountsRight%d\n\r", countsRight);
        //printf("CountsLeft%d\n\r", countsLeft);
        
        totCLeft =+countsLeft;
        totCRight =+countsRight;    
        counter++;
        
        if(counter < 100){  //Wait Time until corretion value added
            sumCorrection = totCLeft-totCRight;
            if(sumCorrection <0){
                corLeft =+1; //Driving Speed Left Tire Addition
                corRight=0;
            }else if(sumCorrection >0){
                corLeft=0;
                corRight=+1; //Driving Speed Right Tire Addition   
            }
            counter = 0;
            totCLeft = 0;
            totCRight = 0;
        }
    }
    controller.setDesiredSpeedRight(0.0f);
    controller.setDesiredSpeedLeft(0.0f);

}