From 7352aab3b0f3d5b2c0d45078180d5f6d8c38aa49 Mon Sep 17 00:00:00 2001
From: Adelin Dobre <adelin.dobre1994@gmail.com>
Date: Fri, 29 Jun 2018 17:27:14 +0300
Subject: [PATCH] Adafruit ServoShield: Add string based constructor

Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
---
 src/adafruitss/adafruitss.cxx | 68 ++++++++++++++++++++++++++++++++++-
 src/adafruitss/adafruitss.hpp | 14 ++++++--
 2 files changed, 79 insertions(+), 3 deletions(-)

diff --git a/src/adafruitss/adafruitss.cxx b/src/adafruitss/adafruitss.cxx
index 302fa740..bf05bdc3 100644
--- a/src/adafruitss/adafruitss.cxx
+++ b/src/adafruitss/adafruitss.cxx
@@ -25,6 +25,7 @@
 #include <string>
 #include <stdexcept>
 
+#include "upm_string_parser.hpp"
 #include "adafruitss.hpp"
 #include <unistd.h>
 #include <math.h>
@@ -33,7 +34,7 @@ using namespace upm;
 
 adafruitss::adafruitss(int bus,int i2c_address)
 {
-    if ( !(m_i2c = mraa_i2c_init(bus)) ) 
+    if ( !(m_i2c = mraa_i2c_init(bus)) )
       {
         throw std::invalid_argument(std::string(__FUNCTION__) +
                                     ": mraa_i2c_init() failed");
@@ -62,6 +63,71 @@ adafruitss::adafruitss(int bus,int i2c_address)
     adafruitss::update();
 }
 
+adafruitss::adafruitss(std::string initStr) : mraaIo(initStr)
+{
+  mraa_io_descriptor* descs = mraaIo.getMraaDescriptors();
+
+  std::vector<std::string> upmTokens;
+
+  if (mraaIo.getLeftoverStr() != "")
+  {
+        upmTokens = UpmStringParser::parse(mraaIo.getLeftoverStr());
+  }
+
+  if(!descs->i2cs)
+  {
+    throw std::invalid_argument(std::string(__FUNCTION__) +
+                                    ": mraa_i2c_init() failed");
+    return;
+  }
+  else
+  {
+    if( !(m_i2c = descs->i2cs[0]) )
+    {
+        throw std::invalid_argument(std::string(__FUNCTION__) +
+                                    ": mraa_i2c_init() failed");
+      return;
+    }
+  }
+
+  m_rx_tx_buf[0]=PCA9685_MODE1;
+  m_rx_tx_buf[1]=0;
+  if (mraa_i2c_write(m_i2c,m_rx_tx_buf,2) != MRAA_SUCCESS)
+  {
+    throw std::invalid_argument(std::string(__FUNCTION__) +
+                                ": mraa_i2c_write() failed");
+    return;
+  }
+
+  adafruitss::setPWMFreq(60);
+
+  adafruitss::update();
+
+  std::string::size_type sz;
+  int old_sz;
+
+  for(std::string tok : upmTokens)
+  {
+    if(tok.substr(0,8) == "pwmFreq:")
+    {
+      float freq = std::stof(tok.substr(8));
+      setPWMFreq(freq);
+    }
+    if(tok.substr(0,6) == "servo:")
+    {
+      uint8_t port = std::stoi(tok.substr(6),&sz,0);
+      tok = tok.substr(6);
+      old_sz = sz+1;
+      uint8_t servo_type = std::stoi(tok.substr(old_sz),&sz,0);
+      tok = tok.substr(old_sz);
+      float degrees = std::stof(tok.substr(sz+1));
+      servo(port, servo_type, degrees);
+    }
+  }
+}
+
+
+
 void adafruitss::setPWMFreq(float freq) {
     float afreq= freq * 0.899683334F;  // Correct for overshoot in the frequency setting (see issue #11). (Tested at 60hz with Logic 4 for 50hz and 60hz)
     float prescaleval = 25000000;
diff --git a/src/adafruitss/adafruitss.hpp b/src/adafruitss/adafruitss.hpp
index bc1e531b..320c69e6 100644
--- a/src/adafruitss/adafruitss.hpp
+++ b/src/adafruitss/adafruitss.hpp
@@ -25,6 +25,7 @@
 #pragma once
 
 #include <mraa/i2c.h>
+#include <mraa/initio.hpp>
 
 #define MAX_BUFFER_LENGTH 6
 
@@ -69,7 +70,7 @@ namespace upm {
   * @web http://www.adafruit.com/product/1411
   * @con i2c
   *
-  * @brief API for the Adafruit Servo Shield 
+  * @brief API for the Adafruit Servo Shield
   *
   * UPM library for the PCA9685-based Adafruit 16-channel servo shield. If 3
   * or more GWS servos are attached, results could be unpredictable. Adafruit
@@ -89,6 +90,14 @@ namespace upm {
      * @param i2c_address Address of the servo shield on the I2C bus
      */
     adafruitss(int bus, int i2c_address);
+
+    /**
+     * Instantiates Adafruit PCA9685 object based on a given string.
+     *
+     * @param initStr string containing specific information for Adafruit PCA9685 initialization.
+     */
+    adafruitss(std::string initStr);
+
     int update(void);
     /**
      * Sets the frequency of the servos
@@ -109,9 +118,10 @@ namespace upm {
   private:
 
     int pca9685_addr;
+    mraa::MraaIo mraaIo;
     mraa_i2c_context m_i2c;
     uint8_t m_rx_tx_buf[MAX_BUFFER_LENGTH];
     float _duration_1ms;
-};
+  };
 
 }