alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +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)
{
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;
}

13
src/adafruitss/adafruitss.h
View File

@ -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;
};