Diff: DelayServo.cpp

Revision:

3:9ac9fdf1856e

Child:

4:039a7d1ce3e9

diff -r 91135f19ac12 -r 9ac9fdf1856e DelayServo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DelayServo.cpp	Sat Nov 08 02:45:59 2014 +0000
@@ -0,0 +1,105 @@
+#include <mbed.h>
+#include <DelayServo.h>
+
+/**
+ * 遅延サーボクラス
+ */
+
+/**
+ * コンストラクタ
+ */
+DelayServo::DelayServo(PinName pin) : ExtendedServo(pin)
+//初期化リスト
+,mStepAngle(DEFAULT_STEP_ANGLE)
+,mCurrentAngle(0)
+,mInterval(DEFAULT_INTERVAL)
+{
+  mTick.attach_us(this, &DelayServo::moveServo, mInterval);
+};
+
+/**
+ * コンストラクタ
+ */
+DelayServo::DelayServo(PinName pin, int stepAngle) : ExtendedServo(pin)
+//初期化リスト
+,mStepAngle(stepAngle)
+,mCurrentAngle(0)
+,mInterval(DEFAULT_INTERVAL)
+{
+  mTick.attach_us(this, &DelayServo::moveServo, mInterval);
+};
+
+/**
+ * コンストラクタ
+ */
+DelayServo::DelayServo(PinName pin, int stepAngle, int interval) : ExtendedServo(pin)
+//初期化リスト
+,mStepAngle(stepAngle)
+,mCurrentAngle(0)
+,mInterval(interval)
+{
+  mTick.attach_us(this, &DelayServo::moveServo, mInterval);
+};
+
+/**
+ * コンストラクタ
+ */
+DelayServo::DelayServo(PinName pin, int stepAngle, int interval,
+  int upperLimit ,int lowerLimit, int offsetAngle, bool isReverse)
+  : ExtendedServo(pin, upperLimit, lowerLimit, offsetAngle, isReverse)
+//初期化リスト
+,mStepAngle(stepAngle)
+,mCurrentAngle(0)
+,mInterval(interval)
+{
+  mTick.attach_us(this, &DelayServo::moveServo, mInterval);
+};
+
+/**
+ * デストラクタ
+ */
+DelayServo::~DelayServo() {
+  mTick.detach();
+}
+
+/**
+ * サーボ移動
+ */
+void DelayServo::moveServo(void)
+{
+  //角度計算
+  if (mTargetAngle - mCurrentAngle >= 0) {//順方向
+    if (mTargetAngle - mCurrentAngle > mStepAngle) {//目標角度-現在角度が上限角度を超えるなら現在値を上限角度分移動
+      mCurrentAngle = mCurrentAngle + mStepAngle;
+    } else {
+      mCurrentAngle = mTargetAngle;//上限角度以下なら現在角度は目標角度
+    }
+  } else {//逆方向
+    if (mTargetAngle - mCurrentAngle < mStepAngle * -1) {
+      mCurrentAngle = mCurrentAngle - mStepAngle;// + (mStepAngle * -1)
+    } else {
+      mCurrentAngle = mTargetAngle;
+    }
+  }
+  
+  ExtendedServo::setAngle(mCurrentAngle);
+}
+
+/**
+ * 更新周期再設定
+ */
+void DelayServo::updateInterval(int interval)
+{
+  mTick.detach();
+  mInterval = interval;
+  mTick.attach_us(this, &DelayServo::moveServo, mInterval);
+}
+
+/**
+ * 角度設定
+ */
+void DelayServo::setAngle(int angle)
+{
+  mTargetAngle = angle;
+  moveServo();
+}
\ No newline at end of file