File content as of revision 6:8364e8639b4e:

#include <mbed.h>
#include <ExtendedServo.h>

/**
 * 拡張サーボクラス
 */

/**
 * コンストラクタ
 */
ExtendedServo::ExtendedServo(PinName pin) : PwmServo(pin)
//初期化リスト
,mOffsetAngle(0)
,mIsReverse(false)
,mUpperLimit(180)
,mLowerLimit(0)
{
  setAngle(mOffsetAngle);
};
 
/**
 * コンストラクタ
 */
ExtendedServo::ExtendedServo(PinName pin, int upperLimit , int lowerLimit) : PwmServo(pin)
//初期化リスト
,mOffsetAngle(0)
,mIsReverse(false)
,mUpperLimit(upperLimit)
,mLowerLimit(lowerLimit)
{
  setAngle(mOffsetAngle);
};
 
/**
 * コンストラクタ
 */
ExtendedServo::ExtendedServo(PinName pin, int upperLimit ,int lowerLimit, int offsetAngle, bool isReverse) : PwmServo(pin)
//初期化リスト
,mOffsetAngle(offsetAngle)
,mIsReverse(isReverse)
,mUpperLimit(upperLimit)
,mLowerLimit(lowerLimit)
{
  setAngle(mOffsetAngle);
};
 
/**
 * デストラクタ
 */
ExtendedServo::~ExtendedServo() {
  //Nothing to do.
}

//setter,getter
void ExtendedServo::setOffsetAngle(int angle)
{
  mOffsetAngle = angle;
}
 
int ExtendedServo::getOffsetAngle()
{
  return mOffsetAngle;
}

void ExtendedServo::setDirection(bool isReverse)
{
  mIsReverse = isReverse;
}
 
bool ExtendedServo::getDirection()
{
  return mIsReverse;
}
 
void ExtendedServo::setUpperLimit(int angle)
{
  mUpperLimit = angle;
}
 
int ExtendedServo::getUpperLimit()
{
  return mUpperLimit;
}
 
void ExtendedServo::setLowerLimit(int angle)
{
  mLowerLimit = angle;
}
 
int ExtendedServo::getLowerLimit()
{
  return mLowerLimit;
}

/**
 * 方向逆転
 */
int ExtendedServo::reverseAngle(int angle, bool isReverse) {
 
  if (isReverse) {
    angle = 180 - angle;
  }
  return angle;
}

/**
 * 上限下限の丸め込み
 */
int ExtendedServo::roundAngle(int angle, int upperLimit, int lowerLimit) {
 
  if(angle > upperLimit) {
    angle = upperLimit;
  } else if (angle < lowerLimit) {
    angle = lowerLimit;
  } else {
    //Nothing to do.
  }
  return angle;
}

/**
 * 角度設定
 */
void ExtendedServo::setAngle(int virtualAngle) {
  int actualAngle = 0;
  
  //オフセットを加算
  actualAngle = virtualAngle + mOffsetAngle;
 
  //方向逆転が必要なら逆転
  actualAngle = reverseAngle(actualAngle, mIsReverse);
 
  //上限下限の丸め込み
  actualAngle = roundAngle(actualAngle, mUpperLimit, mLowerLimit);
 
  //基底クラスのインスタンスに角度(絶対値)を設定
  PwmServo::setAngle(actualAngle);
}

/**
 * 原点回帰
 */
void ExtendedServo::backToStartPoint() {
  ExtendedServo::setAngle(0);
}