FRC - Hackathon / RC_Servo

Pulse Width Modulation RC Servomotor Library compatible with classic and extended models. Models limit are customizable

Dependents:   FRC_2018 0hackton_08_06_18 0hackton_08_06_18_publish Kenya_2019 ... more

RC_Servo.cpp

Committer:
haarkon
Date:
2018-06-05
Revision:
6:cf65fc8b0de1
Parent:
1:8482eba4d652
Child:
7:014d36c33b73

File content as of revision 6:cf65fc8b0de1:

#include "RC_Servo.h"

RC_Servo::RC_Servo(PinName PWM, int _extended) : _RCpwm(PWM)
{
    _tickRC.attach(callback(this,&RC_Servo::generatePwm),0.02);
    if (_extended) {
        _RCpMin = 400;
        _RCpMax = 2400;
    } else {
        _RCpMin = 1000;
        _RCpMax = 2000;
    }
}

void RC_Servo::generatePwm (void)
{

    static int  output = 0;
    static long pulseWidthTime = 0;

    if (pulseWidthTime == 0) pulseWidthTime = _RCpMin;
    _tickRC.detach();
    output = !output;
    _RCpwm = output;

    if (output == 1)    _tickRC.attach (callback(this,&RC_Servo::generatePwm), pulseWidthTime);
    else {
        _tickRC.attach (callback(this,&RC_Servo::generatePwm), 20000-pulseWidthTime);
        pulseWidthTime = RCdelay;
    }
}

int RC_Servo::setLimits (int Tmin, int Tmax)
{
    if ((Tmin > 400) && (Tmin < Tmax))  _RCpMin = Tmin;
    else return Tmin;
    if ((Tmax < 2400) && (Tmin < Tmax)) _RCpMax = Tmax;
    else return Tmax;
    return 0;
}

void RC_Servo::write (float position)
{
    if ((position >= 0) && (position <= 1))
        RCdelay = _RCpMin + (long)((_RCpMax - _RCpMin) * position);
}

RC_Servo& RC_Servo::operator= (float position)
{
    write (position);
    return *this;
}