From 7260c78c335981f1190eb166373b55f9102588d4 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sat, 1 Aug 2015 02:23:32 -0700 Subject: [PATCH] adafruitss: fix the period setting in setPWMFreq() Tweak the fudge factor so it is more accurate for 50Hz and 60Hz periods. (Tested on Logic 4 and it is spot on now) Get rid of compiler warnings when -Wall is set Allow for float angle to be passed into the servo() method for more accurate positioning Signed-off-by: Mihai Tudor Panu --- src/adafruitss/adafruitss.cxx | 65 ++++++++++++++++------------------- src/adafruitss/adafruitss.h | 13 +++---- 2 files changed, 36 insertions(+), 42 deletions(-) diff --git a/src/adafruitss/adafruitss.cxx b/src/adafruitss/adafruitss.cxx index 75b71673..76da60d5 100644 --- a/src/adafruitss/adafruitss.cxx +++ b/src/adafruitss/adafruitss.cxx @@ -30,75 +30,69 @@ using namespace upm; adafruitss::adafruitss(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); + 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); + mraa_i2c_write(m_i2c,m_rx_tx_buf,2); adafruitss::setPWMFreq(60); - adafruitss::update(); } void adafruitss::setPWMFreq(float freq) { - int result; - freq *= 0.88; // Correct for overshoot in the frequency setting (see issue #11). + 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; prescaleval /= 4096; - prescaleval /= freq; + prescaleval /= afreq; prescaleval -= 1; - _pwm_frequency = 60.18; // FInal achieved frequency measured with Logic 8! + float pwm_frequency = freq; // Use actual requested frequency gives the correct pulse width - _duration_1ms = ((4096*_pwm_frequency)/1000); // This is 1ms duration + _duration_1ms = ((4096*pwm_frequency)/1000); // This is 1ms duration - uint8_t prescale = floor(prescaleval + 0.5); + uint8_t prescale = roundf(prescaleval); - result=mraa_i2c_address(m_i2c, pca9685_addr); - uint8_t oldmode=0; - oldmode = mraa_i2c_read_byte_data(m_i2c,PCA9685_MODE1); + mraa_i2c_address(m_i2c, pca9685_addr); + 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); + mraa_i2c_address(m_i2c, pca9685_addr); + 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); + mraa_i2c_address(m_i2c, pca9685_addr); + 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); + mraa_i2c_address(m_i2c, pca9685_addr); + mraa_i2c_write(m_i2c,m_rx_tx_buf,2); - // result=mraa_i2c_write_byte_data(m_i2c,0x00,PCA9685_MODE1); + // 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); + mraa_i2c_address(m_i2c, pca9685_addr); + mraa_i2c_write(m_i2c,m_rx_tx_buf,2); } int adafruitss::update(void) @@ -106,21 +100,19 @@ int adafruitss::update(void) return MRAA_SUCCESS; } - -void adafruitss::servo(uint8_t port, uint8_t servo_type, uint16_t degrees) { +void adafruitss::servo(uint8_t port, uint8_t servo_type, float 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 = 0; - int result; - int r2; + float duration; if(degrees>180) degrees=180; // Ensure within bounds if (degrees<0) degrees=0; switch (servo_type) { + default: case 0: // Standard Servo 1ms to 2ms duration = _duration_1ms + ((_duration_1ms*degrees)/180); break; @@ -135,18 +127,19 @@ void adafruitss::servo(uint8_t port, uint8_t servo_type, uint16_t degrees) { 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*1.4*degrees)/180); simplified to.. duration = (_duration_1ms*0.9) + ((_duration_1ms*degrees)/120); break; } - result=mraa_i2c_address(m_i2c, pca9685_addr); + uint16_t d= roundf(duration); + 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; + m_rx_tx_buf[3]=d; + m_rx_tx_buf[4]=d>>8; + + mraa_i2c_write(m_i2c,m_rx_tx_buf,5); + } - result=mraa_i2c_write(m_i2c,m_rx_tx_buf,5); - r2=result; -} diff --git a/src/adafruitss/adafruitss.h b/src/adafruitss/adafruitss.h index db259e72..9d8a2855 100644 --- a/src/adafruitss/adafruitss.h +++ b/src/adafruitss/adafruitss.h @@ -52,7 +52,6 @@ #define PCA9685_PRESCALE_REG 0xFE #define LED0_REG 0x06 - namespace upm { /** @@ -60,7 +59,7 @@ namespace upm { * @defgroup adafruitss libupm-adafruitss * @ingroup adafruit i2c servos */ - + /** * @library adafruitss * @sensor adafruitss @@ -70,16 +69,18 @@ namespace upm { * @web http://www.adafruit.com/product/1411 * @con i2c * - * @brief API for Adafruit Servo Shield + * + * @brief API for Adafruit Servo Shield * * UPM library for the PCA9685 based Adafruit 16-channel servo shield. 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. * - * @image html adafruitss.jpg + * @image html adafruitss.jpg * @snippet adafruitss.cxx Interesting */ + class adafruitss { public: /** @@ -103,14 +104,14 @@ namespace upm { * @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); + void servo(uint8_t port, uint8_t servo_type, float degrees); + void servo(uint8_t port, uint8_t servo_type, uint16_t degrees) { servo(port, servo_type, (float)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; };