File content as of revision 2:5c8802f876f8:

#include "mbed.h"
#include "pca9685.h"

PCA9685::PCA9685(uint8_t i2c_addr, I2C* i2c_object, float freq):
    PCA9685_ADDR(i2c_addr << 1),
    frequency(freq),
    i2c(i2c_object)
{}

void PCA9685::reset(void){
    write_8(PCA9685_MODE1, 0x00); // reset the device
}

void PCA9685::init(void){
    reset();
    set_pwm_frequency(frequency);
}

void PCA9685::set_pwm_frequency(float freq){
    freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
    float prescaleval = 25000000;
    prescaleval /= 4096;
    prescaleval /= freq;
    prescaleval -= 1;
    char prescale = floor(prescaleval + 0.5);
    
    char oldmode = read_8(PCA9685_MODE1);
    char newmode = (oldmode&0x7F) | 0x10; // sleep
    
    write_8(PCA9685_MODE1, newmode); //go to sleep
    write_8(PCA9685_PRESCALE, prescale); // set the prescaler
    write_8(PCA9685_MODE1, oldmode);
    
    wait(0.5);
    
    write_8(PCA9685_MODE1, oldmode | 0xa1);  
}

void PCA9685::set_servo_angle(uint8_t num, float angle){
    if(angle > 180.0) angle = 180.0;
    if(angle < 0.0) angle = 0.0;
    
    // 0 ~ 180 degree to 1 ~ 2ms
    set_pwm_duty(num, (frequency*angle/180.0 + frequency)*0.1 );
}

void PCA9685::set_pwm_output(uint8_t num, uint16_t on, uint16_t off){
    char cmd[5];
    cmd[0] = PCA9685_LED0_ON_L + 4*num;
    cmd[1] = on % 256;
    cmd[2] = on / 256;
    cmd[3] = off % 256;
    cmd[4] = off / 256;    
    i2c->write(PCA9685_ADDR, cmd, 5);   
}

void PCA9685::set_pwm_duty(uint8_t num, float duty){
    if(duty > 100.0) duty = 100.0;
    if(duty < 0.0) duty = 0.0;
    
    uint16_t on = 0x0000;
    uint16_t off = (uint16_t)(duty * 40.95);
    
    set_pwm_output(num, on, off);
}

void PCA9685::write_8(uint8_t reg, uint8_t msg){
    char cmd[2];
    cmd[0] = reg;
    cmd[1] = msg;
    i2c->write(PCA9685_ADDR, cmd, 2);
}

char PCA9685::read_8(uint8_t reg){
    char cmd = reg;
    i2c->write(PCA9685_ADDR, &cmd, 1);
    i2c->read(PCA9685_ADDR, &cmd, 1);
    return cmd;    
}