Revision 4:03b68322905f, committed 2013-11-09

Comitter:

mdraganic

Date:

Sat Nov 09 18:35:13 2013 +0000

Parent:

3:bebfc64cefe4

Child:

5:7e28f4b64a55

Commit message:

popravljena logika upravljanja motorima

Changed in this revision

--- a/MotorDrivers/Motor.cpp	Sat Nov 09 13:44:18 2013 +0000
+++ b/MotorDrivers/Motor.cpp	Sat Nov 09 18:35:13 2013 +0000
@@ -8,51 +8,22 @@
     
     positiveOut = 0;
     negativeOut = 0;   
-    
-    _isMoving = false;
-}
-
-void Motor::movePositive() {
-
-    direction = 1;
-    move();
-}
-
-void Motor::moveNegative() {
-
-    direction = -1;
-    move();
 }
 
-void Motor::move() {
-
-    positiveOut = 0;
-    negativeOut = 0;
-        
-    switch(direction) {    
-    case 0:
-        return;
-    case 1:
-        positiveOut = 1;
-        break;
-    case -1:
-        negativeOut = 1;
-        break;
-    }
+void Motor::start() {
     
     for (float i = 1; i > 0; i -= motorPwmChangeSpeed) {
         pwmOut = i;
         wait(motorPwmWaitTime);
     }        
     pwmOut = 0;
-    _isMoving = true;
-    
-//    wait_ms(motorDriveTime);
-//    stop();
 }
 
 void Motor::stop() {
 
+    if(direction == 0)
+        return;
+
     for (float i = 0; i < 1; i += motorPwmChangeSpeed) {
         pwmOut = i;
         wait(motorPwmWaitTime);
@@ -62,10 +33,35 @@
     positiveOut = 0;
     negativeOut = 0;
     direction = 0;
-    _isMoving = false;
+}
+
+void Motor::movePositive() {
+
+    if(direction == 1)
+        return;
+
+    if(direction == -1)
+        stop();
+        
+    positiveOut = 1;
+    direction = 1;
+    start();
+}
+
+void Motor::moveNegative() {
+
+    if(direction == -1)
+        return;
+
+    if(direction == 1)
+        stop();
+        
+    negativeOut = 1;
+    direction = -1;
+    start();
 }
 
 bool Motor::isMoving() {
 
-    return _isMoving;
+    return direction == 0;
 }

--- a/MotorDrivers/Motor.h	Sat Nov 09 13:44:18 2013 +0000
+++ b/MotorDrivers/Motor.h	Sat Nov 09 18:35:13 2013 +0000
@@ -15,8 +15,7 @@
     DigitalOut positiveOut, negativeOut;
     PwmOut pwmOut;
     short direction;    
-    void move();
-    bool _isMoving;
+    void start();
 
 public:
     Motor(PinName, PinName, PinName);

--- a/main.cpp	Sat Nov 09 13:44:18 2013 +0000
+++ b/main.cpp	Sat Nov 09 18:35:13 2013 +0000
@@ -9,6 +9,9 @@
 #define calibrateFactorSenzC 1.06
 #define calibrateFactorSenzD 0.9
 
+#define serialOutputEnable      true
+#define serialOutputBaudrate    115200
+
 Serial pc(USBTX, USBRX);
 
 AnalogIn ainSensA(p17);
@@ -46,13 +49,19 @@
     valAzimut = (SensA + SensB) - (SensC + SensD); 
     valElevacija = (SensB + SensC) - (SensA + SensD);
     
+    if(serialOutputEnable) {
+    
 //    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);
+
+        pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija);
+    }
 }
 
 int main() {
 
+    pc.baud(serialOutputBaudrate);
+
     Motor *motorAz = new Motor(p25, p26, p24);
     Motor *motorEl = new Motor(p22, p21, p23);
 
@@ -60,90 +69,35 @@
     
         readValuesForAveraging();
         
-        bool someWrokDone = false;
-        
 // ----- azimut -----
         
-        if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) {
+        if(abs(valAzimut) > sensorStartTreshold) {
             if(valAzimut > 0) 
                 (*motorAz).movePositive();
             else
                 (*motorAz).moveNegative();
-            someWrokDone = true;
         }
         
-        if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
+        if(abs(valAzimut) < sensorStopTreshold) {
             (*motorAz).stop();
-            someWrokDone = true;
         }
         
 // ----- elevacija -----
 
-        if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) {
+        if(abs(valElevacija) > sensorStartTreshold) {
             if(valElevacija > 0) 
                 (*motorEl).movePositive();
             else
                 (*motorEl).moveNegative();
-            someWrokDone = true;
-        }
-        
-        if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
-            (*motorEl).stop();
-            someWrokDone = true;
-        }
-
-// ---- ništa nije dirano ----
-
-        if(!someWrokDone)
-            wait_ms(500);
-        
-        
-        
-        
-        /*
-        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;        
-        }
-        
-        if(valElevacija < (-1 * sensorStartTreshold)) {
-            if(!(*motorEl).isMoving())
-                (*motorEl).moveNegative();
-            continue;        
+        if(abs(valElevacija) < sensorStopTreshold) {
+            (*motorEl).stop();
         }
         
-        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");            
-        }
- */       
+// ----- pauza ------
+
+        wait_ms(100);        
     }
 }