DelayServo.cpp

00001 #include <mbed.h>
00002 #include "DelayServo.h"
00003 #include "def_resources.h"
00004 
00005 /**
00006  * 遅延サーボクラス
00007  */
00008 
00009 /**
00010  * コンストラクタ
00011  */
00012 DelayServo::DelayServo(PinName pin) : ExtendedServo(pin)
00013 //初期化リスト
00014 ,mStepAngle(DEFAULT_STEP_ANGLE)
00015 ,mCurrentAngle(0)
00016 ,mInterval(DEFAULT_INTERVAL)
00017 {
00018   //mTick.attach_us(this, &DelayServo::moveServo, mInterval);
00019 };
00020 
00021 /**
00022  * コンストラクタ
00023  */
00024 DelayServo::DelayServo(PinName pin, int stepAngle) : ExtendedServo(pin)
00025 //初期化リスト
00026 ,mStepAngle(stepAngle)
00027 ,mCurrentAngle(0)
00028 ,mInterval(DEFAULT_INTERVAL)
00029 {
00030   //mTick.attach_us(this, &DelayServo::moveServo, mInterval);
00031 };
00032 
00033 /**
00034  * コンストラクタ
00035  */
00036 DelayServo::DelayServo(PinName pin, int stepAngle, int interval) : ExtendedServo(pin)
00037 //初期化リスト
00038 ,mStepAngle(stepAngle)
00039 ,mCurrentAngle(0)
00040 ,mInterval(interval)
00041 {
00042   //mTick.attach_us(this, &DelayServo::moveServo, mInterval);
00043 };
00044 
00045 /**
00046  * コンストラクタ
00047  */
00048 DelayServo::DelayServo(PinName pin, int stepAngle, int interval,
00049   int upperLimit ,int lowerLimit, int offsetAngle, bool isReverse)
00050   : ExtendedServo(pin, upperLimit, lowerLimit, offsetAngle, isReverse)
00051 //初期化リスト
00052 ,mStepAngle(stepAngle)
00053 ,mCurrentAngle(0)
00054 ,mInterval(interval)
00055 {
00056   //mTick.attach_us(this, &DelayServo::moveServo, mInterval);
00057 };
00058 
00059 /**
00060  * デストラクタ
00061  */
00062 DelayServo::~DelayServo() {
00063   //mTick.detach();
00064 }
00065 
00066 /**
00067  * サーボ移動
00068  */
00069 void DelayServo::moveServo(void)
00070 {
00071   //角度計算
00072   if (mTargetAngle - mCurrentAngle >= 0) {//順方向
00073     if (mTargetAngle - mCurrentAngle > mStepAngle) {//目標角度-現在角度が上限角度を超えるなら現在値を上限角度分移動
00074       mCurrentAngle = mCurrentAngle + mStepAngle;
00075     } else {
00076       mCurrentAngle = mTargetAngle;//上限角度以下なら現在角度は目標角度
00077     }
00078   } else {//逆方向
00079     if (mTargetAngle - mCurrentAngle < mStepAngle * -1) {
00080       mCurrentAngle = mCurrentAngle - mStepAngle;// + (mStepAngle * -1)
00081     } else {
00082       mCurrentAngle = mTargetAngle;
00083     }
00084   }
00085   
00086   ExtendedServo::setAngle(mCurrentAngle);
00087 }
00088 
00089 /**
00090  * 更新周期再設定
00091  */
00092 void DelayServo::updateInterval(int interval)
00093 {
00094   //mTick.detach();
00095   mInterval = interval;
00096   //mTick.attach_us(this, &DelayServo::moveServo, mInterval);
00097 }
00098 
00099 /**
00100  * 角度設定
00101  */
00102 void DelayServo::setAngle(int angle)
00103 {
00104   mTargetAngle = angle;
00105   moveServo();
00106 }