ExtendedServo.cpp

00001 #include <mbed.h>
00002 #include "ExtendedServo.h"
00003 
00004 /**
00005  * 拡張サーボクラス
00006  */
00007 
00008 /**
00009  * コンストラクタ
00010  */
00011 ExtendedServo::ExtendedServo(PinName pin) : PwmServo(pin)
00012 //初期化リスト
00013 ,mOffsetAngle(0)
00014 ,mIsReverse(false)
00015 ,mUpperLimit(180)
00016 ,mLowerLimit(0)
00017 {
00018     setAngle(mOffsetAngle);
00019 };
00020  
00021 /**
00022  * コンストラクタ
00023  */
00024 ExtendedServo::ExtendedServo(PinName pin, int upperLimit, int lowerLimit) : PwmServo(pin)
00025 //初期化リスト
00026 ,mOffsetAngle(0)
00027 ,mIsReverse(false)
00028 ,mUpperLimit(upperLimit)
00029 ,mLowerLimit(lowerLimit)
00030 {
00031     setAngle(mOffsetAngle);
00032 };
00033  
00034 /**
00035  * コンストラクタ
00036  */
00037 ExtendedServo::ExtendedServo(PinName pin, int upperLimit, int lowerLimit, int offsetAngle, bool isReverse) : PwmServo(pin)
00038 //初期化リスト
00039 ,mOffsetAngle(offsetAngle)
00040 ,mIsReverse(isReverse)
00041 ,mUpperLimit(upperLimit)
00042 ,mLowerLimit(lowerLimit)
00043 {
00044     setAngle(mOffsetAngle);
00045 };
00046  
00047 /**
00048  * デストラクタ
00049  */
00050 ExtendedServo::~ExtendedServo() {
00051     //Nothing to do.
00052 }
00053 
00054 //setter,getter
00055 void ExtendedServo::setOffsetAngle(int angle)
00056 {
00057     mOffsetAngle = angle;
00058 }
00059  
00060 int ExtendedServo::getOffsetAngle()
00061 {
00062     return mOffsetAngle;
00063 }
00064 
00065 void ExtendedServo::setDirection(bool isReverse)
00066 {
00067     mIsReverse = isReverse;
00068 }
00069  
00070 bool ExtendedServo::getDirection()
00071 {
00072     return mIsReverse;
00073 }
00074  
00075 void ExtendedServo::setUpperLimit(int angle)
00076 {
00077     mUpperLimit = angle;
00078 }
00079  
00080 int ExtendedServo::getUpperLimit()
00081 {
00082     return mUpperLimit;
00083 }
00084  
00085 void ExtendedServo::setLowerLimit(int angle)
00086 {
00087     mLowerLimit = angle;
00088 }
00089  
00090 int ExtendedServo::getLowerLimit()
00091 {
00092     return mLowerLimit;
00093 }
00094 
00095 /**
00096  * 方向逆転
00097  */
00098 int ExtendedServo::reverseAngle(int angle, bool isReverse) {
00099  
00100     if (isReverse) {
00101         angle = 180 - angle;
00102     }
00103     return angle;
00104 }
00105 
00106 /**
00107  * 上限下限の丸め込み
00108  */
00109 int ExtendedServo::roundAngle(int angle, int upperLimit, int lowerLimit) {
00110  
00111     if(angle > upperLimit) {
00112         angle = upperLimit;
00113     } else if (angle < lowerLimit) {
00114         angle = lowerLimit;
00115     } else {
00116         //Nothing to do.
00117     }
00118     return angle;
00119 }
00120 
00121 /**
00122  * 角度設定
00123  */
00124 void ExtendedServo::setAngle(int virtualAngle) {
00125     int actualAngle = 0;
00126   
00127     //オフセットを加算
00128     actualAngle = virtualAngle + mOffsetAngle;
00129  
00130     //方向逆転が必要なら逆転
00131     actualAngle = reverseAngle(actualAngle, mIsReverse);
00132  
00133     //上限下限の丸め込み
00134     actualAngle = roundAngle(actualAngle, mUpperLimit, mLowerLimit);
00135  
00136     //基底クラスのインスタンスに角度(絶対値)を設定
00137     PwmServo::setAngle(actualAngle);
00138 }
00139 
00140 /**
00141  * 原点回帰
00142  */
00143 void ExtendedServo::backToStartPoint() {
00144     ExtendedServo::setAngle(0);
00145 }