File content as of revision 0:d6402bcd58f7:

#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);
}