K-Study-Team / Adafruit-PWM-Servo-Driver

Library for 16 channel PWM servo driver PCA9685. v2

Dependents:   totoV1-2

Fork of Adafruit-PWM-Servo-Driver by Shundo Kishi

Adafruit_PWMServoDriver.cpp@3:07497eda12c2, 2017-11-14 (annotated)

Committer:
fixair
Date:
Tue Nov 14 18:06:44 2017 +0000
Revision:
3:07497eda12c2
Parent:
2:88bdd5c4e77b 
test spd15

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
fixair 3:07497eda12c2 26 //Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(PinName sda, PinName scl, int addr) : i2c(sda, scl), _i2caddr(addr) {}
fixair 3:07497eda12c2 27
fixair 3:07497eda12c2 28 Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(PinName sda, PinName scl, int addr) {
fixair 3:07497eda12c2 29 i2c = new I2C(sda, scl);
fixair 3:07497eda12c2 30 _i2caddr = addr;
fixair 3:07497eda12c2 31 }
syundo0730 0:9d01b5d37adc 32
syundo0730 0:9d01b5d37adc 33 void Adafruit_PWMServoDriver::i2c_probe(void)
syundo0730 0:9d01b5d37adc 34 {
fixair 3:07497eda12c2 35 printf("Probing I2C devices...\n");
syundo0730 0:9d01b5d37adc 36
syundo0730 0:9d01b5d37adc 37 int count = 0;
fixair 3:07497eda12c2 38 for (int address=1; address<256; address+=1) {
fixair 3:07497eda12c2 39 if (!i2c->write(address, NULL, 0)) { // 0 returned is ok
syundo0730 0:9d01b5d37adc 40 printf(" - I2C device found at address 0x%02X\r\n", address);
syundo0730 0:9d01b5d37adc 41 count++;
syundo0730 0:9d01b5d37adc 42 }
syundo0730 0:9d01b5d37adc 43 }
syundo0730 0:9d01b5d37adc 44 printf("%d devices found\r\n", count);
syundo0730 0:9d01b5d37adc 45 }
syundo0730 0:9d01b5d37adc 46
syundo0730 0:9d01b5d37adc 47 void Adafruit_PWMServoDriver::begin(void) {
syundo0730 0:9d01b5d37adc 48 reset();
syundo0730 0:9d01b5d37adc 49 }
syundo0730 0:9d01b5d37adc 50
syundo0730 0:9d01b5d37adc 51 void Adafruit_PWMServoDriver::setI2Cfreq(int freq) {
fixair 3:07497eda12c2 52 i2c->frequency(freq);
syundo0730 0:9d01b5d37adc 53 }
syundo0730 0:9d01b5d37adc 54
syundo0730 0:9d01b5d37adc 55 void Adafruit_PWMServoDriver::reset(void) {
syundo0730 0:9d01b5d37adc 56 write8(PCA9685_MODE1, 0x0);
syundo0730 0:9d01b5d37adc 57 }
syundo0730 0:9d01b5d37adc 58
syundo0730 0:9d01b5d37adc 59 void Adafruit_PWMServoDriver::setPrescale(uint8_t prescale) {
syundo0730 0:9d01b5d37adc 60 uint8_t oldmode = read8(PCA9685_MODE1);
syundo0730 0:9d01b5d37adc 61 uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
syundo0730 0:9d01b5d37adc 62 write8(PCA9685_MODE1, newmode); // go to sleep
syundo0730 0:9d01b5d37adc 63 wait_ms(5);
syundo0730 0:9d01b5d37adc 64 write8(PCA9685_PRESCALE, prescale); // set the prescaler
syundo0730 0:9d01b5d37adc 65 write8(PCA9685_MODE1, oldmode);
syundo0730 0:9d01b5d37adc 66 wait_ms(5);
syundo0730 0:9d01b5d37adc 67 write8(PCA9685_MODE1, oldmode | 0xa1);
syundo0730 0:9d01b5d37adc 68 }
syundo0730 0:9d01b5d37adc 69
syundo0730 0:9d01b5d37adc 70 void Adafruit_PWMServoDriver::setPWMFreq(float freq) {
syundo0730 0:9d01b5d37adc 71 //Serial.print("Attempting to set freq ");
syundo0730 0:9d01b5d37adc 72 //Serial.println(freq);
syundo0730 0:9d01b5d37adc 73 float prescaleval = 25000000;
syundo0730 0:9d01b5d37adc 74 prescaleval /= 4096;
syundo0730 0:9d01b5d37adc 75 prescaleval /= freq;
syundo0730 0:9d01b5d37adc 76 //Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
syundo0730 0:9d01b5d37adc 77 uint8_t prescale = floor(prescaleval + 0.5) - 1;
syundo0730 0:9d01b5d37adc 78 //Serial.print("Final pre-scale: "); Serial.println(prescale);
syundo0730 0:9d01b5d37adc 79 setPrescale(prescale);
syundo0730 0:9d01b5d37adc 80 }
syundo0730 0:9d01b5d37adc 81
syundo0730 0:9d01b5d37adc 82 void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) {
syundo0730 0:9d01b5d37adc 83 // hmm doesnt work, whyso? ( Not in AI mode. See line 54 above. ( Works now!! :D )
syundo0730 0:9d01b5d37adc 84
syundo0730 0:9d01b5d37adc 85 char cmd[5];
syundo0730 0:9d01b5d37adc 86 cmd[0] = LED0_ON_L + 4 * num;
syundo0730 0:9d01b5d37adc 87 cmd[1] = on;
syundo0730 0:9d01b5d37adc 88 cmd[2] = on >> 8;
syundo0730 0:9d01b5d37adc 89 cmd[3] = off;
syundo0730 0:9d01b5d37adc 90 cmd[4] = off >> 8;
fixair 3:07497eda12c2 91 i2c->write(_i2caddr, cmd, 5);
syundo0730 0:9d01b5d37adc 92
syundo0730 0:9d01b5d37adc 93 /*write8(LED0_ON_L+4*num, on);
syundo0730 0:9d01b5d37adc 94 write8(LED0_ON_H+4*num, on >> 8);
syundo0730 0:9d01b5d37adc 95 write8(LED0_OFF_L+4*num, off);
syundo0730 0:9d01b5d37adc 96 write8(LED0_OFF_H+4*num, off >> 8);*/
syundo0730 0:9d01b5d37adc 97 }
syundo0730 0:9d01b5d37adc 98
syundo0730 2:88bdd5c4e77b 99 // Set pwm duty in us order
syundo0730 1:69033d5e289b 100 void Adafruit_PWMServoDriver::setDuty(uint8_t num, uint16_t duty) {
syundo0730 2:88bdd5c4e77b 101 float pulselength = 10000; // 10,000 us per second
syundo0730 2:88bdd5c4e77b 102 duty = 4094 * duty / pulselength;
syundo0730 1:69033d5e289b 103 setPWM(num, 0, duty);
syundo0730 1:69033d5e289b 104 }
syundo0730 1:69033d5e289b 105
syundo0730 0:9d01b5d37adc 106 uint8_t Adafruit_PWMServoDriver::read8(char addr) {
fixair 3:07497eda12c2 107 i2c->write(_i2caddr, &addr, 1);
syundo0730 0:9d01b5d37adc 108 char rtn;
fixair 3:07497eda12c2 109 i2c->read(_i2caddr, &rtn, 1);
syundo0730 0:9d01b5d37adc 110 return rtn;
syundo0730 0:9d01b5d37adc 111 }
syundo0730 0:9d01b5d37adc 112
syundo0730 0:9d01b5d37adc 113 void Adafruit_PWMServoDriver::write8(char addr, char d) {
syundo0730 0:9d01b5d37adc 114 char cmd[2];
syundo0730 0:9d01b5d37adc 115 cmd[0] = addr;
syundo0730 0:9d01b5d37adc 116 cmd[1] = d;
fixair 3:07497eda12c2 117 i2c->write(_i2caddr, cmd, 2);
syundo0730 0:9d01b5d37adc 118 }