servo: removed defines from es08a and made engine move smoothly

Signed-off-by: Kiveisha Yevgeniy <yevgeniy.kiveisha@intel.com>

examples/es08a.cxx

@@ -28,38 +28,16 @@
 #include <signal.h>
 #include <stdlib.h>
 
-int running = 0;
-
-void
-sig_handler(int signo)
-{
-    printf("got signal\n");
-    if (signo == SIGINT) {
-        printf("exiting application\n");
-        running = 1;
-    }
-}
-
 int
 main(int argc, char **argv)
 {
     //! [Interesting]
     upm::ES08A *servo = new upm::ES08A(5);    
-
-    signal(SIGINT, sig_handler);
-
-    int clock = 0;
-    while (!running) {
-        for (int i = 0; i < 18; i++) {
-            servo->setAngle (clock);
-            clock += 10;
-        }
-
-        for (int i = 0; i < 18; i++) {
-            servo->setAngle (clock);
-            clock -= 10;
-        }
-    }
+    servo->setAngle (180);
+    servo->setAngle (90);
+    servo->setAngle (0);
+    servo->setAngle (90);
+    servo->setAngle (180);
     //! [Interesting]
 
     std::cout << "exiting application" << std::endl;

src/servo/es08a.cxx

@@ -32,6 +32,8 @@ using namespace upm;
 ES08A::ES08A (int pin) : Servo(pin) {
     m_name          = "ES08A";
     m_maxAngle      = 180.0;
+    m_minPulseWidth = 600;
+    m_maxPulseWidth = 2200;
 }
 
 ES08A::~ES08A() {

src/servo/es08a.h

@@ -28,9 +28,6 @@
 
 namespace upm {
 
-#define MIN_PULSE_WIDTH      600
-#define MAX_PULSE_WIDTH      2500
-
 /**
  * @brief C++ API for ES08A servo component
  *

src/servo/servo.cxx

@@ -25,6 +25,7 @@
 #include <iostream>
 #include <unistd.h>
 #include <stdlib.h>
+#include <math.h>
 
 #include "servo.h"
 
@@ -33,13 +34,21 @@ using namespace upm;
 Servo::Servo (int pin) {
     maa_result_t error = MAA_SUCCESS;
 
+    m_minPulseWidth   = MIN_PULSE_WIDTH;
+    m_maxPulseWidth   = MAX_PULSE_WIDTH;
+    m_maxPeriod       = MAX_PERIOD;
+
     m_maxAngle        = 180.0;
     m_servoPin        = pin;
     m_pwmServoContext = maa_pwm_init (m_servoPin);
+    
+    m_currAngle = 180;
+    
+    setAngle (0);
 }
 
 Servo::~Servo () {
-
+    maa_pwm_close (m_pwmServoContext);
 }
 
 /*
@@ -59,14 +68,26 @@ maa_result_t Servo::setAngle (int angle) {
         return MAA_ERROR_UNSPECIFIED;
     }
     
+    if (angle > m_maxAngle || angle < 0) {
+        return MAA_ERROR_UNSPECIFIED;
+    }
+
+    int period = (m_maxPulseWidth - m_minPulseWidth) / m_maxAngle;
+    
+    int cycles = (int)(100.0 * (abs (m_currAngle - angle) / m_maxAngle));
+
+    // int cycles = (int)(100.0 * ((float)angle / (float)m_maxAngle));
+
     maa_pwm_enable (m_pwmServoContext, 1);
-    for (int cycles = 0; cycles < 128; cycles++) {
-        maa_pwm_period_us (m_pwmServoContext, MAX_PERIOD);
+    for (int cycle = 0; cycle < cycles; cycle++) {
+        maa_pwm_period_us (m_pwmServoContext, m_maxPeriod);
         maa_pwm_pulsewidth_us (m_pwmServoContext, calcPulseTraveling(angle));
     }
     maa_pwm_enable (m_pwmServoContext, 0);
 
-    std::cout << "angle = " << angle << " ,pulse = " << calcPulseTraveling(angle) << std::endl;
+    std::cout << "angle = " << angle << " ,pulse = " << calcPulseTraveling(angle) << ", cycles " << cycles << std::endl;
+    
+    m_currAngle = angle;
 }
 
 /*
@@ -75,14 +96,44 @@ maa_result_t Servo::setAngle (int angle) {
 int Servo::calcPulseTraveling (int value) {
     // if bigger than the boundaries
     if (value > m_maxAngle) {
-        return MAX_PULSE_WIDTH;
+        return m_maxPulseWidth;
     }
 
     // if less than the boundaries
     if (value  < 0) {
-        return MIN_PULSE_WIDTH;
+        return m_minPulseWidth;
     }
 
     // the conversion
-    return (int) ((float)MIN_PULSE_WIDTH + ((float)value / m_maxAngle) * ((float)MAX_PULSE_WIDTH - (float)MIN_PULSE_WIDTH));
+    return (int) ((float)m_minPulseWidth + ((float)value / m_maxAngle) * ((float)m_maxPulseWidth - (float)m_minPulseWidth));
+}
+
+void 
+Servo::setMinPulseWidth (int width) {
+    m_minPulseWidth = width;
+}
+
+void 
+Servo::setMaxPulseWidth (int width) {
+    m_maxPulseWidth = width;
+}
+
+void 
+Servo::setMaxPeriod (int width) {
+    m_maxPeriod = width;
+}
+
+int 
+Servo::getMinPulseWidth () {
+    return m_minPulseWidth;
+}
+    
+int 
+Servo::getMaxPulseWidth () {
+    return m_maxPulseWidth;
+}
+
+int 
+Servo::getMaxPeriod () {
+    return m_maxPeriod;
 }

src/servo/servo.h

@@ -79,6 +79,43 @@ class Servo {
         {
             return m_name;
         }
+        
+        /**
+         * Set min pulse width
+         *
+         * @param width HIGH signal width
+         */
+        void setMinPulseWidth (int width);
+        
+        /**
+         * Set max pulse width
+         *
+         * @param width HIGH signal width
+         */
+        void setMaxPulseWidth (int width);
+        
+        /**
+         * Set max period width
+         *
+         * @param width PWM period width
+         */
+        void setMaxPeriod (int width);
+        
+        /**
+         * Return min pulse width
+         */
+        int getMinPulseWidth ();
+        
+        /**
+         * Return max pulse width
+         */
+        int getMaxPulseWidth ();
+        
+        /**
+         * Return max PWM period width
+         */
+        int getMaxPeriod ();
+        
     protected:
         int calcPulseTraveling (int value);
 
@@ -86,7 +123,11 @@ class Servo {
         int                 m_servoPin;
         float               m_maxAngle;
         maa_pwm_context     m_pwmServoContext;
-        // maa_gpio_context    m_servoPinCtx;
+        int                 m_currAngle;
+        
+        int                 m_minPulseWidth;
+        int                 m_maxPulseWidth;
+        int                 m_maxPeriod;
 };
 
 }