File content as of revision 4:e3f388933954:

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




using namespace std;

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

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()
{

    int countsRight = counterRight.read();  //EncoderCounts auslesen  
    int countsRight0 = countsRight;         //ReferenzCounts setzten
    int countsLeft = counterLeft.read();
    int countsLeft0 = countsLeft;
    //int totCLeft, totCRight, 
    int sumCorrection =0;
    //int sumCor = 0;
    float corLeft = 0;
    float corRight = 0;
    //int counter = 0;
    
    
    
    
    //printf("Los gehts\n");
        
    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f
    
        //kontrastSensor.check();
        controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit
        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;    
        
        if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)){   // irSensor0 durch irSensor3 ersetzt
            
//            int i = 0;
//            while (i < 10){
//                
//                sumCorrection += irSensor3.read()-irSensor2.read(); //irSensor0.read() => sensorRight irSensor2.read() => sensorLeftFornt    // irSensor0 durch irSensor3 ersetzt
//                //sumCor = irSensor2.read()-irSensor3.read();
//                //double AvSensDist;
//                i++;
//            }
//            sumCorrection = sumCorrection/(double)i;
            sumCorrection = irSensor2.read()-irSensor3.read();
            printf("                                        SumCorrection:  %d\n\r", sumCorrection);
            printf("                    DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
            printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());  
        }else{
            corRight=0;
            corLeft=0;
        }
        
        //if(counter > 10){  //Wait Time until corretion value added
            
            
            
            if(sumCorrection < -1){    // (steht naeher an rechter Wand) Arsch zu weit links
                //corLeft = 1 * -sumCorrection; //Driving Speed Left Tire Addition
                corLeft = 20.0f;            // Correction neu 20 statt 1
                corRight=0;
            }else if(sumCorrection > 1){    // (steht naeher an linker Wand) Arsch zu weit rechts
                corLeft=0;
                corRight = 20.0f;           // Correction neu 20 statt 1
                //corRight = 1 * sumCorrection; //Driving Speed Right Tire Addition   
            }else{
              
              
                    corLeft=0;
                    corRight=0;
                
                     
              //  if(sumCor < -2){
              //      corLeft=0;
              //      corRight = 1 * sumCor; //Driving Speed Right Tire Addition
                    
              //  }else if(sumCor > 2){
              //      corLeft = 1 * -sumCor; //Driving Speed Left Tire Addition
              //      corRight=0;
              //  }else{
                    
              //      corLeft=0;
              //      corRight=0;
              //  }
            }
                
            //counter = 0;
            sumCorrection =0;
           // totCLeft = 0;
            //totCRight = 0;
        //}
        //counter++;
        
        
    }
    controller.setDesiredSpeedRight(0.0f); 
    controller.setDesiredSpeedLeft(0.0f); 

}