Diff: main.cpp

Revision:

1:2e7d4aa6e79e

Parent:

0:7447b8021b33

diff -r 7447b8021b33 -r 2e7d4aa6e79e main.cpp
--- a/main.cpp	Fri Nov 08 19:09:47 2013 +0000
+++ b/main.cpp	Fri Nov 08 22:33:31 2013 +0000
@@ -8,23 +8,60 @@
 
 int valAzimut = 0;
 int valElevacija = 0;
+float SensA, SensB, SensC, SensD;
 
 int main() {
 
-    Motor *motorEl = new Motor(p25, p26);
-    Motor *motorAz = new Motor(p23, p24);
-
-    while(1) {
+    Motor *motorEl = new Motor(p25, p26, p24);
+    Motor *motorAz = new Motor(p22, p21, p23);
+    
+    (*motorAz).stop();
+    (*motorEl).stop();
     
-        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());
-                
+    while(1) {
+    SensA= 0;   // initiate values before averaging
+    SensB= 0;
+    SensC= 0;
+    SensD= 0;
+    for (int i=0; i<=9; i++) {  
+        SensA= SensA + ainSensA;  //get 10 samples of sensors values
+        SensB= SensB + ainSensB;
+        SensC= SensC + ainSensC;
+        SensD= SensD + ainSensD;
+    }
+    SensA= SensA/10;
+    SensB= SensB/10;
+    SensC= SensC/10;
+    SensD= SensD/10;
+        
+        valAzimut = (SensA + SensB) - (SensC + SensD); 
+        valElevacija = (SensB + SensC) - (SensA + SensD);
+        
+        if (valAzimut > 0.2){
+        // positive azimuth deviation
+            (*motorAz).movePositiveSlow();
+        }
+        else if (valAzimut < -0.2) {
+        // negative azimuth deviation
+            (*motorAz).moveNegativeSlow();
+        }
+        (*motorAz).stopslow(); 
+        
+        if (valElevacija > 0.2){
+        // positive azimuth deviation
+            (*motorEl).movePositiveSlow();
+        }
+        else if (valElevacija < -0.2) {
+        // negative azimuth deviation
+            (*motorEl).moveNegativeSlow();
+        }
+        (*motorEl).stopslow(); 
+        
+        /*        
         (*motorEl).stop();        
         (*motorEl).movePositive();        
         (*motorEl).moveNegative();        
-        
+        */
     
     }
 }