Diff: ExtendedServo.cpp

Revision:

0:916047fa0d8b

Child:

1:74a29b62a144

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ExtendedServo.cpp	Wed Sep 17 15:17:24 2014 +0000
@@ -0,0 +1,150 @@
+/*
+拡張サーボクラス
+*/
+ 
+#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);
+}
\ No newline at end of file