Diff: Control/ControllerManager.cpp

Revision:

7:6f7bd18ce796

Parent:

6:0d9fa7152934

Child:

8:ca92cb674004

--- a/Control/ControllerManager.cpp	Sat Oct 10 20:16:31 2015 +0000
+++ b/Control/ControllerManager.cpp	Sat Oct 10 22:53:05 2015 +0000
@@ -9,9 +9,6 @@
  *（ニュートラルが0.5からずれているかもしれない）、サーボの角度に変換してglobalに値をセットする
  */
 
-//次の目標はlocalメモリ上にニュートラル等の情報を保持し、リセットされる度に、そこから値を取得する
-//globalのinitializeを作って、起動毎に呼び出す
-
 
 ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
     : controller(b, h, v){
@@ -193,18 +190,23 @@
         return value;
     }
 }
-//ニュートラル基準でピッチ、ヨーともに[-1.0,1.0]の範囲で値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
+//ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
 double ControllerManager::pitchratio(){
-    return 2.0*(controller.getV() - neutralpitchdegree)/(maxpitchdegree-minpitchdegree);
+    if(controller.getV() < neutralpitchdegree){
+        return clamp((controller.getV() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
+    }else{
+        return clamp((controller.getV() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
+    }
 }
 double ControllerManager::yawratio(){
-    return -2.0*(controller.getH() - neutralyawdegree)/(maxpitchdegree-minpitchdegree);
+    if(controller.getH() < neutralyawdegree){
+        return clamp((controller.getH() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
+    }else{
+        return clamp((controller.getH() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
+    }
 }
 
 
-
-
-
 //pitchratioを設定したmaxpitch,minpitch倍にする
 double ControllerManager::doublepitch(double pitchratio){
     if(pitchratio<0){