File content as of revision 0:22a971e060ed:

#include "mbed.h"
#include "ServoMotorControl.h"

static float clamp(float value, float min, float max) {
    if(value < min) {
        return min;
    } else if(value > max) {
        return max;
    } else {
        return value;
    }
}


Servo::Servo(PinName pwm) : _pwm(pwm)
{
    _pwm = 0;
}

void Servo::turn2Angle(int degree)
{
    float angle2value;
    switch (degree){
    case 0: angle2value = 1; break;
    case 15: angle2value = 1.25; break;
    case 30: angle2value = 1.5; break;
    case 45: angle2value = 1.75; break;
    case 60: angle2value = 2; break;
    case 75: angle2value = 2.25; break;
    case 90: angle2value = 2.5; break;
    case 105: angle2value = 2.75; break;
    case 120: angle2value = 3; break;
    case 135: angle2value = 3.25; break;
    case 150: angle2value = 3.5; break;
    case 165: angle2value = 3.75; break;
    case 180: angle2value = 4; break;
    default: angle2value = 1; break;}
    turn2pulse(angle2value);
    
}

void Servo::turn2pulse(float value)
{
    //0     1
    //30    1.5
    //60    2
    //90    2.5
    //120   3
    //150   3.5
    //180   4
    if (value < 1)
    {
        value = 1;
    }
    else if (value > 4)
    {
        value = 4;
    }
    value = value / 1500;
    _pwm.pulsewidth(value);
}
float Servo::positionRead()
{
    return _position;
}