Revision 1:2e7d4aa6e79e, committed 2013-11-08

Comitter:

cvitas

Date:

Fri Nov 08 22:33:31 2013 +0000

Parent:

0:7447b8021b33

Commit message:

Version 1a

Changed in this revision

diff -r 7447b8021b33 -r 2e7d4aa6e79e Motor.cpp
--- a/Motor.cpp	Fri Nov 08 19:09:47 2013 +0000
+++ b/Motor.cpp	Fri Nov 08 22:33:31 2013 +0000
@@ -1,7 +1,7 @@
 #include "Motor.h"
 
-Motor::Motor(PinName positivePin, PinName negativePin): positiveOut(positivePin), negativeOut(negativePin) {
-
+Motor::Motor(PinName positivePin, PinName negativePin, PinName enablePin): positiveOut(positivePin), negativeOut(negativePin), enableOut(enablePin) {
+enableOut.period(0.010);  // set PWM period to 10 ms
 }
 
 void Motor::movePositive() {
@@ -10,12 +10,22 @@
     move();
 }
 
+void Motor::moveNegativeSlow() {
+
+    direction = -1;
+    moveslow();
+}
+void Motor::movePositiveSlow() {
+
+    direction = 1;
+    moveslow();
+}
+
 void Motor::moveNegative() {
 
     direction = -1;
     move();
 }
-
 void Motor::move() {
 
     positiveOut = 0;
@@ -34,10 +44,45 @@
     
     wait_ms(motorDriveTime);
 }
+void Motor::moveslow() {
 
+    positiveOut = 0;
+    negativeOut = 0;
+        
+    switch(direction) {    
+    case 0:
+        return;
+    case 1:
+        positiveOut = 1;
+        break;
+    case -1:
+        negativeOut = 1;
+        break;
+    }
+    for (int i=0; i<1; i=i+0.1) { // slow running start 0.5 s
+        enableOut= i;
+        wait(0.05);
+    }
+    wait_ms(motorDriveTime);
+}
+void Motor::initpwm() {
+
+    enableOut.period(0.010);  // set PWM period to 10 ms
+    enableOut = 0.5;  // set initial duty cycle to 50%
+}
 void Motor::stop() {
 
     positiveOut = 0;
     negativeOut = 0;
     direction = 0;
 }
+void Motor::stopslow() {
+
+    for (int i=1; i>0; i=i-0.1) {
+        enableOut= i;
+        wait(0.05);
+    }
+    positiveOut = 0;
+    negativeOut = 0;
+    direction = 0;
+}

diff -r 7447b8021b33 -r 2e7d4aa6e79e Motor.h
--- a/Motor.h	Fri Nov 08 19:09:47 2013 +0000
+++ b/Motor.h	Fri Nov 08 22:33:31 2013 +0000
@@ -9,15 +9,21 @@
 
 private:
     DigitalOut positiveOut, negativeOut;
+    PwmOut enableOut;
+    
+    void initpwm();
     void move();
+    void moveslow();
     short direction;
 
 public:
-    Motor(PinName, PinName);
+    Motor(PinName, PinName, PinName);
     void movePositive();
     void moveNegative();
     void stop();
-
+    void movePositiveSlow();
+    void moveNegativeSlow();
+    void stopslow();
 
 };
 

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