Shundo Kishi / Adafruit-PWM-Servo-Driver

Library for 16 channel PWM servo driver PCA9685.

Dependents:   Adafruit-PWM-Servo-Driver_sample Hobby_Humanoid_controlor Servos Adafruit-MortorShield_sample ... more

Adafruit_PWMServoDriver.cpp@2:88bdd5c4e77b, 2013-09-04 (annotated)

Committer:
syundo0730
Date:
Wed Sep 04 03:58:53 2013 +0000
Revision:
2:88bdd5c4e77b
Parent:
1:69033d5e289b 
modified about duty handling

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:9d01b5d37adc 1 /***************************************************
syundo0730 0:9d01b5d37adc 2 This is a library for our Adafruit 16-channel PWM & Servo driver
syundo0730 0:9d01b5d37adc 3
syundo0730 0:9d01b5d37adc 4 Pick one up today in the adafruit shop!
syundo0730 0:9d01b5d37adc 5 ------> http://www.adafruit.com/products/815
syundo0730 0:9d01b5d37adc 6
syundo0730 0:9d01b5d37adc 7 These displays use I2C to communicate, 2 pins are required to
syundo0730 0:9d01b5d37adc 8 interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4
syundo0730 0:9d01b5d37adc 9
syundo0730 0:9d01b5d37adc 10 Adafruit invests time and resources providing this open source code,
syundo0730 0:9d01b5d37adc 11 please support Adafruit and open-source hardware by purchasing
syundo0730 0:9d01b5d37adc 12 products from Adafruit!
syundo0730 0:9d01b5d37adc 13
syundo0730 0:9d01b5d37adc 14 Written by Limor Fried/Ladyada for Adafruit Industries.
syundo0730 0:9d01b5d37adc 15 BSD license, all text above must be included in any redistribution
syundo0730 0:9d01b5d37adc 16 ****************************************************/
syundo0730 0:9d01b5d37adc 17
syundo0730 0:9d01b5d37adc 18 /*****************************
syundo0730 0:9d01b5d37adc 19 This program was ported from https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library.
syundo0730 0:9d01b5d37adc 20 I also added some functions.
syundo0730 0:9d01b5d37adc 21 Shundo Kishi
syundo0730 0:9d01b5d37adc 22 *****************************/
syundo0730 0:9d01b5d37adc 23
syundo0730 0:9d01b5d37adc 24 #include "Adafruit_PWMServoDriver.h"
syundo0730 0:9d01b5d37adc 25
syundo0730 0:9d01b5d37adc 26 Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(PinName sda, PinName scl, int addr) : i2c(sda, scl), _i2caddr(addr) {}
syundo0730 0:9d01b5d37adc 27
syundo0730 0:9d01b5d37adc 28 void Adafruit_PWMServoDriver::i2c_probe(void)
syundo0730 0:9d01b5d37adc 29 {
syundo0730 0:9d01b5d37adc 30 printf("Searching for I2C devices...\n");
syundo0730 0:9d01b5d37adc 31
syundo0730 0:9d01b5d37adc 32 int count = 0;
syundo0730 0:9d01b5d37adc 33 for (int address=4; address<256; address+=2) {
syundo0730 0:9d01b5d37adc 34 if (!i2c.write(address, NULL, 0)) { // 0 returned is ok
syundo0730 0:9d01b5d37adc 35 printf(" - I2C device found at address 0x%02X\r\n", address);
syundo0730 0:9d01b5d37adc 36 count++;
syundo0730 0:9d01b5d37adc 37 }
syundo0730 0:9d01b5d37adc 38 }
syundo0730 0:9d01b5d37adc 39 printf("%d devices found\r\n", count);
syundo0730 0:9d01b5d37adc 40 }
syundo0730 0:9d01b5d37adc 41
syundo0730 0:9d01b5d37adc 42 void Adafruit_PWMServoDriver::begin(void) {
syundo0730 0:9d01b5d37adc 43 reset();
syundo0730 0:9d01b5d37adc 44 }
syundo0730 0:9d01b5d37adc 45
syundo0730 0:9d01b5d37adc 46 void Adafruit_PWMServoDriver::setI2Cfreq(int freq) {
syundo0730 0:9d01b5d37adc 47 i2c.frequency(freq);
syundo0730 0:9d01b5d37adc 48 }
syundo0730 0:9d01b5d37adc 49
syundo0730 0:9d01b5d37adc 50 void Adafruit_PWMServoDriver::reset(void) {
syundo0730 0:9d01b5d37adc 51 write8(PCA9685_MODE1, 0x0);
syundo0730 0:9d01b5d37adc 52 }
syundo0730 0:9d01b5d37adc 53
syundo0730 0:9d01b5d37adc 54 void Adafruit_PWMServoDriver::setPrescale(uint8_t prescale) {
syundo0730 0:9d01b5d37adc 55 uint8_t oldmode = read8(PCA9685_MODE1);
syundo0730 0:9d01b5d37adc 56 uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
syundo0730 0:9d01b5d37adc 57 write8(PCA9685_MODE1, newmode); // go to sleep
syundo0730 0:9d01b5d37adc 58 wait_ms(5);
syundo0730 0:9d01b5d37adc 59 write8(PCA9685_PRESCALE, prescale); // set the prescaler
syundo0730 0:9d01b5d37adc 60 write8(PCA9685_MODE1, oldmode);
syundo0730 0:9d01b5d37adc 61 wait_ms(5);
syundo0730 0:9d01b5d37adc 62 write8(PCA9685_MODE1, oldmode | 0xa1);
syundo0730 0:9d01b5d37adc 63 }
syundo0730 0:9d01b5d37adc 64
syundo0730 0:9d01b5d37adc 65 void Adafruit_PWMServoDriver::setPWMFreq(float freq) {
syundo0730 0:9d01b5d37adc 66 //Serial.print("Attempting to set freq ");
syundo0730 0:9d01b5d37adc 67 //Serial.println(freq);
syundo0730 0:9d01b5d37adc 68 float prescaleval = 25000000;
syundo0730 0:9d01b5d37adc 69 prescaleval /= 4096;
syundo0730 0:9d01b5d37adc 70 prescaleval /= freq;
syundo0730 0:9d01b5d37adc 71 //Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
syundo0730 0:9d01b5d37adc 72 uint8_t prescale = floor(prescaleval + 0.5) - 1;
syundo0730 0:9d01b5d37adc 73 //Serial.print("Final pre-scale: "); Serial.println(prescale);
syundo0730 0:9d01b5d37adc 74 setPrescale(prescale);
syundo0730 0:9d01b5d37adc 75 }
syundo0730 0:9d01b5d37adc 76
syundo0730 0:9d01b5d37adc 77 void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) {
syundo0730 0:9d01b5d37adc 78 // hmm doesnt work, whyso? ( Not in AI mode. See line 54 above. ( Works now!! :D )
syundo0730 0:9d01b5d37adc 79
syundo0730 0:9d01b5d37adc 80 char cmd[5];
syundo0730 0:9d01b5d37adc 81 cmd[0] = LED0_ON_L + 4 * num;
syundo0730 0:9d01b5d37adc 82 cmd[1] = on;
syundo0730 0:9d01b5d37adc 83 cmd[2] = on >> 8;
syundo0730 0:9d01b5d37adc 84 cmd[3] = off;
syundo0730 0:9d01b5d37adc 85 cmd[4] = off >> 8;
syundo0730 0:9d01b5d37adc 86 i2c.write(_i2caddr, cmd, 5);
syundo0730 0:9d01b5d37adc 87
syundo0730 0:9d01b5d37adc 88 /*write8(LED0_ON_L+4*num, on);
syundo0730 0:9d01b5d37adc 89 write8(LED0_ON_H+4*num, on >> 8);
syundo0730 0:9d01b5d37adc 90 write8(LED0_OFF_L+4*num, off);
syundo0730 0:9d01b5d37adc 91 write8(LED0_OFF_H+4*num, off >> 8);*/
syundo0730 0:9d01b5d37adc 92 }
syundo0730 0:9d01b5d37adc 93
syundo0730 2:88bdd5c4e77b 94 // Set pwm duty in us order
syundo0730 1:69033d5e289b 95 void Adafruit_PWMServoDriver::setDuty(uint8_t num, uint16_t duty) {
syundo0730 2:88bdd5c4e77b 96 float pulselength = 10000; // 10,000 us per second
syundo0730 2:88bdd5c4e77b 97 duty = 4094 * duty / pulselength;
syundo0730 1:69033d5e289b 98 setPWM(num, 0, duty);
syundo0730 1:69033d5e289b 99 }
syundo0730 1:69033d5e289b 100
syundo0730 0:9d01b5d37adc 101 uint8_t Adafruit_PWMServoDriver::read8(char addr) {
syundo0730 0:9d01b5d37adc 102 i2c.write(_i2caddr, &addr, 1);
syundo0730 0:9d01b5d37adc 103 char rtn;
syundo0730 0:9d01b5d37adc 104 i2c.read(_i2caddr, &rtn, 1);
syundo0730 0:9d01b5d37adc 105 return rtn;
syundo0730 0:9d01b5d37adc 106 }
syundo0730 0:9d01b5d37adc 107
syundo0730 0:9d01b5d37adc 108 void Adafruit_PWMServoDriver::write8(char addr, char d) {
syundo0730 0:9d01b5d37adc 109 char cmd[2];
syundo0730 0:9d01b5d37adc 110 cmd[0] = addr;
syundo0730 0:9d01b5d37adc 111 cmd[1] = d;
syundo0730 0:9d01b5d37adc 112 i2c.write(_i2caddr, cmd, 2);
syundo0730 0:9d01b5d37adc 113 }