File content as of revision 0:79ed3a6c9ccc:

#include "mbed.h"
#include "rtos.h"
#include "angle.h"

char i;

//konstruktor
IRsensor::IRsensor(PinName IRpin) : IRsens(IRpin) {}


//vraci uhel micku na kole
float IRsensor::get_angle(void)
{    
    if(i != 20) {
        phi_new = IRsens.read();
        //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
        //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
        phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
        s_phi[7] = s_phi[6]; s_phi[6] = s_phi[5];
        s_phi[5] = s_phi[4]; s_phi[4] = s_phi[3];
        s_phi[3] = s_phi[2]; s_phi[2] = s_phi[1];
        s_phi[1] = s_phi[0]; s_phi[0] = phi_new;   
    }
        
    if(i == 20) {
        phi_new = IRsens.read();
        //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
        //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
        phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
        s_phi[0] = phi_new;  s_phi[1] = s_phi[0];
        s_phi[2] = s_phi[1]; s_phi[3] = s_phi[2];
        s_phi[4] = s_phi[3]; s_phi[5] = s_phi[4];
        s_phi[6] = s_phi[5]; s_phi[7] = s_phi[6];
        i = 0;
    }
        
    phi_new = (s_phi[0] + s_phi[1] + s_phi[2] + s_phi[3] + s_phi[4] + s_phi[5] + s_phi[6] + s_phi[7])/8.0;

    return phi_new;
    
}