File content as of revision 2:e0f1e8662b8c:

#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;
    }
}
//ニュートラル基準でピッチ、ヨーともに[-1.0,1.0]の範囲で値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
double ControllerManager::pitchratio(){
    return 2.0*(controller.getV() - neutralpitchdegree)/(maxpitchdegree-minpitchdegree);
}
double ControllerManager::yawratio(){
    return -2.0*(controller.getH() - neutralyawdegree)/(maxpitchdegree-minpitchdegree);
}





//pitchratioを設定したmaxpitch,minpitch倍にする
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));
}