Diff: angle.cpp

Revision:

0:79ed3a6c9ccc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle.cpp	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,41 @@
+#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;
+    
+}
\ No newline at end of file