Diff: Control/ControllerManager.cpp

Revision:

8:ca92cb674004

Parent:

7:6f7bd18ce796

Child:

9:d1fc0805ec7d

diff -r 6f7bd18ce796 -r ca92cb674004 Control/ControllerManager.cpp
--- a/Control/ControllerManager.cpp	Sat Oct 10 22:53:05 2015 +0000
+++ b/Control/ControllerManager.cpp	Tue Oct 13 13:10:21 2015 +0000
@@ -5,7 +5,7 @@
 #include "Trim.h"
 
 /*
- *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で
+ *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で、trim値を加え、
  *（ニュートラルが0.5からずれているかもしれない）、サーボの角度に変換してglobalに値をセットする
  */
 
@@ -24,10 +24,14 @@
     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);
@@ -36,125 +40,6 @@
 
 
 
-
-
-
-/*
-void ControllerManager::initialize(){
-
-    if(!Global::initializeswitch){
-        printf("startcontollerinitialize\r\n");
-        printf("neutralcontrollerdegree?\r\n");
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        wait(0.2);
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        wait(0.2);
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        wait(0.2);
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        wait(0.2);
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        wait(0.2);
-        Global::led1=1;
-        Global::led2=1;
-        wait(0.2);
-        Global::led1=0;
-        Global::led2=0;
-        neutralpitchdegree=controller.getV();
-        Global::setneutralpitchdegree(neutralpitchdegree);
-        neutralyawdegree=controller.getH();
-        Global::setneutralyawdegree(neutralyawdegree);
-        
-        
-        printf("neutralpitchdegree=%f\tneutralyawdegree=%f\r\n",neutralpitchdegree,neutralyawdegree);
-        wait(1.0);
-        printf("maxcontrollerdegree?\r\n");
-        Global::led1=1;
-        wait(0.2);
-        Global::led1=0;
-        wait(0.2);
-        Global::led1=1;
-        wait(0.2);
-        Global::led1=0;
-        wait(0.2);
-        Global::led1=1;
-        wait(0.2);
-        Global::led1=0;
-        wait(0.2);
-        Global::led1=1;
-        wait(0.2);
-        Global::led1=0;
-        wait(0.2);
-        Global::led1=1;
-        wait(0.2);
-        Global::led1=0;
-        while(Global::initializeswitch){
-            Global::led1=1;
-            wait(0.1);
-        }
-        maxpitchdegree=controller.getV();
-        Global::setmaxpitchdegree(maxpitchdegree);
-        maxyawdegree=controller.getH();
-        Global::setmaxyawdegree(maxyawdegree);
-        printf("maxpitchdegree=%f\tmaxyawdegree=%f\r\n",maxpitchdegree,maxyawdegree);
-        Global::led1=0;
-        wait(1.0);
-        printf("mincontrollerdegree?\r\n");
-        Global::led2=1;
-        wait(0.2);
-        Global::led2=0;
-        wait(0.2);
-        Global::led2=1;
-        wait(0.2);
-        Global::led2=0;
-        wait(0.2);
-        Global::led2=1;
-        wait(0.2);
-        Global::led2=0;
-        wait(0.2);
-        Global::led2=1;
-        wait(0.2);
-        Global::led2=0;
-        wait(0.2);
-        Global::led2=1;
-        wait(0.2);
-        Global::led2=0;
-        while(Global::initializeswitch){
-            Global::led2=1;
-            wait(0.1);
-        }
-        minpitchdegree=controller.getV();
-        Global::setminpitchdegree(minpitchdegree);
-        minyawdegree=controller.getH();
-        Global::setminyawdegree(minyawdegree);
-        printf("minpitchdegree=%f\tminyawdegree=%f\r\n",minpitchdegree,minyawdegree);
-        Global::led2=0;
-        wait(1.0);
-    }
-    printf("endcontollerinitialize\r\n");
-    wait(1.0);
-}*/
-
 void ControllerManager::update(){
     maxpitch = Global::getmaxpitch();
     minpitch = Global::getminpitch();
@@ -166,20 +51,23 @@
     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());
-//    printf("%f %f\n", getpitch(), getyaw());
+    Global::setpitchdegree(controller.getV());//生データであることに注意
+    Global::setyawdegree(controller.getH());//生データであることに注意
+//    printf("pitch=%f yaw=%f\n", getpitch(), getyaw());
+//    printf("pitchdeg(raw)=%f yawdeg(raw)=%f\n", getpitchdegree(), getyawdegree());
 }
 
 
 
 
-
-
-
-
-
 //設定した最大、最小値を超えない値を返す
 static float clamp(double value, double min, double max) {
     if(value < min) {
@@ -190,7 +78,7 @@
         return value;
     }
 }
-//ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
+//ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
 double ControllerManager::pitchratio(){
     if(controller.getV() < neutralpitchdegree){
         return clamp((controller.getV() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
@@ -206,20 +94,39 @@
     }
 }
 
+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;
+    }
+}
+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倍にする
+
+//pitchratioを設定したmaxpitch,minpitch倍にする（ニュートラル付近の値の加工もここで行う）
 double ControllerManager::doublepitch(double pitchratio){
     if(pitchratio<0){
-       return clamp(-pitchratio*minpitch,minpitch,maxpitch);
+       return -pitchratioplayed(pitchratio)*minpitch;
     }else{
-        return clamp(pitchratio*maxpitch,minpitch,maxpitch);
+        return pitchratioplayed(pitchratio)*maxpitch;
     }
 }
 double ControllerManager::doubleyaw(double yawratio){
     if(yawratio<0){
-        return clamp(-yawratio*minyaw,minyaw,maxyaw);
+        return -yawratioplayed(yawratio)*minyaw;
     }else{
-        return clamp(yawratio*maxyaw,minyaw,maxyaw);
+        return yawratioplayed(yawratio)*maxyaw;
     }
 }
 int ControllerManager::intpitch(double pitchratio){
@@ -239,16 +146,17 @@
 
 
 
-
+//ここでtrim値を加える
 double ControllerManager::getpitch(){
-    return calc(doublepitch(pitchratio()),intpitch(pitchratio()));
+    return clamp(doublepitch(pitchratio())+Global::gettrimpitch(), minpitch, maxpitch);
 }
 double ControllerManager::getyaw(){
-    return calc(doubleyaw(yawratio()),intyaw(yawratio()));
+    return clamp(doubleyaw(yawratio())+Global::gettrimyaw(), minyaw, maxyaw);
 }
+
 double ControllerManager::getpitch(double _pitchratio){
-    return calc(doublepitch(_pitchratio),intpitch(_pitchratio));
+    return doublepitch(_pitchratio);
 }
 double ControllerManager::getyaw(double _yawratio){
-    return calc(doubleyaw(_yawratio),intyaw(_yawratio));
+    return doubleyaw(_yawratio);
 }
\ No newline at end of file