BLUESINK / pca9685

PCA9685 library

Dependents:   s-rov-firmware DISCO-F746NG_rtos_test

pca9685.cpp@2:5c8802f876f8, 2017-01-26 (annotated)

Committer:
YJ_Kim
Date:
Thu Jan 26 17:04:46 2017 +0000
Revision:
2:5c8802f876f8
Parent:
1:7e071acc57b1 
Change i2c object to pointer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YJ_Kim 0:c6fb5a9a8f91 1 #include "mbed.h"
YJ_Kim 0:c6fb5a9a8f91 2 #include "pca9685.h"
YJ_Kim 0:c6fb5a9a8f91 3
YJ_Kim 2:5c8802f876f8 4 PCA9685::PCA9685(uint8_t i2c_addr, I2C* i2c_object, float freq):
YJ_Kim 0:c6fb5a9a8f91 5 PCA9685_ADDR(i2c_addr << 1),
YJ_Kim 0:c6fb5a9a8f91 6 frequency(freq),
YJ_Kim 0:c6fb5a9a8f91 7 i2c(i2c_object)
YJ_Kim 0:c6fb5a9a8f91 8 {}
YJ_Kim 0:c6fb5a9a8f91 9
YJ_Kim 0:c6fb5a9a8f91 10 void PCA9685::reset(void){
YJ_Kim 0:c6fb5a9a8f91 11 write_8(PCA9685_MODE1, 0x00); // reset the device
YJ_Kim 0:c6fb5a9a8f91 12 }
YJ_Kim 0:c6fb5a9a8f91 13
YJ_Kim 0:c6fb5a9a8f91 14 void PCA9685::init(void){
YJ_Kim 0:c6fb5a9a8f91 15 reset();
YJ_Kim 0:c6fb5a9a8f91 16 set_pwm_frequency(frequency);
YJ_Kim 0:c6fb5a9a8f91 17 }
YJ_Kim 0:c6fb5a9a8f91 18
YJ_Kim 0:c6fb5a9a8f91 19 void PCA9685::set_pwm_frequency(float freq){
YJ_Kim 0:c6fb5a9a8f91 20 freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11).
YJ_Kim 0:c6fb5a9a8f91 21 float prescaleval = 25000000;
YJ_Kim 0:c6fb5a9a8f91 22 prescaleval /= 4096;
YJ_Kim 0:c6fb5a9a8f91 23 prescaleval /= freq;
YJ_Kim 0:c6fb5a9a8f91 24 prescaleval -= 1;
YJ_Kim 0:c6fb5a9a8f91 25 char prescale = floor(prescaleval + 0.5);
YJ_Kim 0:c6fb5a9a8f91 26
YJ_Kim 0:c6fb5a9a8f91 27 char oldmode = read_8(PCA9685_MODE1);
YJ_Kim 0:c6fb5a9a8f91 28 char newmode = (oldmode&0x7F) | 0x10; // sleep
YJ_Kim 0:c6fb5a9a8f91 29
YJ_Kim 0:c6fb5a9a8f91 30 write_8(PCA9685_MODE1, newmode); //go to sleep
YJ_Kim 0:c6fb5a9a8f91 31 write_8(PCA9685_PRESCALE, prescale); // set the prescaler
YJ_Kim 0:c6fb5a9a8f91 32 write_8(PCA9685_MODE1, oldmode);
YJ_Kim 0:c6fb5a9a8f91 33
YJ_Kim 0:c6fb5a9a8f91 34 wait(0.5);
YJ_Kim 0:c6fb5a9a8f91 35
YJ_Kim 0:c6fb5a9a8f91 36 write_8(PCA9685_MODE1, oldmode | 0xa1);
YJ_Kim 0:c6fb5a9a8f91 37 }
YJ_Kim 0:c6fb5a9a8f91 38
YJ_Kim 0:c6fb5a9a8f91 39 void PCA9685::set_servo_angle(uint8_t num, float angle){
YJ_Kim 0:c6fb5a9a8f91 40 if(angle > 180.0) angle = 180.0;
YJ_Kim 0:c6fb5a9a8f91 41 if(angle < 0.0) angle = 0.0;
YJ_Kim 0:c6fb5a9a8f91 42
YJ_Kim 0:c6fb5a9a8f91 43 // 0 ~ 180 degree to 1 ~ 2ms
YJ_Kim 1:7e071acc57b1 44 set_pwm_duty(num, (frequency*angle/180.0 + frequency)*0.1 );
YJ_Kim 0:c6fb5a9a8f91 45 }
YJ_Kim 0:c6fb5a9a8f91 46
YJ_Kim 0:c6fb5a9a8f91 47 void PCA9685::set_pwm_output(uint8_t num, uint16_t on, uint16_t off){
YJ_Kim 0:c6fb5a9a8f91 48 char cmd[5];
YJ_Kim 0:c6fb5a9a8f91 49 cmd[0] = PCA9685_LED0_ON_L + 4*num;
YJ_Kim 0:c6fb5a9a8f91 50 cmd[1] = on % 256;
YJ_Kim 0:c6fb5a9a8f91 51 cmd[2] = on / 256;
YJ_Kim 0:c6fb5a9a8f91 52 cmd[3] = off % 256;
YJ_Kim 0:c6fb5a9a8f91 53 cmd[4] = off / 256;
YJ_Kim 2:5c8802f876f8 54 i2c->write(PCA9685_ADDR, cmd, 5);
YJ_Kim 0:c6fb5a9a8f91 55 }
YJ_Kim 0:c6fb5a9a8f91 56
YJ_Kim 0:c6fb5a9a8f91 57 void PCA9685::set_pwm_duty(uint8_t num, float duty){
YJ_Kim 1:7e071acc57b1 58 if(duty > 100.0) duty = 100.0;
YJ_Kim 0:c6fb5a9a8f91 59 if(duty < 0.0) duty = 0.0;
YJ_Kim 0:c6fb5a9a8f91 60
YJ_Kim 0:c6fb5a9a8f91 61 uint16_t on = 0x0000;
YJ_Kim 1:7e071acc57b1 62 uint16_t off = (uint16_t)(duty * 40.95);
YJ_Kim 0:c6fb5a9a8f91 63
YJ_Kim 0:c6fb5a9a8f91 64 set_pwm_output(num, on, off);
YJ_Kim 0:c6fb5a9a8f91 65 }
YJ_Kim 0:c6fb5a9a8f91 66
YJ_Kim 0:c6fb5a9a8f91 67 void PCA9685::write_8(uint8_t reg, uint8_t msg){
YJ_Kim 0:c6fb5a9a8f91 68 char cmd[2];
YJ_Kim 0:c6fb5a9a8f91 69 cmd[0] = reg;
YJ_Kim 0:c6fb5a9a8f91 70 cmd[1] = msg;
YJ_Kim 2:5c8802f876f8 71 i2c->write(PCA9685_ADDR, cmd, 2);
YJ_Kim 0:c6fb5a9a8f91 72 }
YJ_Kim 0:c6fb5a9a8f91 73
YJ_Kim 0:c6fb5a9a8f91 74 char PCA9685::read_8(uint8_t reg){
YJ_Kim 0:c6fb5a9a8f91 75 char cmd = reg;
YJ_Kim 2:5c8802f876f8 76 i2c->write(PCA9685_ADDR, &cmd, 1);
YJ_Kim 2:5c8802f876f8 77 i2c->read(PCA9685_ADDR, &cmd, 1);
YJ_Kim 0:c6fb5a9a8f91 78 return cmd;
YJ_Kim 0:c6fb5a9a8f91 79 }