File content as of revision 2:695c74c6d483:

#include "CYS8218Controller.hpp"

CYS8218Controller::CYS8218Controller(PinName pwmPin, float initAngle) :
  _servo(pwmPin)
{
  _angle = initAngle;
  ZEROPW = 0.0015f; //1500us default position
  PW_PER_DEG = 0.0005f/45.0f; //500us -> 45deg default
  _servo.period_us(5000);

  if( initAngle != -1 )
    SetServo();
  else
    _servo.pulsewidth(0);
}

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

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

  _servo.pulsewidth(pw);
}

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

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

void CYS8218Controller::Set(float angle)
{
  _angle = angle;
  SetServo();
}

void CYS8218Controller::Move(float relAngle)
{
  _angle += relAngle;
  SetServo();
}

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