angle.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "angle.h"
00004 
00005 char i;
00006 
00007 //konstruktor
00008 IRsensor::IRsensor(PinName IRpin) : IRsens(IRpin) {}
00009 
00010 
00011 //vraci uhel micku na kole
00012 float IRsensor::get_angle(void)
00013 {    
00014     if(i != 20) {
00015         phi_new = IRsens.read();
00016         //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
00017         //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
00018         phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
00019         s_phi[7] = s_phi[6]; s_phi[6] = s_phi[5];
00020         s_phi[5] = s_phi[4]; s_phi[4] = s_phi[3];
00021         s_phi[3] = s_phi[2]; s_phi[2] = s_phi[1];
00022         s_phi[1] = s_phi[0]; s_phi[0] = phi_new;   
00023     }
00024         
00025     if(i == 20) {
00026         phi_new = IRsens.read();
00027         //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
00028         //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
00029         phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
00030         s_phi[0] = phi_new;  s_phi[1] = s_phi[0];
00031         s_phi[2] = s_phi[1]; s_phi[3] = s_phi[2];
00032         s_phi[4] = s_phi[3]; s_phi[5] = s_phi[4];
00033         s_phi[6] = s_phi[5]; s_phi[7] = s_phi[6];
00034         i = 0;
00035     }
00036         
00037     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;
00038 
00039     return phi_new;
00040     
00041 }