pca9685ss: Initial implementation

Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>

examples/c++/CMakeLists.txt

@@ -100,6 +100,7 @@ add_executable (at42qt1070-example at42qt1070.cxx)
 add_executable (grovemd-example grovemd.cxx)
 add_executable (pca9685-example pca9685.cxx)
 add_executable (groveeldriver-example groveeldriver.cxx)
+add_executable (pca9685ss-example pca9685ss.cxx)
 
 include_directories (${PROJECT_SOURCE_DIR}/src/hmc5883l)
 include_directories (${PROJECT_SOURCE_DIR}/src/grove)
@@ -282,3 +283,4 @@ target_link_libraries (at42qt1070-example at42qt1070 ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (grovemd-example grovemd ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (pca9685-example pca9685 ${CMAKE_THREAD_LIBS_INIT})
 target_link_libraries (groveeldriver-example groveeldriver ${CMAKE_THREAD_LIBS_INIT})
+target_link_libraries (pca9685ss-example pca9685 ${CMAKE_THREAD_LIBS_INIT})

examples/c++/pca9685ss.cxx

@@ -0,0 +1,77 @@
+/*
+ * Author: Stan Gifford <stan@gifford.id.au>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+/**
+ * Description
+ * Demo program for Adafruit 16 channel servo shield/controller
+ * Physical setup for tests is a single servo attached to one channel.
+ * Note - when 3 or more GWS servos attached results unpredictable.
+ * Adafruit do recommend a Cap be installed on the board which should alleviate the issue.
+ * I (and Adafruit) are unable to give any Capacitor sizing data.
+ */
+
+#include <iostream>
+#include "pca9685ss.h"
+#include <unistd.h>
+
+using namespace std;
+
+int main() {
+
+int n;
+
+//! [Interesting]
+  upm::pca9685ss* servos = new upm::pca9685ss(6,0x40);
+
+  for (;;)
+  {
+    cout << "Setting all to 0" << endl;
+    for (n = 0; n < 16; n++)
+        servos->servo(n, 1, 0); // GWS Mini Servo = Type 1.
+    usleep(1000000); // Wait 1 second
+
+    cout << "Setting all to 45" << endl;
+    for (n = 0; n < 16; n++)
+        servos->servo(n, 1, 45);
+    usleep(1000000); // Wait 1 second
+
+    cout << "Setting all to 90" << endl;
+    for (n = 0; n < 16; n++)
+        servos->servo(n, 1, 90);
+    usleep(1000000); // Wait 1 second
+
+    cout << "Setting all to 135" << endl;
+    for (n = 0; n < 16; n++)
+        servos->servo(n, 1, 135);
+    usleep(1000000); // Wait 1 second
+
+    cout << "Setting all to 180" << endl;
+    for (n = 0; n < 16; n++)
+        servos->servo(n, 1, 160);
+    usleep(2000000); // Wait 1 second
+  }
+//! [Interesting]
+  return 0;
+}

src/pca9685/jsupm_pca9685ss.i

@@ -0,0 +1,7 @@
+%module jsupm_pca9685
+%include "../upm.i"
+
+%include "pca9685ss.h"
+%{
+    #include "pca9685ss.h"
+%}

src/pca9685/pca9685ss.cxx

@@ -0,0 +1,152 @@
+/*
+ * Author: Stan Gifford <stan@gifford.id.au>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "pca9685ss.h"
+#include <unistd.h>
+#include <math.h>
+
+using namespace upm;
+
+pca9685ss::pca9685ss(int bus,int i2c_address)
+{
+    int n;
+    int result;
+
+    mraa_init();
+
+
+    m_i2c = mraa_i2c_init(bus);
+
+    pca9685_addr =  i2c_address;
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    m_rx_tx_buf[0]=PCA9685_MODE1;
+    m_rx_tx_buf[1]=0;
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
+
+    pca9685ss::setPWMFreq(60);
+
+
+    pca9685ss::update();
+}
+
+void pca9685ss::setPWMFreq(float freq) {
+    int result;
+    freq *= 0.88;  // Correct for overshoot in the frequency setting (see issue #11).
+    float prescaleval = 25000000;
+    prescaleval /= 4096;
+    prescaleval /= freq;
+    prescaleval -= 1;
+    _pwm_frequency = 60.18; // FInal achieved frequency measured with Logic 8!
+
+    _duration_1ms = ((4096*_pwm_frequency)/1000);  // This is 1ms duration
+
+    uint8_t prescale = floor(prescaleval + 0.5);
+
+
+
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    uint8_t oldmode=0;
+    oldmode = mraa_i2c_read_byte_data(m_i2c,PCA9685_MODE1);
+
+
+    m_rx_tx_buf[0]=PCA9685_MODE1;
+    m_rx_tx_buf[1]=0x10; // sleep
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
+
+
+
+    m_rx_tx_buf[0]=PCA9685_PRESCALE;
+    m_rx_tx_buf[1]=prescale;
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
+
+
+
+
+    m_rx_tx_buf[0]=PCA9685_MODE1;
+    m_rx_tx_buf[1]=0x00;
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
+
+    // result=mraa_i2c_write_byte_data(m_i2c,0x00,PCA9685_MODE1);
+
+    usleep(5000);
+
+
+    m_rx_tx_buf[0]=PCA9685_MODE1;
+    m_rx_tx_buf[1]=0xa1;
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
+}
+
+int pca9685ss::update(void)
+{
+    return MRAA_SUCCESS;
+}
+
+
+void pca9685ss::servo(uint8_t port, uint8_t servo_type, uint16_t degrees) {
+    // Set Servo values
+    // Degrees is from 0 to 180
+    // servo_type: 0 = standard 1ms to 2ms
+    //             1 = extended 0.6ms to 2.4ms
+    //             2 = extended 0.8ms to 2.2ms
+
+    uint16_t duration;
+    int result;
+    int r2;
+
+    if(degrees>180) degrees=180;        // Ensure within bounds
+    if (degrees<0) degrees=0;
+    switch (servo_type) {
+      case 0:              // Standard Servo 1ms to 2ms
+         duration = _duration_1ms + ((_duration_1ms*degrees)/180);
+         break;
+
+      case 1:              // Extended Servo 0.6ms to 2.4ms, i.e. 1.8ms from 0 to 180
+         //duration = (_duration_1ms*0.6) + ((_duration_1ms*1.8*degrees)/180); simplified to..
+         duration = (_duration_1ms*0.6) + ((_duration_1ms*degrees)/100);
+         break;
+
+      case 2:              // Extended Servo 0.8ms to 2.2ms, i.e. 1.4ms from 0 to 180
+         //duration = (_duration_1ms*0.8) + ((_duration_1ms*1.4*degrees)/180); simplified to..
+         duration = (_duration_1ms*0.8) + ((_duration_1ms*degrees)/128);
+         break;
+      case 3:              // Extended Servo 0.9ms to 2.1ms,  - GWS Mini STD BB servo
+         //duration = (_duration_1ms*0.8) + ((_duration_1ms*1.4*degrees)/180); simplified to..
+         duration = (_duration_1ms*0.9) + ((_duration_1ms*degrees)/120);
+         break;
+   }
+
+    result=mraa_i2c_address(m_i2c, pca9685_addr);
+    m_rx_tx_buf[0]=LED0_REG+4*port;
+    m_rx_tx_buf[1]=0;
+    m_rx_tx_buf[2]=0;
+    m_rx_tx_buf[3]=duration;
+    m_rx_tx_buf[4]=duration>>8;
+
+    result=mraa_i2c_write(m_i2c,m_rx_tx_buf,5);
+    r2=result;
+}

src/pca9685/pca9685ss.h

@@ -0,0 +1,105 @@
+/*
+ * Author: Stan Gifford <stan@gifford.id.au>
+ * Copyright (c) 2015 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <mraa/i2c.h>
+
+#define MAX_BUFFER_LENGTH 6
+
+#define SERVO_MIN       0
+#define SERVO_MAX       180
+#define SERVO_FREQ      60
+#define PCA9685_SUBADR1 0x2
+#define PCA9685_SUBADR2 0x3
+#define PCA9685_SUBADR3 0x4
+
+#define PCA9685_MODE1    0x0
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L  0x6
+#define LED0_ON_H  0x7
+#define LED0_OFF_L 0x8
+#define LED0_OFF_H 0x9
+
+#define ALLLED_ON_L  0xFA
+#define ALLLED_ON_H  0xFB
+#define ALLLED_OFF_L 0xFC
+#define ALLLED_OFF_H 0xFD
+
+#define PCA9685_MODE1_REG       0x00
+#define PCA9685_PRESCALE_REG    0xFE
+#define LED0_REG                0x06
+
+
+namespace upm {
+
+ /**
+ * @brief pca9685 based servo controller
+ * @defgroup pca9685 libupm-pca9685
+ */
+ 
+ /**
+  * @brief
+  * UPM library for pca9685 Adafruit 16-channel servo shield/controller. When 3 or more GWS servos attached results unpredictable.
+  * Adafruit do recommend a capacitor be installed on the board which should alleviate the issue. Sizing depends on servos and count.
+  *
+  * @ingroup i2c
+  * @snippet pca9685ss.cxx Interesting
+  */
+  class pca9685ss {
+  public:
+    /**
+     * Creates a pca9685ss object
+     *
+     * @param bus number of used i2c bus
+     * @param i2c_address address of servo controller on i2c bus
+     */
+    pca9685ss(int bus, int i2c_address);
+    int update(void);
+    /**
+     * Sets the frequency for your servos
+     *
+     * @param freq the frequency at which the servos operate
+     */
+    void setPWMFreq(float freq);
+    /**
+     * Moves the one of the servos to the specified angle
+     *
+     * @param port port of the servo on the controller (servo number)
+     * @param servo_type can be 0 = standard 1ms to 2ms, 1 = extended 0.6ms to 2.4ms, or 2 = extended 0.8ms to 2.2ms
+     * @param degrees angle to set the servo to
+     */
+    void servo(uint8_t port, uint8_t servo_type, uint16_t degrees);
+
+  private:
+
+    int pca9685_addr;
+    mraa_i2c_context m_i2c;
+    uint8_t m_rx_tx_buf[MAX_BUFFER_LENGTH];
+    float _pwm_frequency;
+    float _duration_1ms;
+};
+
+}

src/pca9685/pyupm_pca9685ss.i

@@ -0,0 +1,9 @@
+%module pyupm_pca9685
+%include "../upm.i"
+
+%feature("autodoc", "3");
+
+%include "pca9685ss.h"
+%{
+    #include "pca9685ss.h"
+%}