Diff: Control/ControllerManager.cpp

Revision:

0:2a15bd367891

Child:

2:e0f1e8662b8c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/ControllerManager.cpp	Mon Sep 07 01:19:55 2015 +0000
@@ -0,0 +1,235 @@
+#include "mbed.h"
+#include "Global.h"
+#include "ControllerManager.h"
+#include "Joystick.h"
+#include "Trim.h"
+
+
+//係数変更でパイロットに合わせるたぶん使わん
+ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
+    : controller(b, h, v){
+    maxpitch = Global::getmaxpitch();//
+    minpitch = Global::getminpitch();//
+    maxyaw = Global::getmaxyaw();//
+    minyaw = Global::getminyaw();//
+    
+    
+    maxpitchdegree=1.0;
+    neutralpitchdegree=0.5;
+    minpitchdegree=0.0;
+    maxyawdegree=1.0;
+    neutralyawdegree=0.5;
+    minyawdegree=0.0;
+    
+}
+
+
+//非線形にする時用
+double ControllerManager::calc(double doublex,int intx){
+    return doublex;
+//    return intx+pow(doublex-intx,3);
+}
+
+
+
+
+
+
+
+
+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(){
+    Global::setpitch(getpitch() + Global::gettrimpitch());
+    Global::setyaw(getyaw() + Global::gettrimyaw());
+    printf("cont\n");
+    
+}
+
+
+
+
+
+
+
+
+
+
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+double ControllerManager::pitchratio(){
+    return 2.0*(controller.getV() - neutralpitchdegree)/(maxpitchdegree-minpitchdegree);
+}
+double ControllerManager::yawratio(){
+    return -2.0*(controller.getH() - neutralyawdegree)/(maxpitchdegree-minpitchdegree);
+}
+
+
+
+
+
+
+double ControllerManager::doublepitch(double pitchratio){
+    if(pitchratio<0){
+       return clamp(-pitchratio*minpitch,minpitch,maxpitch);
+    }else{
+        return clamp(pitchratio*maxpitch,minpitch,maxpitch);
+    }
+}
+double ControllerManager::doubleyaw(double yawratio){
+    if(yawratio<0){
+        return clamp(-yawratio*minyaw,minyaw,maxyaw);
+    }else{
+        return clamp(yawratio*maxyaw,minyaw,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);
+    }
+}
+
+
+
+
+double ControllerManager::getpitch(){
+    return calc(doublepitch(pitchratio()),intpitch(pitchratio()));
+}
+double ControllerManager::getyaw(){
+    return calc(doubleyaw(yawratio()),intyaw(yawratio()));
+}
+double ControllerManager::getpitch(double _pitchratio){
+    return calc(doublepitch(_pitchratio),intpitch(_pitchratio));
+}
+double ControllerManager::getyaw(double _yawratio){
+    return calc(doubleyaw(_yawratio),intyaw(_yawratio));
+}
\ No newline at end of file