diff --git a/Adafruit_PWMServoDriver.cpp b/Adafruit_PWMServoDriver.cpp index 71d6e5f..bb2caaf 100644 --- a/Adafruit_PWMServoDriver.cpp +++ b/Adafruit_PWMServoDriver.cpp @@ -29,7 +29,7 @@ #include "Adafruit_PWMServoDriver.h" -//#define ENABLE_DEBUG_OUTPUT +// #define ENABLE_DEBUG_OUTPUT /*! * @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a diff --git a/Adafruit_PWMServoDriverGroup.cpp b/Adafruit_PWMServoDriverGroup.cpp new file mode 100644 index 0000000..619c849 --- /dev/null +++ b/Adafruit_PWMServoDriverGroup.cpp @@ -0,0 +1,254 @@ +#include "Adafruit_PWMServoDriverGroup.h" + +/*! + * @brief Instantiates new PCA9685 PWM driver chips with user defined + * I2C addresses on a TwoWire interface + * @param nDrivers Number of PWM driver chips to instantiate + * @param nServosEach Number of servos to allocate on each driver + * @param addr Array of 7-bit I2C addresses to locate the chips, default is + * 0x40 through 0x44 + */ +Adafruit_PWMServoDriverGroup::Adafruit_PWMServoDriverGroup( + const uint8_t nDrivers, const uint8_t nServosEach, const uint8_t *addr) { + _nDrivers = nDrivers; + _nServosEach = nServosEach; + + _drivers = (Adafruit_PWMServoDriver **)malloc( + nDrivers * sizeof(Adafruit_PWMServoDriver *)); + + for (uint8_t i = 0; i < _nDrivers; i++) { + _drivers[i] = new Adafruit_PWMServoDriver(addr[i], Wire); + } +} + +/*! + * @brief Instantiates new PCA9685 PWM driver chips with I2C addresses on a + * TwoWire interface + * @param nDrivers Number of PWM driver chips to instantiate + * @param nServosEach Number of servos to allocate on each driver + * @param addr Array of 7-bit I2C addresses to locate the chips, default is + * 0x40 thorugh 0x44 + * @param i2c A reference to a 'TwoWire' object that we'll use to communicate + */ +Adafruit_PWMServoDriverGroup::Adafruit_PWMServoDriverGroup( + const uint8_t nDrivers, const uint8_t nServosEach, const uint8_t *addr, + TwoWire &i2c) { + _nDrivers = nDrivers; + _nServosEach = nServosEach; + + _drivers = (Adafruit_PWMServoDriver **)malloc( + nDrivers * sizeof(Adafruit_PWMServoDriver *)); + + for (uint8_t i = 0; i < _nDrivers; i++) { + _drivers[i] = new Adafruit_PWMServoDriver(addr[i], i2c); + } +} + +/*! + * @brief Gets the number of PCA9685 PWM driver chips connected to this class + @returns Number of PCA9685 PWM driver chips + */ +uint8_t Adafruit_PWMServoDriverGroup::getNumDrivers() { return _nDrivers; } + +/*! + * @brief Gets the number of servos associated with each PCA9685 PWM chip + @returns Number of servos connected to each PCA9685 PWM driver chip + */ +uint8_t Adafruit_PWMServoDriverGroup::getNumServosEach() { + return _nServosEach; +} + +/*! + * @brief Gets the total number of servos associated with this + @returns Total number of servos + */ +uint8_t Adafruit_PWMServoDriverGroup::getNumServos() { + return _nDrivers * _nServosEach; +} + +/*! + * @brief Gets the Adafruit_PWMServoDriver associated with a given servo and + * the local servo number on that device + * @param num Number of servo in master list + * @param localId returns the number of the servo as known to the PCA9685 chip + * @return The Adafruit_PWMServoDriver associated with the requested servo + */ +Adafruit_PWMServoDriver * +Adafruit_PWMServoDriverGroup::getDriver(uint8_t num, uint8_t &localId) { + uint8_t driverId = 0; + if (num > _nServosEach - 1) + driverId = num / _nServosEach; + localId = num - (driverId * _nServosEach); + + return _drivers[driverId]; +} + +/*! + * @brief Setups the I2C interface and hardware + * @param prescale + * Sets External Clock (Optional) + * @return true if successful, otherwise false + */ +bool Adafruit_PWMServoDriverGroup::begin(uint8_t prescale) { + bool status = true; + for (int i = 0; i < _nDrivers; i++) + status &= _drivers[i]->begin(prescale); + + return status; +} + +/*! + * @brief Sends a reset command to the PCA9685 chips over I2C + */ +void Adafruit_PWMServoDriverGroup::reset() { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->reset(); +} + +/*! + * @brief Puts boards into sleep mode + */ +void Adafruit_PWMServoDriverGroup::sleep() { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->sleep(); +} + +/*! + * @brief Wakes boards from sleep + */ +void Adafruit_PWMServoDriverGroup::wakeup() { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->wakeup(); +} + +/*! + * @brief Sets EXTCLK pin to use the external clock + * @param prescale + * Configures the prescale value to be used by the external clock + */ +void Adafruit_PWMServoDriverGroup::setExtClk(uint8_t prescale) { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->setExtClk(prescale); +} + +/*! + * @brief Sets the PWM frequency for all chips, up to ~1.6 KHz + * @param freq Floating point frequency that we will attempt to match + */ +void Adafruit_PWMServoDriverGroup::setPWMFreq(float freq) { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->setPWMFreq(freq); +} + +/*! + * @brief Sets the output mode of the PCA9685s to either + * open drain or push pull / totempole. + * Warning: LEDs with integrated zener diodes should + * only be driven in open drain mode. + * @param totempole Totempole if true, open drain if false. + */ +void Adafruit_PWMServoDriverGroup::setOutputMode(bool totempole) { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->setOutputMode(totempole); +} + +/*! + * @brief Gets the PWM output of one of the PCA9685 pins + * @param num One of the PWM output pins, from 0 to (nDrivers * nServosEach - + * 1) + * @param off If true, returns PWM OFF value, otherwise PWM ON + * @return requested PWM output value + */ +uint16_t Adafruit_PWMServoDriverGroup::getPWM(uint8_t num, bool off) { + uint8_t localId = 0; + auto driver = getDriver(num, localId); + return driver->getPWM(localId, off); +} + +/*! + * @brief Sets the PWM output of one of the PCA9685 pins + * @param num One of the PWM output pins, from 0 to (nDrivers * nServosEach - + * 1) + * @param on At what point in the 4096-part cycle to turn the PWM output ON + * @param off At what point in the 4096-part cycle to turn the PWM output OFF + * @return 0 if successful, otherwise 1 + */ +uint8_t Adafruit_PWMServoDriverGroup::setPWM(uint8_t num, uint16_t on, + uint16_t off) { + uint8_t localId = 0; + auto driver = getDriver(num, localId); + return driver->setPWM(localId, on, off); +} + +/*! + * @brief Helper to set pin PWM output. Sets pin without having to deal with + * on/off tick placement and properly handles a zero value as completely off and + * 4095 as completely on. Optional invert parameter supports inverting the + * pulse for sinking to ground. + * @param num One of the PWM output pins, from 0 to (nDrivers * nServosEach - + * 1) + * @param val The number of ticks out of 4096 to be active, should be a value + * from 0 to 4095 inclusive. + * @param invert If true, inverts the output, defaults to 'false' + */ +void Adafruit_PWMServoDriverGroup::setPin(uint8_t num, uint16_t val, + bool invert) { + uint8_t localId = 0; + auto driver = getDriver(num, localId); + driver->setPin(localId, val, invert); +} + +/*! + * @brief Reads set Prescale from PCA9685 + * @return prescale value + */ +uint8_t Adafruit_PWMServoDriverGroup::readPrescale() { + return _drivers[0]->readPrescale(); +} + +/*! + * @brief Sets the PWM output of one of the PCA9685 pins based on the input + * microseconds, output is not precise + * @param num One of the PWM output pins, from 0 to (nDrivers * nServosEach - + * 1) + * @param microseconds The number of Microseconds to turn the PWM output ON + */ +void Adafruit_PWMServoDriverGroup::writeMicroseconds(uint8_t num, + uint16_t microseconds) { + uint8_t localId = 0; + auto driver = getDriver(num, localId); + + driver->writeMicroseconds(localId, microseconds); +} + +/*! + * @brief Getter for the internally tracked oscillator used for freq + * calculations + * @param id The id of the PCA9685 chip to get frequency from + * @returns The frequency the PCA9685 thinks it is running at (it cannot + * introspect) + */ +uint32_t Adafruit_PWMServoDriverGroup::getOscillatorFrequency(uint8_t id) { + return _drivers[id]->getOscillatorFrequency(); +} + +/*! + * @brief Setter for the internally tracked oscillator used for freq + * calculations + * @param freq The frequency the PCA9685 should use for frequency calculations + */ +void Adafruit_PWMServoDriverGroup::setOscillatorFrequency(uint32_t freq) { + for (int i = 0; i < _nDrivers; i++) + _drivers[i]->setOscillatorFrequency(freq); +} + +/*! + * @brief Setter for the internally tracked oscillator used for freq + * calculations + * @param id The id of the PCA9685 chip to set the frequency on + * @param freq The frequency the PCA9685 should use for frequency calculations + */ +void Adafruit_PWMServoDriverGroup::setOscillatorFrequency(uint8_t id, + uint32_t freq) { + _drivers[id]->setOscillatorFrequency(freq); +} diff --git a/Adafruit_PWMServoDriverGroup.h b/Adafruit_PWMServoDriverGroup.h new file mode 100644 index 0000000..396c0b0 --- /dev/null +++ b/Adafruit_PWMServoDriverGroup.h @@ -0,0 +1,63 @@ +/*! + * @file Adafruit_PWMServoDriverGroup.h + * + * This Library sets up a group of Adafruit_PWMServoDrivers + * to act like a single Adafruit_PWMServoDrivers object + * + * + * Designed specifically to work with the Adafruit 16-channel PWM & Servo + * driver. + * + * BSD license, all text above must be included in any redistribution + */ +#ifndef _ADAFRUIT_PWMServoDriverGroup_H +#define _ADAFRUIT_PWMServoDriverGroup_H + +#include "Adafruit_PWMServoDriver.h" + +#define PCA9685_I2C_ADDRESS_1 0x40 /**< Default PCA9685 I2C Slave Address */ +#define PCA9685_I2C_ADDRESS_2 0x41 /**< Second I2C Slave Address */ +#define PCA9685_I2C_ADDRESS_3 0x42 /**< Third I2C Slave Address */ +#define PCA9685_I2C_ADDRESS_4 0x43 /**< Fourth I2C Slave Address */ +#define PCA9685_I2C_ADDRESS_5 0x44 /**< Fifth I2C Slave Address */ + +/*! + * @brief Class that stores state and functions for interacting with multiple + * PCA9685 PWM chips as if they are a single instance + */ +class Adafruit_PWMServoDriverGroup { +public: + Adafruit_PWMServoDriverGroup(const uint8_t ndrivers, + const uint8_t nServosEach, const uint8_t *addr); + Adafruit_PWMServoDriverGroup(const uint8_t ndrivers, + const uint8_t nServosEach, const uint8_t *addr, + TwoWire &i2c); + + Adafruit_PWMServoDriver *getDriver(uint8_t num, uint8_t &localNum); + bool begin(uint8_t prescale = 0); + void reset(); + void sleep(); + void wakeup(); + void setExtClk(uint8_t prescale); + void setPWMFreq(float freq); + void setOutputMode(bool totempole); + uint16_t getPWM(uint8_t num, bool off = false); + uint8_t setPWM(uint8_t num, uint16_t on, uint16_t off); + void setPin(uint8_t num, uint16_t val, bool invert = false); + uint8_t readPrescale(void); + void writeMicroseconds(uint8_t num, uint16_t microseconds); + void setOscillatorFrequency(uint32_t freq); + void setOscillatorFrequency(uint8_t id, uint32_t freq); + uint32_t getOscillatorFrequency(uint8_t id); + + uint8_t getNumDrivers(); + uint8_t getNumServos(); + uint8_t getNumServosEach(); + +private: + uint8_t _nDrivers; + uint8_t _nServosEach; + Adafruit_PWMServoDriver **_drivers; +}; + +#endif \ No newline at end of file diff --git a/examples/servoGroup/servoGroup.ino b/examples/servoGroup/servoGroup.ino new file mode 100644 index 0000000..88fd187 --- /dev/null +++ b/examples/servoGroup/servoGroup.ino @@ -0,0 +1,117 @@ +/*************************************************** + This is an example for our Adafruit 16-channel PWM & Servo driver + Servo test - this will drive 8 servos, one after the other on the + first 8 pins of the PCA9685 + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/products/815 + + These drivers use I2C to communicate, 2 pins are required to + interface. + + Adafruit invests time and resources providing this open source code, + please support Adafruit and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#include +#include + +#define N_PWM_DRIVERS 2 // Total number of PCA9685 PWM driver chips in project +#define N_SERVOS_EACH 8 // The feather wings have 8 servos per driver board + +constexpr uint8_t PWM_ADDRS[] = {0X40, 0X41}; // Provide an address for each driver board + +Adafruit_PWMServoDriverGroup pwm = Adafruit_PWMServoDriverGroup(N_PWM_DRIVERS, N_SERVOS_EACH, PWM_ADDRS); +// you can also call it with a different I2C interface +//Adafruit_PWMServoDriverGroup pwm = Adafruit_PWMServoDriverGroup(N_PWM_DRIVERS, N_SERVOS_EACH, PWM_ADDRS, Wire); + +// Depending on your servo make, the pulse width min and max may vary, you +// want these to be as small/large as possible without hitting the hard stop +// for max range. You'll have to tweak them as necessary to match the servos you +// have! +#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096) +#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096) +#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 +#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 +#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates + +// our servo # counter +uint8_t servonum = 0; + +void setup() { + Serial.begin(9600); + Serial.println("ServoGroup test!"); + + pwm.begin(); + /* + * In theory the internal oscillator (clock) is 25MHz but it really isn't + * that precise. You can 'calibrate' this by tweaking this number until + * you get the PWM update frequency you're expecting! + * The int.osc. for the PCA9685 chip is a range between about 23-27MHz and + * is used for calculating things like writeMicroseconds() + * Analog servos run at ~50 Hz updates, It is importaint to use an + * oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip. + * 1) Attach the oscilloscope to one of the PWM signal pins and ground on + * the I2C PCA9685 chip you are setting the value for. + * 2) Adjust setOscillatorFrequency() until the PWM update frequency is the + * expected value (50Hz for most ESCs) + * Setting the value here is specific to each individual I2C PCA9685 chip and + * affects the calculations for the PWM update frequency. + * Failure to correctly set the int.osc value will cause unexpected PWM results + */ + pwm.setOscillatorFrequency(27000000); + pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates + + delay(10); +} + +// You can use this function if you'd like to set the pulse length in seconds +// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise! +void setServoPulse(uint8_t n, double pulse) { + double pulselength; + + pulselength = 1000000; // 1,000,000 us per second + pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates + Serial.print(pulselength); Serial.println(" us per period"); + pulselength /= 4096; // 12 bits of resolution + Serial.print(pulselength); Serial.println(" us per bit"); + pulse *= 1000000; // convert input seconds to us + pulse /= pulselength; + Serial.println(pulse); + pwm.setPWM(n, 0, pulse); +} + +void loop() { + // Drive each servo one at a time using setPWM() + Serial.println(servonum); + for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) { + pwm.setPWM(servonum, 0, pulselen); + } + + delay(500); + for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) { + pwm.setPWM(servonum, 0, pulselen); + } + + delay(500); + + // Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding! + // The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior. + for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) { + pwm.writeMicroseconds(servonum, microsec); + } + + delay(500); + for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) { + pwm.writeMicroseconds(servonum, microsec); + } + + delay(500); + + servonum++; + if (servonum > (N_PWM_DRIVERS * N_SERVOS_EACH - 1)) servonum = 0; // Testing the all servo channels +}