File content as of revision 6:7bbcdd07bc2d:

/*Roboshark V5
Regler.cpp
Erstellt: V.Ahlers
geändert: V.Ahlers
V.5.18
Regler zum geradeaus fahren
*/

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


using namespace std;

const float Regler :: PERIOD = 0.2f;
const int Regler :: FIXSPEED = 50;
float faktor = 0.02;



Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft):
IrRight (IrRight), IrLeft (IrLeft) {
        
        SpeedR = 0;
        SpeedL = 0;
        ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
    }
     
Regler::~Regler(){
    ticker.detach();
    }
    
    void Regler::setSpeed (){
        measR2 = IrRight.read();            // Converts and read the analog input value (value from 0.0 to 1.0)
        measR2 = measR2* 1000;              // Change the value to be in the 0 to 1000 range
        measL2 = IrLeft.read();             // Converts and read the analog input value (value from 0.0 to 1.0)
        measL2 = measL2 * 1000;             // Change the value to be in the 0 to 1000 range
        
        if((measR2 < 100)) {
            if(measL2 > 280){
                div3 = measL2 - 280;
                kor3 = faktor*div3;
                div2 = 0;
                div1 = 0;
                div4 = 0;
                SpeedR = FIXSPEED;
                SpeedL = FIXSPEED + kor3;
            } else if (measR2 < 280){
                div4 = 280 - measR2;
                kor4 = faktor*div4;
                div2 = 0;
                div1 = 0;
                div3 = 0;
                SpeedR = FIXSPEED + kor4;
                SpeedL = FIXSPEED;
                }
    
        if ((measR2 > measL2) && (meas2R > 100)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
            div1 = measR2 - measL2;
            kor1 = faktor*div1;
            div2 = 0;
            SpeedR = FIXSPEED;
            SpeedL = FIXSPEED + kor1;
        } else if ((measR2 < measL2)&& meas2R > 100) {
            div2 = measL2 - measR2;
            kor2 = 0.02f*div2;
            div1 = 0;
            SpeedR = FIXSPEED + kor2;
            SpeedL = FIXSPEED;
        } else { 
            SpeedR = FIXSPEED;
            SpeedL = FIXSPEED;
            }
//printf("Div1 = %f\n",div1);
//printf("Div2 = %f\n",div2);  
//printf("SpeedR1 = %f\n",SpeedR);
//printf("SpeedL1 = %f\n",SpeedL);        
}
float Regler :: get_SpeedR (){
        SpeedR = SpeedR;
        //printf("SpeedR2 = %f\n",SpeedR);
        return SpeedR;
    }
float Regler :: get_SpeedL (){
        SpeedL = SpeedL;
        //printf("SpeedL2 = %f\n",SpeedL);  
        return SpeedL;
    }