File content as of revision 5:9a1ec02229dd:

#include "mbed.h"
#include "Global.h"
#include "ControllerManager.h"
#include "Joystick.h"
#include "Trim.h"

/*
 *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で
 *（ニュートラルが0.5からずれているかもしれない）、サーボの角度に変換してglobalに値をセットする
 */

//次の目標はlocalメモリ上にニュートラル等の情報を保持し、リセットされる度に、そこから値を取得する
//globalのinitializeを作って、起動毎に呼び出す

//動的なニュートラルの設定と両立したい
//initialize値の変更はinitializeタスクで行う


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=Global::getmaxpitchdegree();
    neutralpitchdegree=Global::getneutralpitchdegree();
    minpitchdegree=Global::getminpitchdegree();
    maxyawdegree=Global::getmaxyawdegree();
    neutralyawdegree=Global::getneutralyawdegree();
    minyawdegree=Global::getminyawdegree();
    
}


//非線形にする時用
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(){
    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();
    
    Global::setpitch(getpitch());
    Global::setyaw(getyaw());
//    printf("%f %f\n", getpitch(), getyaw());
}









//設定した最大、最小値を超えない値を返す
static float clamp(double value, double min, double 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));
}