Diff: main.cpp

Revision:

2:0bf41ad96558

Parent:

1:3500bf8487d0

Child:

3:bebfc64cefe4

--- a/main.cpp	Sat Nov 09 06:49:43 2013 +0000
+++ b/main.cpp	Sat Nov 09 07:04:13 2013 +0000
@@ -7,8 +7,31 @@
 AnalogIn ainSensC(p19);
 AnalogIn ainSensD(p20);
 
-int valAzimut = 0;
-int valElevacija = 0;
+float valAzimut = 0;
+float valElevacija = 0;
+
+float SensA, SensB, SensC, SensD;
+void readValuesForAveraging() {
+
+    SensA = 0;
+    SensB = 0;
+    SensC = 0;
+    SensD = 0;
+    
+    for (int i = 0; i <= 9; i++) {  
+        SensA += ainSensA;
+        SensB += ainSensB;
+        SensC += ainSensC;
+        SensD += ainSensD;
+    }
+    SensA /= 10;
+    SensB /= 10;
+    SensC /= 10;
+    SensD /= 10;
+        
+    valAzimut = (SensA + SensB) - (SensC + SensD); 
+    valElevacija = (SensB + SensC) - (SensA + SensD);
+}
 
 int main() {
 
@@ -17,15 +40,24 @@
 
     while(1) {
     
-        valAzimut = (ainSensA.read_u16() + ainSensB.read_u16()) 
-                - (ainSensC.read_u16() + ainSensD.read_u16()); 
-        valElevacija = (ainSensB.read_u16() + ainSensC.read_u16()) 
-                - (ainSensA.read_u16() + ainSensD.read_u16());
-                
-        (*motorEl).stop();        
-        (*motorEl).movePositive();        
-        (*motorEl).moveNegative();        
+        readValuesForAveraging();
         
-    
+        if (valAzimut > 0.2){ // positive azimuth deviation
+            (*motorAz).movePositive();
+        }
+        else if (valAzimut < -0.2) { // negative azimuth deviation
+            (*motorAz).moveNegative();
+        }
+        (*motorAz).stop(); 
+        
+        if (valElevacija > 0.2){ // positive azimuth deviation
+            (*motorEl).movePositive();
+        }
+        else if (valElevacija < -0.2) { // negative azimuth deviation
+            (*motorEl).moveNegative();
+        }
+        (*motorEl).stop(); 
     }
 }
+
+