Diff: Control/ControllerManager.cpp

Revision:

0:d6402bcd58f7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/ControllerManager.cpp	Sat Dec 01 15:10:42 2018 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "Global.h"
+#include "ControllerManager.h"
+#include "Trim.h"
+
+/*
+ *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で
+ *（ニュートラルが0.5からずれているかもしれない）、trim値を加え、
+ *サーボの角度に変換してglobalに値をセットする
+ */
+
+//コンストラクタでpitch, yawのデフォルトGlobal内から取ってきて、セット
+ControllerManager::ControllerManager(PinName h, PinName v, PinName hTrimup, PinName hTrimdown)
+    : controllerH(h), controllerV(v), pitchtrim(hTrimup, hTrimdown){
+    maxpitch = Global::getmaxpitch();
+    minpitch = Global::getminpitch();
+    maxyaw = Global::getmaxyaw();
+    minyaw = Global::getminyaw();
+    
+    maxpitchdegree=Global::getmaxpitchdegree();
+    neutralpitchdegree=Global::getneutralpitchdegree();
+    minpitchdegree=Global::getminpitchdegree();
+    maxyawdegree=Global::getmaxyawdegree();
+    neutralyawdegree=Global::getneutralyawdegree();
+    minyawdegree=Global::getminyawdegree();
+    
+    maxpitchplayratio = Global::getmaxpitchplayratio();
+    minpitchplayratio = Global::getminpitchplayratio();
+    maxyawplayratio = Global::getmaxyawplayratio();
+    minyawplayratio = Global::getminyawplayratio();
+}
+
+
+//非線形にする時用．今回は別の関数で非線形？にしている
+double ControllerManager::calc(double doublex,int intx){
+    return doublex;
+//    return intx+pow(doublex-intx,3);
+}
+
+
+
+
+void ControllerManager::update(){
+    //updateのたびにめったに変化しないmax,min,neutralの値をglobalから読み込むの非効率な気がする。もっといいやりかたがあるはず...
+    maxpitch = Global::getmaxpitch();
+    minpitch = Global::getminpitch();
+    maxyaw = Global::getmaxyaw();
+    minyaw = Global::getminyaw();
+    maxpitchdegree=Global::getmaxpitchdegree();
+    neutralpitchdegree=Global::getneutralpitchdegree();
+    minpitchdegree=Global::getminpitchdegree();
+    maxyawdegree=Global::getmaxyawdegree();
+    neutralyawdegree=Global::getneutralyawdegree();
+    minyawdegree=Global::getminyawdegree();
+    maxpitchplayratio = Global::getmaxpitchplayratio();
+    minpitchplayratio = Global::getminpitchplayratio();
+    maxyawplayratio = Global::getmaxyawplayratio();
+    minyawplayratio = Global::getminyawplayratio();
+
+    
+    Global::setpitch(getpitch());
+    Global::setyaw(getyaw());
+    Global::setpitchdegree(controllerV.read());//生データであることに注意
+    Global::setyawdegree(controllerH.read());//生データであることに注意
+    Global::setinttrimpitch(pitchtrim.getint());
+}
+
+
+
+
+//設定した最大、最小値を超えない値を返す
+static float clamp(double value, double min, double max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+//ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
+//式中のパラメータは全てAnalogIn,[0, 1]の範囲であることに注意
+double ControllerManager::pitchratio(){
+    if(controllerV.read() < neutralpitchdegree){
+        return clamp((controllerV.read() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
+    }else{
+        return clamp((controllerV.read() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
+    }
+}
+double ControllerManager::yawratio(){
+    if(controllerH.read() < neutralyawdegree){
+        return clamp((controllerH.read() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
+    }else{
+        return clamp((controllerH.read() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
+    }
+}
+
+//pitchratioに遊びの追加
+//wikiに詳細が載っている
+double ControllerManager::pitchratioplayed(double pitchratio){
+    if(pitchratio > maxpitchplayratio){
+        return (pitchratio - maxpitchplayratio) / (1.0 - maxpitchplayratio);
+    }else if(pitchratio < minpitchplayratio){
+        return (pitchratio - minpitchplayratio) / (minpitchplayratio + 1.0);
+    }else{
+        return 0;
+    }
+}
+//pitchratioplayedと同様
+double ControllerManager::yawratioplayed(double yawratio){
+    if(yawratio > maxyawplayratio){
+        return (yawratio - maxyawplayratio) / (1.0 - maxyawplayratio);
+    }else if(yawratio < minyawplayratio){
+        return (yawratio - minyawplayratio) / (minyawplayratio + 1.0);
+    }else{
+        return 0;
+    }
+}
+
+
+//pitchratioを設定したmaxpitch,minpitch倍にする（ニュートラル付近の値の加工もここで行う）
+double ControllerManager::doublepitch(double pitchratio){
+    if(pitchratio<0){
+       return -pitchratioplayed(pitchratio)*minpitch;
+    }else{
+        return pitchratioplayed(pitchratio)*maxpitch;
+    }
+}
+double ControllerManager::doubleyaw(double yawratio){
+    if(yawratio<0){
+        return -yawratioplayed(yawratio)*minyaw;
+    }else{
+        return yawratioplayed(yawratio)*maxyaw;
+    }
+}
+int ControllerManager::intpitch(double pitchratio){
+    if(pitchratio<0){
+       return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
+    }else{
+        return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
+    }
+}
+int ControllerManager::intyaw(double yawratio){
+    if(yawratio<0){
+        return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
+    }else{
+        return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
+    }
+}
+
+
+
+//ここでtrim値を加える
+double ControllerManager::getpitch(){//trim値を加えた結果maxminを超えないようにclamp
+    return clamp(doublepitch(pitchratio())+pitchtrim.get(Global::gettrimpitchrate()), minpitch, maxpitch);
+}
+double ControllerManager::getyaw(){
+    return clamp(doubleyaw(yawratio()), minyaw, maxyaw);
+}
+
+//おそらくデバッグ用(pitch, yawが正しく計算されているか、確認)
+double ControllerManager::getpitch(double _pitchratio){
+    return doublepitch(_pitchratio);
+}
+double ControllerManager::getyaw(double _yawratio){
+    return doubleyaw(_yawratio);
+}
\ No newline at end of file