File content as of revision 7:862d80e0ea2d:

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

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


using namespace std;

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



Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor):
IrRight (IrRight), IrLeft (IrLeft), iRSensor (iRSensor) {
        
        SpeedR = 0;
        SpeedL = 0;
        ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
    }
     
Regler::~Regler(){
    ticker.detach();
    }
    
    void Regler::setSpeed (){
        measR2 = iRSensor.readR();            // Converts and read the analog input value (value from 0.0 to 1.0)
        measL2 = iRSensor.readL();             
        
        if((measR2 > 100) && (measL2 < 100)) {                //keine Wnad rechts
            if(measL2 > 47){
                div3 = measL2 - 47;
                kor3 = faktor*div3;
                div2 = 0;
                div1 = 0;
                div4 = 0;
                div5 = 0;
                div6 = 0;
                SpeedR = FIXSPEED;
                SpeedL = FIXSPEED + kor3;
            } else if (measL2 < 52){
                div4 = 52 - measR2;
                kor4 = faktor*div4;
                div2 = 0;
                div1 = 0;
                div3 = 0;
                div5 = 0;
                div6 = 0;
                SpeedR = FIXSPEED;
                SpeedL = FIXSPEED + kor4;
                }else { 
            SpeedR = FIXSPEED;
            SpeedL = FIXSPEED;
            }
        }
        if((measL2 > 100) &&(measR2 < 100)) {                //keine Wnad links
            if(measR2 > 47){
                div5 = measR2 - 47;
                kor5 = faktor*div5;
                div2 = 0;
                div1 = 0;
                div4 = 0;
                div6 = 0;
                div3 = 0;
                SpeedR = FIXSPEED;
                SpeedL = FIXSPEED + kor5;
            } else if (measR2 < 52){
                div6 = 52 - measR2;
                kor6 = faktor*div4;
                div2 = 0;
                div1 = 0;
                div3 = 0;
                div4 = 0;
                div5 = 0;
                SpeedR = FIXSPEED + kor6;
                SpeedL = FIXSPEED;
            } else { 
            SpeedR = FIXSPEED;
            SpeedL = FIXSPEED;
            }
        }
        if ((measR2 < measL2)&& (measL2 - measR2 > 3)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
            div1 = measR2 - measL2;                             // An beiden seinen Wände
            kor1 = 0.1f*div1;
                div2 = 0;
                div3 = 0;
                div4 = 0;
                div5 = 0;
                div6 = 0;
            SpeedR = FIXSPEED;
            SpeedL = FIXSPEED + kor1;
        } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
            div2 = measL2 - measR2;
            kor2 = 0.1f*div2;
                div1 = 0;
                div3 = 0;
                div4 = 0;
                div5 = 0;
                div6 = 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 :: getSpeedR (){
        SpeedR = SpeedR;
        //printf("SpeedR2 = %f\n",SpeedR);
        return SpeedR;
    }
    float Regler :: getSpeedL (){
        SpeedL = SpeedL;
        //printf("SpeedL2 = %f\n",SpeedL);  
        return SpeedL;
    }