tarou yamada / servo

Dependents:   serial_connected_mcu_nucleo serial_connected_mcu_nucleo

servo.cpp

Committer:
inst
Date:
2016-06-25
Revision:
5:a4d580b86638
Parent:
4:31ed2ce96ca3

File content as of revision 5:a4d580b86638:

#include "servo.hpp"
#include "mbed.h"

const float servo::_min_position_msec = 0.75f;
const float servo::_max_position_msec = 2.15f;

servo::servo(PinName pin) : _pwm(pin) {
    _pwm.period(20.0f * 0.001f);
}

void servo::set_position(float pos) {
    if (pos >= 1.0f) {
        pos = 1.0f;
    } else if (pos <= -1.0f) {
        pos = -1.0f;
    }
    pos += 1.0f;
    pos *= 0.5f;
    
    float msec = (1.0f - pos) * _min_position_msec + pos * _max_position_msec;
    _pwm.pulsewidth(msec * 0.001f);
}