Saturday, August 8, 2015

Servo Controller: Arduino Servo Class

Soldering up the servo shield and running Adafruit's examples applications were very straightforward. I recommend following Adafruit's fantastic tutorials to work through that. https://learn.adafruit.com/adafruit-16-channel-pwm-slash-servo-shield/overview However, to manage many servos at once I needed an object model to cleanly handle them all. I want to be able to support not only many servos but many servos on multiple servo drivers.

I created a new ServoModel class that contains the index of the servo, a pointer to the servo driver, and the minimum and maximum pulse values. A constructor defines all of those basic values, but there are additional functions to redefine min/max values (configuring for example) and setting the position based on a specific pulse or just a percentage.

Includes and Constants


#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

typedef long int32;

const uint16_t pulseLengthMinimum = 0;
const uint16_t pulseLengthMaximum = 4096;
const int32 numberOfServoDrivers = 1;
const int32 numberOfServos = 6;
const int32 numberOfServosPerDriver = 16;

ServoModel Class


class ServoModel
{
  private:
  int32 servoNumber;
  Adafruit_PWMServoDriver *servoDriver;
  uint16_t minimumValue_pulseLength;
  uint16_t maximumValue_pulseLength;
  uint16_t currentValue_pulseLength;
  
  public:
  ServoModel()
  {
  }

  ServoModel(Adafruit_PWMServoDriver *newServoDriver, int newServoNumber,
             uint16_t newMinimumValue_pulseLength, uint16_t newMaximumValue_pulseLength,
             uint16_t newValue_percent)
  {
    servoDriver = newServoDriver;
    servoNumber = newServoNumber;
    minimumValue_pulseLength = newMinimumValue_pulseLength;
    maximumValue_pulseLength = newMaximumValue_pulseLength;
    
    setToValue_percent(newValue_percent);
  }

  uint16_t setLowerPulseLength(uint16_t newValue_pulseLength)
  {
    if (newValue_pulseLength < pulseLengthMinimum)
    {
      newValue_pulseLength = pulseLengthMinimum;
    }
    minimumValue_pulseLength = newValue_pulseLength;
    return minimumValue_pulseLength;
  }

  uint16_t setUpperPulseLength(uint16_t newValue_pulseLength)
  {
    if (newValue_pulseLength > pulseLengthMaximum)
    {
      newValue_pulseLength = pulseLengthMaximum;
    }
    maximumValue_pulseLength = newValue_pulseLength;
    return maximumValue_pulseLength;
  }

  uint16_t setToMinimumValue()
  {
    return setToValue_pulseLength(minimumValue_pulseLength);
  }

  uint16_t setToMaximumValue()
  {
    return setToValue_pulseLength(maximumValue_pulseLength);
  }

  uint16_t setToValue_pulseLength(uint16_t newValue_pulseLength)
  {
    if (newValue_pulseLength < minimumValue_pulseLength)
    {
      newValue_pulseLength = minimumValue_pulseLength;
    }
    else if (newValue_pulseLength > maximumValue_pulseLength)
    {
      newValue_pulseLength = maximumValue_pulseLength;
    }
    currentValue_pulseLength = newValue_pulseLength;
    servoDriver->setPWM(servoNumber, 0, currentValue_pulseLength);
    return currentValue_pulseLength;
  }

  uint16_t setToValue_percent(float newValue_percent)
  {
    uint16_t newValue_pulseLength = minimumValue_pulseLength + (uint16_t)((newValue_percent / 100) * 
                                    (float)(maximumValue_pulseLength - minimumValue_pulseLength));
    return setToValue_pulseLength(newValue_pulseLength);
  }
};

Global Variables


Adafruit_PWMServoDriver servoDrivers[numberOfServoDrivers];
ServoModel servos[numberOfServos];

One Time Setup (When the controller initializes.)


void setup() {
  Serial.begin(9600);
  Serial.println("Begin Setup");

  // Initialize Servo Drivers
  Serial.println("Initialize Servo Drivers");
  servoDrivers[0] = Adafruit_PWMServoDriver(0x40);
  servoDrivers[0].begin();
  servoDrivers[0].setPWMFreq(60);
  
  // Initialize Servo Objects
  // The following loop automatically assigns the servos to the
  // available servo drivers.
  Serial.println("Initialize Servo Objects");
  int32 driverNumber = 0;
  int32 servoNumber = 0;
  for (int32 servoIndex = 0; servoIndex < numberOfServos; servoIndex++)
  {
    // Initialize with a min and max pulse length of 200 and 550 respectively.
    // This value varies by servo! Do not exceeds the physical limits of your servo!
    servos[servoIndex] = ServoModel(&servoDrivers[driverNumber], servoNumber,
                                    200, 550, 50);
    servoNumber ++;
    if (servoNumber >= numberOfServosPerDriver)
    {
      servoNumber = 0;
      driverNumber ++;
      if (driverNumber >= numberOfServoDrivers)
      {
        // Not good, we're out of drivers for how many servos specified!
        driverNumber = 0;
      } // driverNumber >= numberOfServoDrivers
    } // servoNumber >= numberOfServosPerDriver
  } // servoIndex

  Serial.println("Completed Setup");
}

How to Use


The code above is the foundation for how the microcontroller will interact with the servos.

To instruct servo 0 to go to its minimum value:
servos[0].setToMinimumValue();

To instructor servo 1 to go to 25%:
servos[1].setToValue_percent(25);


My next post will show how I'm reading inputs from the serial port to manipulate the values of the servo objects.


Copyright (c) 2015 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

No comments:

Post a Comment