diff --git a/examples/c++/CMakeLists.txt b/examples/c++/CMakeLists.txt index 97ac2d1c..a8bf8326 100644 --- a/examples/c++/CMakeLists.txt +++ b/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}) diff --git a/examples/c++/pca9685ss.cxx b/examples/c++/pca9685ss.cxx new file mode 100644 index 00000000..c2d32898 --- /dev/null +++ b/examples/c++/pca9685ss.cxx @@ -0,0 +1,77 @@ +/* + * Author: Stan Gifford + * 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 +#include "pca9685ss.h" +#include + +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; +} diff --git a/src/pca9685/jsupm_pca9685ss.i b/src/pca9685/jsupm_pca9685ss.i new file mode 100644 index 00000000..3a7d79b4 --- /dev/null +++ b/src/pca9685/jsupm_pca9685ss.i @@ -0,0 +1,7 @@ +%module jsupm_pca9685 +%include "../upm.i" + +%include "pca9685ss.h" +%{ + #include "pca9685ss.h" +%} diff --git a/src/pca9685/pca9685ss.cxx b/src/pca9685/pca9685ss.cxx new file mode 100644 index 00000000..8a57554c --- /dev/null +++ b/src/pca9685/pca9685ss.cxx @@ -0,0 +1,152 @@ +/* + * Author: Stan Gifford + * 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 +#include + +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; +} diff --git a/src/pca9685/pca9685ss.h b/src/pca9685/pca9685ss.h new file mode 100644 index 00000000..097eeec0 --- /dev/null +++ b/src/pca9685/pca9685ss.h @@ -0,0 +1,105 @@ +/* + * Author: Stan Gifford + * 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 + +#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; +}; + +} diff --git a/src/pca9685/pyupm_pca9685ss.i b/src/pca9685/pyupm_pca9685ss.i new file mode 100644 index 00000000..f275b27c --- /dev/null +++ b/src/pca9685/pyupm_pca9685ss.i @@ -0,0 +1,9 @@ +%module pyupm_pca9685 +%include "../upm.i" + +%feature("autodoc", "3"); + +%include "pca9685ss.h" +%{ + #include "pca9685ss.h" +%}