Diff: pca9685.cpp

Revision:

0:c6fb5a9a8f91

Child:

1:7e071acc57b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pca9685.cpp	Mon Jan 16 04:54:27 2017 +0000
@@ -0,0 +1,79 @@
+#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)/1000.0 );
+}
+
+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 > 1.0) duty = 1.0;
+    if(duty < 0.0) duty = 0.0;
+    
+    uint16_t on = 0x0000;
+    uint16_t off = (uint16_t)(duty * 4095);
+    
+    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;    
+}
\ No newline at end of file