File content as of revision 15:55221f9de707:

#include "Servo.h"
#include "mbed.h"



// The next line determines the maximum number of servo objects 
// that can be successfully created.
// By default, this is set to 26, as there are only 26 unique 
// DigitalOut pins provided by the mbed.
Servo *Servo::servos[26];
unsigned int Servo::numServos = 0;
Ticker* Servo::refreshTicker;



// The pin must be specified when the object is initialized, but 
// the initial pulse width can be omitted, giving a default of 
// 1500 us. 
// This should be at about half the range of most servos.
Servo::Servo(PinName pin, bool start) : signalPin(pin)
{
    // Set default calibration
    calibrate(2000, 1000, 60.f, -60.f);
    
    pulseWidth = center;
    
    if (numServos == 0)
    {
        refreshTicker = new Ticker();
        
        // Start the ticker that refreshes all servos
        refreshTicker->attach_us(&Servo::refresh, period);
    }
    
    servos[numServos++] = this;
    
    enabled = start;
}



bool Servo::calibrate(unsigned int plus45, unsigned int minus45, float upperLimit, float lowerLimit)
{
    // Check if given parameters are valid
    if (upperLimit > lowerLimit)
    {
        center = (plus45 + minus45) / 2;
        usPerDegree = ((int)plus45 - (int)center) / 45.0f;
        this->upperLimit = upperLimit;
        this->lowerLimit = lowerLimit;   
        return true;
    }
    else
    {
        return false;
    }
}



void Servo::write(float degrees)
{
    // Limit to the valid angle range
    degrees = (degrees > upperLimit ? upperLimit : (degrees < lowerLimit ? lowerLimit : degrees));
    
    pulseWidth = center + (int)(degrees * usPerDegree);
}



void Servo::writeWidth(unsigned int width)
{
    // Make sure that the pulse width is less than the refresh period
    pulseWidth = width < period ? width : period;
}



float Servo::read()
{
    return ((int)pulseWidth - (int)center) / usPerDegree;
}



int Servo::readWidth()
{
    return pulseWidth;
}



void Servo::disable()
{
    enabled = false;
}



void Servo::enable()
{
    enabled = true;
}



void Servo::operator=(float degrees)
{
    write(degrees);
}



Servo::operator float()
{
    return read();
}



Servo::operator int()
{
    return readWidth();
}



void Servo::refresh()
{
    // Start all of the individual servo width timeouts and write a logical 1 to their signal pins
    for (unsigned int i = 0; i < numServos; i++)
    {
        if (servos[i]->enabled)
        {
            servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth);
            Servo::servos[i]->signalPin.write(1);
        }
    }
}



void Servo::timeout()
{
    // Write a logical zero to the servo's signal pin
    signalPin = 0;
}