File content as of revision 22:24c9c2dedca9:

#include "HbManager.h"
#include "globalFlags.h"
#include "uart.h"

//------------------------------------------------------
//姿勢状態取得
//------------------------------------------------------
void HbManager::getAttitude(){
    //姿勢状態取得
    nowAngle = imu->GetYaw();
    nowRate  = imu->GetGyroZ();
    //ヨー角キャリブレーション   
    if(gf_Cal.bf.yaw==true){
        imu->CalYaw();
        gf_Cal.bf.yaw=false;
        sp.printf("Yaw caribration %f\t",nowAngle);
        nowAngle=0;
    }
    //ジャイロオフセットキャリブレーション   
    if(gf_Cal.bf.gy==true){
        imu->CalGyro();
        gf_Cal.bf.gy=false;
        sp.printf("Gyro caribration %d\t",nowRate);
        nowRate=0;
    }
    
    if(gf_Print.bf.yaw) {sp.printf("Ang\t%f\t",nowAngle); }
    if(gf_Print.bf.gy)  {sp.printf("Gyr\t%f\t",nowRate); }
}
//------------------------------------------------------
//姿勢制御
//------------------------------------------------------
void HbManager::controlAttitude(){
    //
    float pidRtn;

  //  if(gf.state == CHK_ATT){
        pidRtn= att->pid(0,nowAngle,nowRate);
  //  }else{
  //      pidRtn =0.0;
  //  }

    //
    if(gf_Print.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
    if(gf_Print.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
    if(gf_Print.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
    if(gf_Print.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
    if(gf_Print.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
    
    //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
    float fl = pidRtn * -1;
    float fr = pidRtn;
    float bl = pidRtn;
    float br = pidRtn * -1;
    
    motorVal[0] = (INT16)fl;//前左
    motorVal[1] = (INT16)fr;//前右
    motorVal[2] = (INT16)bl;//後左
    motorVal[3] = (INT16)br;//後右
}
        
//------------------------------------------------------
//モーター制御
//------------------------------------------------------
void HbManager::controlMotor(){
    
    mot[0]->setValue(motorVal[0]);
    mot[1]->setValue(motorVal[1]);
    mot[2]->setValue(motorVal[2]);
    mot[3]->setValue(motorVal[3]);

    if(gf_Print.bf.m1){ sp.printf("m1=%d,",motorVal[0]); }
    if(gf_Print.bf.m2){ sp.printf("m2=%d,",motorVal[0]); }
    if(gf_Print.bf.m3){ sp.printf("m3=%d,",motorVal[0]); }
    if(gf_Print.bf.m4){ sp.printf("m4=%d,",motorVal[0]); }

}

//------------------------------------------------------
//エンジン制御
//------------------------------------------------------
void HbManager::controlEngine(){
    UINT16  rpm[2];
    int     i;
    
    //エンジン回転数読み出し
    rpm[0] = eng[0]->getRpm();
    rpm[1] = eng[1]->getRpm();
    
    if(gf_Print.bf.e1){ sp.printf("%d",rpm[0]); }
    if(gf_Print.bf.e2){ sp.printf("%d,",rpm[1]); }
    
    //アクセル設定
    for(i=0; i<2; i++){
        if(gf_AxReq[0].bf.req==true){
            sp.printf("val=%d(%X)\r\n" , gf_AxReq[0].bf.val , gf_AxReq[0].dt);
            accelVal[i] = gf_AxReq[i].bf.val;
            eng[i]->setAccell(accelVal[i]);
            gf_AxReq[i].bf.req=false;
        }
    }
    
}
void HbManager::getUserCommand(){
    usrSW = ope->GetUserOpe();
}

//======================================================
//コンストラクタ
//======================================================
HbManager::HbManager(){
    //メンバクラスインスタンス
    eng[0]  = new HbEngine(0);
    eng[1]  = new HbEngine(1);
    att     = new HbAttitude(4.0 , 4 , 0 , 0);//パラメータ outP P I D
    mot[0]  = new HbMotor(0);
    mot[1]  = new HbMotor(1);
    mot[2]  = new HbMotor(2);
    mot[3]  = new HbMotor(3);

//    #include "mbed.h"
    imu     = new Imu(p28,p27);

    ope = new HbUserOpe();

    //初期化    
    motorVal[0]=0;
    motorVal[1]=0;
    motorVal[2]=0;
    motorVal[3]=0;
}