add ServoGroup class

Author: Charles-Ellison

Adafruit_PWMServoDriverGroup.cpp

@@ -0,0 +1,221 @@
+#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 with
+ */
+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
+*/
+uint8_t Adafruit_PWMServoDriverGroup::getNumDrivers() {
+  return _nDrivers;
+}
+
+/*  @brief Gets the number of servos associated with each PCA9685 PWM chip
+*/
+uint8_t Adafruit_PWMServoDriverGroup::getNumServosEach() {
+  return _nServosEach;
+}
+
+/*  @brief Gets the total number of servos associated with this class
+*/
+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
+ *  @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);
+}
+
+void Adafruit_PWMServoDriverGroup::setOscillatorFrequency(uint8_t id, uint32_t freq){
+   _drivers[id]->setOscillatorFrequency(freq);
+}
+
+

Adafruit_PWMServoDriverGroup.h

@@ -0,0 +1,61 @@
+/*!
+ *  @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
+#define PCA9685_I2C_ADDRESS_2 0x41
+#define PCA9685_I2C_ADDRESS_3 0x42
+#define PCA9685_I2C_ADDRESS_4 0x43
+#define PCA9685_I2C_ADDRESS_5 0x44
+
+/*!
+ *  @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