File content as of revision 1:74a29b62a144:

/*
拡張サーボクラス
*/
 
#include <mbed.h>
#include <ExtendedServo.h>

ExtendedServo::ExtendedServo(PinName pin)
{
  if (mPwmServo != NULL) {
    delete mPwmServo ;
  }
  mPwmServo = new PwmServo(pin);
  mOffsetAngle = 0;
  mIsReverse   = false;
  mUpperLimit  = 180;
  mLowerLimit  = 0;
  setAngle(mOffsetAngle);
}
  //初期化子を使用した記述
  /*:mOffsetAngle(0)
  ,mIsReverse(false)
  ,mUpperLimit(180)
  ,mLowerLimit(0)
  {
    setAngle(mOffsetAngle);
  };*/
 
 
ExtendedServo::ExtendedServo(PinName pin, int upperLimit , int lowerLimit) {
  if (mPwmServo != NULL) {
    delete mPwmServo ;
  }
  mPwmServo = new PwmServo(pin);  
  mOffsetAngle = 0;
  mIsReverse   = false;
  mUpperLimit  = upperLimit;
  mLowerLimit  = lowerLimit;
  setAngle(mOffsetAngle);
}
 
 
ExtendedServo::ExtendedServo(PinName pin, int upperLimit ,int lowerLimit, int offsetAngle, bool isReverse) {
  if (mPwmServo != NULL) {
    delete mPwmServo ;
  }
  mPwmServo = new PwmServo(pin);
  mOffsetAngle = offsetAngle;
  mIsReverse = isReverse;
  mUpperLimit = upperLimit;
  mLowerLimit = lowerLimit;
  setAngle(mOffsetAngle);
}
 
 
ExtendedServo::~ExtendedServo() {
  if (mPwmServo != NULL) {
    delete mPwmServo ;
    mPwmServo  = NULL;
  }
}
 
//setter,getter
void ExtendedServo::setOffsetAngle(int angle)
{
  mOffsetAngle = angle;
}
 
int ExtendedServo::getOffsetAngle()
{
  return mOffsetAngle;
}

void ExtendedServo::setDirection(bool isReverse)
{
  mIsReverse = isReverse;
}
 
int 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;
}
 
 
//サーボ角度設定（-90～+90）
void ExtendedServo::setAngle(int virtualAngle) {
  int actualAngle = 0;
 
  //オフセットを加算
  actualAngle = virtualAngle + mOffsetAngle;
 
  //方向逆転が必要なら逆転
  actualAngle = reverseAngle(actualAngle, mIsReverse);
 
  //上限下限の丸め込み
  actualAngle = roundAngle(actualAngle, mUpperLimit, mLowerLimit);
 
  //PWMクラスインスタンスに設定
  if(mPwmServo != NULL) {
    mPwmServo->setAngle(actualAngle);
  } else {
    //ERROR
  }
}
 
 
//原点復帰（0度設定時動作と同じ）
void ExtendedServo::backToStartPoint() {
  setAngle(0);
}