File content as of revision 8:ca92cb674004:

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

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


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();
    
    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(){
    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(controller.getV());//生データであることに注意
    Global::setyawdegree(controller.getH());//生データであることに注意
//    printf("pitch=%f yaw=%f\n", getpitch(), getyaw());
//    printf("pitchdeg(raw)=%f yawdeg(raw)=%f\n", getpitchdegree(), getyawdegree());
}




//設定した最大、最小値を超えない値を返す
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を返す)
double ControllerManager::pitchratio(){
    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(){
    if(controller.getH() < neutralyawdegree){
        return clamp((controller.getH() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
    }else{
        return clamp((controller.getH() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
    }
}

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;
    }
}
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(){
    return clamp(doublepitch(pitchratio())+Global::gettrimpitch(), minpitch, maxpitch);
}
double ControllerManager::getyaw(){
    return clamp(doubleyaw(yawratio())+Global::gettrimyaw(), minyaw, maxyaw);
}

double ControllerManager::getpitch(double _pitchratio){
    return doublepitch(_pitchratio);
}
double ControllerManager::getyaw(double _yawratio){
    return doubleyaw(_yawratio);
}