alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-24 01:10:22 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

adafruitss: fix the period setting in setPWMFreq()

Browse Source 
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 <mihai.tudor.panu@intel.com>
This commit is contained in:
Jim Morris 2015-08-01 02:23:32 -07:00 committed by Mihai Tudor Panu
parent 197b56003c
commit 7260c78c33
2 changed files with 36 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
src/adafruitss/adafruitss.cxx
View File

@ -30,75 +30,69 @@ using namespace upm;
adafruitss::adafruitss(int bus,int i2c_address) adafruitss::adafruitss(int bus,int i2c_address)
{ {
int n;
int result;
mraa_init(); mraa_init();
m_i2c = mraa_i2c_init(bus); m_i2c = mraa_i2c_init(bus);
pca9685_addr = i2c_address; 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[0]=PCA9685_MODE1;
m_rx_tx_buf[1]=0; 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::setPWMFreq(60);
adafruitss::update(); adafruitss::update();
} }
void adafruitss::setPWMFreq(float freq) { void adafruitss::setPWMFreq(float freq) {
int result; 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)
freq *= 0.88; // Correct for overshoot in the frequency setting (see issue #11).
float prescaleval = 25000000; float prescaleval = 25000000;
prescaleval /= 4096; prescaleval /= 4096;
prescaleval /= freq; prescaleval /= afreq;
prescaleval -= 1; 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); mraa_i2c_address(m_i2c, pca9685_addr);
uint8_t oldmode=0; mraa_i2c_read_byte_data(m_i2c,PCA9685_MODE1);
oldmode = mraa_i2c_read_byte_data(m_i2c,PCA9685_MODE1);
m_rx_tx_buf[0]=PCA9685_MODE1; m_rx_tx_buf[0]=PCA9685_MODE1;
m_rx_tx_buf[1]=0x10; // sleep m_rx_tx_buf[1]=0x10; // sleep
result=mraa_i2c_address(m_i2c, pca9685_addr); mraa_i2c_address(m_i2c, pca9685_addr);
result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2); mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
m_rx_tx_buf[0]=PCA9685_PRESCALE; m_rx_tx_buf[0]=PCA9685_PRESCALE;
m_rx_tx_buf[1]=prescale; m_rx_tx_buf[1]=prescale;
result=mraa_i2c_address(m_i2c, pca9685_addr); mraa_i2c_address(m_i2c, pca9685_addr);
result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2); mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
m_rx_tx_buf[0]=PCA9685_MODE1; m_rx_tx_buf[0]=PCA9685_MODE1;
m_rx_tx_buf[1]=0x00; m_rx_tx_buf[1]=0x00;
result=mraa_i2c_address(m_i2c, pca9685_addr); mraa_i2c_address(m_i2c, pca9685_addr);
result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2); 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); usleep(5000);
m_rx_tx_buf[0]=PCA9685_MODE1; m_rx_tx_buf[0]=PCA9685_MODE1;
m_rx_tx_buf[1]=0xa1; m_rx_tx_buf[1]=0xa1;
result=mraa_i2c_address(m_i2c, pca9685_addr); mraa_i2c_address(m_i2c, pca9685_addr);
result=mraa_i2c_write(m_i2c,m_rx_tx_buf,2); mraa_i2c_write(m_i2c,m_rx_tx_buf,2);
} }
int adafruitss::update(void) int adafruitss::update(void)
@ -106,21 +100,19 @@ int adafruitss::update(void)
return MRAA_SUCCESS; return MRAA_SUCCESS;
} }
void adafruitss::servo(uint8_t port, uint8_t servo_type, float degrees) {
void adafruitss::servo(uint8_t port, uint8_t servo_type, uint16_t degrees) {
// Set Servo values // Set Servo values
// Degrees is from 0 to 180 // Degrees is from 0 to 180
// servo_type: 0 = standard 1ms to 2ms // servo_type: 0 = standard 1ms to 2ms
// 1 = extended 0.6ms to 2.4ms // 1 = extended 0.6ms to 2.4ms
// 2 = extended 0.8ms to 2.2ms // 2 = extended 0.8ms to 2.2ms
uint16_t duration = 0; float duration;
int result;
int r2;
if(degrees>180) degrees=180; // Ensure within bounds if(degrees>180) degrees=180; // Ensure within bounds
if (degrees<0) degrees=0; if (degrees<0) degrees=0;
switch (servo_type) { switch (servo_type) {
default:
case 0: // Standard Servo 1ms to 2ms case 0: // Standard Servo 1ms to 2ms
duration = _duration_1ms + ((_duration_1ms*degrees)/180); duration = _duration_1ms + ((_duration_1ms*degrees)/180);
break; 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); duration = (_duration_1ms*0.8) + ((_duration_1ms*degrees)/128);
break; break;
case 3: // Extended Servo 0.9ms to 2.1ms, - GWS Mini STD BB servo 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); duration = (_duration_1ms*0.9) + ((_duration_1ms*degrees)/120);
break; 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[0]=LED0_REG+4*port;
m_rx_tx_buf[1]=0; m_rx_tx_buf[1]=0;
m_rx_tx_buf[2]=0; m_rx_tx_buf[2]=0;
m_rx_tx_buf[3]=duration; m_rx_tx_buf[3]=d;
m_rx_tx_buf[4]=duration>>8; 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;
}

13
src/adafruitss/adafruitss.h
View File

@ -52,7 +52,6 @@
#define PCA9685_PRESCALE_REG 0xFE #define PCA9685_PRESCALE_REG 0xFE
#define LED0_REG 0x06 #define LED0_REG 0x06
namespace upm { namespace upm {
/** /**
@ -60,7 +59,7 @@ namespace upm {
* @defgroup adafruitss libupm-adafruitss * @defgroup adafruitss libupm-adafruitss
* @ingroup adafruit i2c servos * @ingroup adafruit i2c servos
*/ */
/** /**
* @library adafruitss * @library adafruitss
* @sensor adafruitss * @sensor adafruitss
@ -70,16 +69,18 @@ namespace upm {
* @web http://www.adafruit.com/product/1411 * @web http://www.adafruit.com/product/1411
* @con i2c * @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 * UPM library for the PCA9685 based Adafruit 16-channel servo shield. When 3
* or more GWS servos attached results unpredictable. Adafruit do recommend a * or more GWS servos attached results unpredictable. Adafruit do recommend a
* capacitor be installed on the board which should alleviate the issue. * capacitor be installed on the board which should alleviate the issue.
* Sizing depends on servos and count. * Sizing depends on servos and count.
* *
* @image html adafruitss.jpg * @image html adafruitss.jpg
* @snippet adafruitss.cxx Interesting * @snippet adafruitss.cxx Interesting
*/ */
class adafruitss { class adafruitss {
public: 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 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 * @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: private:
int pca9685_addr; int pca9685_addr;
mraa_i2c_context m_i2c; mraa_i2c_context m_i2c;
uint8_t m_rx_tx_buf[MAX_BUFFER_LENGTH]; uint8_t m_rx_tx_buf[MAX_BUFFER_LENGTH];
float _pwm_frequency;
float _duration_1ms; float _duration_1ms;
}; };