File content as of revision 2:738b08cb03ea:

#include "Servo.hpp"

Servo::Servo(PinName pwmPin, float zeroAngle, float minAngle, float maxAngle) :
    _servo(pwmPin)
{
    PW_PER_DEG = 0.0005f/45.0f;
    ZEROPW = 0.0014f;

    _minAngle = minAngle;
    _maxAngle = maxAngle;
    _angle = zeroAngle;
    _servo.period_us(5000);
    _servo.pulsewidth(0);
    SaveZero();        
}

void Servo::SetServo()
{
    float pw = ZEROPW+_angle*PW_PER_DEG;

    pw = ( pw < MIN_PW ? MIN_PW : pw > MAX_PW ? MAX_PW : pw);

    _servo.pulsewidth(pw);
}

void Servo::SaveZero()
{
    ZEROPW = (ZEROPW+_angle*PW_PER_DEG);
}

void Servo::Calibrate(float actualAngle)
{
    PW_PER_DEG = (_angle*PW_PER_DEG)/actualAngle;
}

void Servo::Set(float angle)
{
    if( _angle < _minAngle)
        _angle = _minAngle;
    else if( _angle > _maxAngle )
        _angle = _maxAngle;
    else 
        _angle = angle;

    SetServo();
}

void Servo::Move(float relAngle)
{
    float newAngle = _angle + relAngle;

    if( newAngle < _minAngle )
        _angle = _minAngle;
    else if( newAngle > _maxAngle )
        _angle = _maxAngle;
    else
        _angle = newAngle;

    SetServo();
}

void Servo::SetZero(float angle)
{
  ZEROPW = (ZEROPW+angle*PW_PER_DEG);
}

void Servo::Off()
{
    _servo.pulsewidth(0);
}

float Servo::GetAngle() {
    return _angle;   
}

void Servo::GotoMin() {
    _angle = _minAngle;  
    SetServo();
}

void Servo::GotoMax() {
    _angle = _maxAngle;
    SetServo();
}