Revision 3:bebfc64cefe4, committed 2013-11-09

Comitter:

mdraganic

Date:

Sat Nov 09 13:44:18 2013 +0000

Parent:

2:0bf41ad96558

Child:

4:03b68322905f

Commit message:

sve skupa i zajedno

Changed in this revision

--- a/MotorDrivers/Motor.cpp	Sat Nov 09 07:04:13 2013 +0000
+++ b/MotorDrivers/Motor.cpp	Sat Nov 09 13:44:18 2013 +0000
@@ -1,10 +1,15 @@
 #include "Motor.h"
 
-Motor::Motor() : positiveOut(NC), negativeOut(NC) {
-}
-
-Motor::Motor(PinName positivePin, PinName negativePin): 
-        positiveOut(positivePin), negativeOut(negativePin) {
+Motor::Motor(PinName positivePin, PinName negativePin, PinName pwmPin): 
+        positiveOut(positivePin), negativeOut(negativePin), pwmOut(pwmPin) {
+        
+    pwmOut.period(motorPwmPeriod);
+    pwmOut = motorPwmInitDutyCycle;
+    
+    positiveOut = 0;
+    negativeOut = 0;   
+    
+    _isMoving = false;
 }
 
 void Motor::movePositive() {
@@ -35,13 +40,32 @@
         break;
     }
     
-    direction = 0;
-    wait_ms(motorDriveTime);
+    for (float i = 1; i > 0; i -= motorPwmChangeSpeed) {
+        pwmOut = i;
+        wait(motorPwmWaitTime);
+    }        
+    pwmOut = 0;
+    _isMoving = true;
+    
+//    wait_ms(motorDriveTime);
+//    stop();
 }
 
 void Motor::stop() {
 
+    for (float i = 0; i < 1; i += motorPwmChangeSpeed) {
+        pwmOut = i;
+        wait(motorPwmWaitTime);
+    }
+    pwmOut = 1;
+
     positiveOut = 0;
     negativeOut = 0;
     direction = 0;
+    _isMoving = false;
 }
+
+bool Motor::isMoving() {
+
+    return _isMoving;
+}

--- a/MotorDrivers/Motor.h	Sat Nov 09 07:04:13 2013 +0000
+++ b/MotorDrivers/Motor.h	Sat Nov 09 13:44:18 2013 +0000
@@ -3,23 +3,27 @@
 
 #include "mbed.h"
 
-#define motorDriveTime 1000 // vrijeme koje se motor kreće, u milisekundama.
+#define motorDriveTime          100    // vrijeme koje se motor kreće, u milisekundama.
+#define motorPwmPeriod          0.010   // PWM period to 10 ms
+#define motorPwmInitDutyCycle   0.5     // PWM initial duty cycle, 50%
+#define motorPwmWaitTime        0.02    // PWM wait time in sec.
+#define motorPwmChangeSpeed     0.2     // PWM value change
 
 class Motor {
 
 private:
     DigitalOut positiveOut, negativeOut;
-    
-protected:
+    PwmOut pwmOut;
     short direction;    
-    Motor();
     void move();
+    bool _isMoving;
 
 public:
-    Motor(PinName, PinName);
+    Motor(PinName, PinName, PinName);
     void movePositive();
     void moveNegative();
-    void stop();    
+    void stop(); 
+    bool isMoving();   
     
 };
 

--- a/MotorDrivers/MotorPwm.cpp	Sat Nov 09 07:04:13 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,47 +0,0 @@
-#include "MotorPwm.h"
-
-MotorPwm::MotorPwm(PinName positivePin, PinName negativePin): 
-        positiveOutPwm(positivePin), negativeOutPwm(negativePin) {
-        
-    positiveOutPwm.period(motorPwmPeriod);
-    positiveOutPwm = motorPwmInitDutyCycle;
-    negativeOutPwm.period(motorPwmPeriod);
-    negativeOutPwm = motorPwmInitDutyCycle;
-}
-
-void MotorPwm::move() {
-
-    positiveOutPwm = 0;
-    negativeOutPwm = 0;
-    
-    switch(direction) {    
-    case 0:
-        return;
-    case 1:
-        for (int i = 0; i < 1; i += 0.1) {
-            positiveOutPwm = i;
-            wait(motorPwmWaitTime);
-        }    
-        break;
-    case -1:
-        for (int i = 0; i < 1; i += 0.1) {
-            negativeOutPwm = i;
-            wait(motorPwmWaitTime);
-        }    
-        break;
-    }
-    
-    direction = 0;
-    wait_ms(motorDriveTime);
-}
-
-void MotorPwm::stop() {
-
-    for (int i = 1; i > 0; i -= 0.1) {
-        positiveOutPwm = i;
-        negativeOutPwm = i;
-        wait(motorPwmWaitTime);
-    }
-    direction = 0;
-}
-

--- a/MotorDrivers/MotorPwm.h	Sat Nov 09 07:04:13 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#ifndef MOTORPWM_H
-#define MOTORPWM_H
-
-#include "mbed.h"
-#include "Motor.h"
-
-#define motorPwmPeriod 0.010 // PWM period to 10 ms
-#define motorPwmInitDutyCycle 0.5 // PWM initial duty cycle, 50%
-#define motorPwmWaitTime 0.05 // PWM wait time in sec.
-
-class MotorPwm : public Motor {
-
-private:
-    PwmOut positiveOutPwm, negativeOutPwm;
-    
-protected:
-    void move();
-
-public:
-    MotorPwm(PinName, PinName);
-    void stop();    
-    
-};
-
-
-
-
-
-#endif

--- 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(); 
+ */       
     }
 }