File content as of revision 0:916047fa0d8b:

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


ExtendedServo::ExtendedServo(PinName pin)
{
  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) {
  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) {
  mPwmServo = new PwmServo(pin);
  mOffsetAngle = offsetAngle;
  mIsReverse = isReverse;
  mUpperLimit = upperLimit;
  mLowerLimit = lowerLimit;
  setAngle(mOffsetAngle);
}
 
 
//TODO:デストラクタ
ExtendedServo::~ExtendedServo() {
  //delete hoge;
}
 
//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::convertAngle(int angle) {
  return 180 - (angle + 90);
}
 
//方向逆転
int ExtendedServo::reverseAngle(int angle, bool isReverse) {
 
  if (isReverse) {
    angle = angle * -1; 
  }
  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 = convertAngle(virtualAngle);
 
  //オフセットを加算
  actualAngle = actualAngle + mOffsetAngle;
 
  //方向逆転が必要なら逆転
  actualAngle = reverseAngle(actualAngle, mIsReverse);
 
  //上限下限の丸め込み
  actualAngle = roundAngle(actualAngle, mUpperLimit, mLowerLimit);
 
  //PWMクラスインスタンスに設定
  //if(mServo != NULL) if(mServo.isTrue()) {
    mServo.setAngle(actualAngle);
  //} else {
    //ERROR
  //}
}
 
 
//原点復帰（0度設定時動作と同じ）
void ExtendedServo::backToStartPoint() {
  setAngle(0);
}