Diff: main.cpp

Revision:

3:bebfc64cefe4

Parent:

2:0bf41ad96558

Child:

4:03b68322905f

--- a/main.cpp	Sat Nov 09 07:04:13 2013 +0000
+++ b/main.cpp	Sat Nov 09 13:44:18 2013 +0000
@@ -1,6 +1,15 @@
 #include "mbed.h"
 #include "Motor.h"
-#include "MotorPwm.h"
+
+#define sensorStartTreshold 0.07
+#define sensorStopTreshold 0.02
+
+#define calibrateFactorSenzA 1
+#define calibrateFactorSenzB 0.97
+#define calibrateFactorSenzC 1.06
+#define calibrateFactorSenzD 0.9
+
+Serial pc(USBTX, USBRX);
 
 AnalogIn ainSensA(p17);
 AnalogIn ainSensB(p18);
@@ -18,45 +27,123 @@
     SensC = 0;
     SensD = 0;
     
-    for (int i = 0; i <= 9; i++) {  
+    for (int i = 0; i < 20; i++) {  
         SensA += ainSensA;
         SensB += ainSensB;
         SensC += ainSensC;
         SensD += ainSensD;
     }
-    SensA /= 10;
-    SensB /= 10;
-    SensC /= 10;
-    SensD /= 10;
+    SensA /= 20;
+    SensB /= 20;
+    SensC /= 20;
+    SensD /= 20;
+        
+    SensA *= calibrateFactorSenzA;
+    SensB *= calibrateFactorSenzB;
+    SensC *= calibrateFactorSenzC;
+    SensD *= calibrateFactorSenzD;
         
     valAzimut = (SensA + SensB) - (SensC + SensD); 
     valElevacija = (SensB + SensC) - (SensA + SensD);
+    
+//    pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", 
+//            valAzimut, valElevacija, SensA, SensB, SensC, SensD);
+    pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija);
 }
 
 int main() {
 
-    Motor *motorEl = new MotorPwm(p25, p26);
-    Motor *motorAz = new MotorPwm(p23, p24);
+    Motor *motorAz = new Motor(p25, p26, p24);
+    Motor *motorEl = new Motor(p22, p21, p23);
 
     while(1) {
     
         readValuesForAveraging();
         
-        if (valAzimut > 0.2){ // positive azimuth deviation
-            (*motorAz).movePositive();
+        bool someWrokDone = false;
+        
+// ----- azimut -----
+        
+        if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) {
+            if(valAzimut > 0) 
+                (*motorAz).movePositive();
+            else
+                (*motorAz).moveNegative();
+            someWrokDone = true;
+        }
+        
+        if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
+            (*motorAz).stop();
+            someWrokDone = true;
         }
-        else if (valAzimut < -0.2) { // negative azimuth deviation
-            (*motorAz).moveNegative();
+        
+// ----- elevacija -----
+
+        if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) {
+            if(valElevacija > 0) 
+                (*motorEl).movePositive();
+            else
+                (*motorEl).moveNegative();
+            someWrokDone = true;
         }
-        (*motorAz).stop(); 
+        
+        if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
+            (*motorEl).stop();
+            someWrokDone = true;
+        }
+
+// ---- ništa nije dirano ----
+
+        if(!someWrokDone)
+            wait_ms(500);
+        
+        
+        
         
-        if (valElevacija > 0.2){ // positive azimuth deviation
-            (*motorEl).movePositive();
+        /*
+        if(valAzimut > sensorStartTreshold) {
+            if(!(*motorAz).isMoving())
+                (*motorAz).movePositive();
+            continue;        
+        }
+        
+        if(valAzimut < (-1 * sensorStartTreshold)) {
+            if(!(*motorAz).isMoving())
+                (*motorAz).moveNegative();
+            continue;        
+        }
+        
+        if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
+            (*motorAz).stop();
+        }
+        
+        if(valElevacija > sensorStartTreshold) {
+            if(!(*motorEl).isMoving())
+                (*motorEl).movePositive();
+            continue;        
         }
-        else if (valElevacija < -0.2) { // negative azimuth deviation
+        
+        if(valElevacija < (-1 * sensorStartTreshold)) {
+            if(!(*motorEl).isMoving())
+                (*motorEl).moveNegative();
+            continue;        
+        }
+        
+        if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
+            (*motorEl).stop();
+        }
+        */
+        
+ /*       
+        if (valElevacija > sensorTreshold){ // positive azimuth deviation
+            (*motorEl).movePositive();
+            pc.printf("elevacija pozitiv \n");            
+        }
+        else if (valElevacija < (-1 * sensorTreshold)) { // negative azimuth deviation
             (*motorEl).moveNegative();
+            pc.printf("elevacija negativ \n");            
         }
-        (*motorEl).stop(); 
+ */       
     }
 }