File content as of revision 33:eb260dbfc22a:

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

//------------------------------------------------------
//姿勢状態取得
//------------------------------------------------------
void HbManager::getAttitude(){
    float tmpf;
    INT16 tmpi;
    //姿勢状態取得
    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) {
        tmpf = imu->GetYawRef();
        sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf);
    }
    if(gf_Print.bf.gy)  {
        tmpi = imu->GetGyroRef();
        sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi);
    }
}
void HbManager::calAtt(){
    if(att == NULL) return;
    if(imu == NULL) return;
    imu->CalYaw();
    sp.printf("Yaw auto caribration %f\t",nowAngle);
    nowAngle=0;
    imu->CalGyro();
    sp.printf("Gyro auto caribration %d\t\r\n",nowRate);
    nowRate=0;
}

//------------------------------------------------------
//姿勢制御
//------------------------------------------------------
void HbManager::controlAttitude(){
    //
    float pidRtn;
    float pidRtn2;

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

    //
    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.v) {sp.printf("PID:V=%f,",att->getV()); }
    if(gf_Print.bf.fb){
        sp.printf("PID FB,%f,",pidRtn);
        sp.printf("%f,",pidRtn2);
        sp.printf("%f,",nowAngle);
        sp.printf("%f,",nowRate);
        }
    
    //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
    // float fl = pidRtn * -1;
    // float fr = pidRtn;
    // float bl = pidRtn;
    // float br = pidRtn * -1;
    // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
    float fl = pidRtn * 2.25 * -1;
    float fr = pidRtn * 2.25;
    float bl = pidRtn * 2.25;
    float br = pidRtn * 2.25 * -1;
    for(int i = 0; i < 4; ++i){
        gf_MtReq[i].req = true;
        //gf_MtReq[i].req = false;
    }
    gf_MtReq[0].val = (INT16)fl;
    gf_MtReq[1].val = (INT16)fr;
    gf_MtReq[2].val = (INT16)bl;
    gf_MtReq[3].val = (INT16)br;
    // motorVal[0] = (INT16)fl;//前左
    // motorVal[1] = (INT16)fr;//前右
    // motorVal[2] = (INT16)bl;//後左
    // motorVal[3] = (INT16)br;//後右
}

void HbManager::controlAttitude(float cmdAngle){
    //
    float pidRtn;

    pidRtn= att->pid(cmdAngle,nowAngle,nowRate);
    //
    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::setAttPara(typPidPara para){
    if(att == NULL) return;
    att->setPp(para.PP);
    att->setP(para.P);
    att->setI(para.I);
    att->setD(para.D);
    att->setIMax(para.IMax);
    att->setIMin(para.IMin);
    att->setV(para.V);
}

//------------------------------------------------------
//モーター制御
//------------------------------------------------------
void HbManager::controlMotor(){
    INT16 tmp,dif;
    for(int i = 0; i < 4; ++i){
        if(gf_MtReqOfs[i].req){
            tmp = subProp[i]->getValue(OFS);
            dif = tmp - (INT16)gf_MtReqOfs[i].val;
            if(dif == 0){
                gf_MtReqOfs[i].req = false;
            }
            else {
                //if(dif > 0){
                //    --tmp;
                //}
                //else{
                //    ++tmp;
                //}
                if(1000 < dif){
                    tmp-=100;
                }
                else if(100 < dif){
                    tmp-= 10;
                }
                else if(100 > dif && 10 < dif){
                    tmp-= 5;
                }
                else if ( 10 >= dif && 1 <= dif){
                    tmp-=1;
                }
                else if ( -1000 > dif){
                    tmp+=100;
                }
                else if ( -100 > dif){
                    tmp+=10;
                }
                else if(-100 < dif && -10 > dif){
                    tmp+= 5;
                }
                else if ( -10 <= dif && -1 >= dif){
                    tmp+=1;
                }
                subProp[i]->setValue(OFS, tmp);
            }
        }
    }

    for(int i = 0; i< 4;++i){
        if(gf_MtReq[i].req){
            subProp[i]->setValue(ATT,gf_MtReq[i].val);
            //setMotVal((eMotPos)i, gf_MtReq[i].val);
        }
    }

    if(gf_Print.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
    if(gf_Print.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
    if(gf_Print.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }
    if(gf_Print.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); }
}

// void HbManager::addMotVal(eMotPos pos, INT16 add){
//     INT16 tmp;
//     if(pos == F_L){
//         tmp = subProp[0]->getValue();
//         subProp[0]->setValue(tmp + add);
//     }
//     else if(pos == F_R){
//         tmp = subProp[1]->getValue();
//         subProp[1]->setValue(tmp + add);
//     }
//     else if(pos == R_L){
//         tmp = subProp[2]->getValue();
//         subProp[2]->setValue(tmp + add);
//     }
//     else if(pos == R_R){
//         tmp = subProp[3]->getValue();
//         subProp[3]->setValue(tmp + add);
//     }
// }

// void HbManager::subMotVal(eMotPos pos, INT16 sub){
//     INT16 tmp;
//     if(pos == F_L){
//         tmp = subProp[0]->getValue();
//         subProp[0]->setValue(tmp - sub);
//     }
//     else if(pos == F_R){
//         tmp = subProp[1]->getValue();
//         subProp[1]->setValue(tmp - sub);
//     }
//     else if(pos == R_L){
//         tmp = subProp[2]->getValue();
//         subProp[2]->setValue(tmp - sub);
//     }
//     else if(pos == R_R){
//         tmp = subProp[3]->getValue();
//         subProp[3]->setValue(tmp - sub);
//     }
// }

// void HbManager::setMotPara(UCHAR num, typMotPara para){
// //    if(mot[num] == NULL) return;
//     //mot[num]->setOffset(para.offset);
// //    mot[num]->setLimit(para.limit_low, para.limit_hi);
// }

// void HbManager::getCurMotVal(){
//     for(int i = 0; i < 4; ++i){
//        motorValD[i] = mot[i]->getCurrentValue();
//     }
// }

INT16 HbManager::getCurMotVal(eMotPos pos){
    if(subProp[(int)pos] == NULL) return 0;
    return subProp[(int)pos]->getValue(USER);
}
void HbManager::setMotVal(eMotPos pos, INT16 val){
    if(subProp[(int)pos] == NULL) return;
    subProp[(int)pos]->setValue(USER, val);
}

// bool HbManager::stopMotor(){
//     typFlg4Mot flg;
//     INT16 tmp = 0;
//     //bool flg[4] = {false,false,false,false};
//     for(int i = 0; i < 4; ++i){
//         if(motorValD[i] > 0){
// //            motorValD[i] = motorValD[i]--;
// //            tmp  = motorValD[i]-5;
//             if(tmp < 0){
//                 motorValD[i] = 0;
//             }else{
//                 motorValD[i] = tmp;
//             }
// //            mot[i]->setValueDirect(motorValD[i]);
//         }
//         else{
//             flg[i] = true;
//         }
//     }
//     //for(int i = 0; i<4; ++i){
//     //    if(flg[i] == false) return false;
//     //}
//     return flg.count() == 4;
//     //return true;
// }

// void HbManager::initMotVal(){
//     for(int i = 0; i < 4; ++i){
//         motorVal[i]=0;
//         motorValD[i]=0;
//     }
// }

void HbManager::setMotFPGA(UCHAR num, INT16 val){
    //UCHAR tmpN = num / 2;
    //UCHAR tmp = num % 2;
//    mot[tmpN]->setValueFPGA(tmp, val);
//    sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val);
}

//void HbManager::initChkMotor(){
//    motNum = 0;
//    motVal = 300;
//    sp.printf("InitCalled!");
//}

//bool HbManager::chkMotor(){
//    mot[motNum]->setValue(motVal);
//    if(motVal < 500){
//        ++motVal;
//    } else {
//        if(motNum < 3){
//            mot[motNum]->setValue(0);
//            ++motNum;
//            motVal = 300;
//        }
//        else {
//            mot[motNum]->setValue(0);
//            motNum = 0;
//            motVal = 300;
//            return true;
//        }
//    }
//    return false;
//}

//------------------------------------------------------
//エンジン制御
//------------------------------------------------------
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;
        }
    }
    
}

//------------------------------------------------------
//ステート遷移関連
//------------------------------------------------------
bool HbManager::chkOverIDLE(){
    return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
}

bool HbManager::chkInRangeIDLE(){
    return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
}
// ステート遷移関連end
//------------------------------------------------------

//------------------------------------------------------
//ユーザー操作関連
//------------------------------------------------------
void HbManager::getUserCommand(){
    usrSW = ope->GetUserOpe();
    //sp.printf("SW Val=%X\r\n", usrSW.w);
}

INT16 HbManager::getUserMotAxl(){
    return ope->GetAinAccel();
}

bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
    return ope->ChkCtrlSW(stype);
}
bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
    return ope->ChkCtrlSwRiseEdge(stype);
}
bool HbManager::chkSWUserOpeAny(){
    return ope->ChkCtrlSwAny();
}
typUserSw HbManager::getUserSw(){
    return usrSW;
}

void HbManager::chkSW(enmHbState stat){
    
    INT16 tmpRpm;
        // Front Left 
    if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
        gf_MtReqU[0].req=true;
        tmpRpm = getCurMotVal(F_L);
        if(tmpRpm < 4000){
            setMotVal(F_L, 4000);
        }
    } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
        if(gf_MtReqU[0].req==true){
            setMotVal(F_L, 4000);
        }
    } else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
        if(gf_MtReqU[0].req){
            tmpRpm = getCurMotVal(F_L);
            if(tmpRpm == 0){
                gf_MtReqU[0].req = false;
            }else if(tmpRpm > 1000){
                tmpRpm-=100;
            } else if(tmpRpm >= 100){
                tmpRpm-=50;
            } else if(tmpRpm > 0){
                tmpRpm-=10;
                if(tmpRpm < 0) tmpRpm = 0;
            } else if(tmpRpm < -1000){
                tmpRpm+=100;
            } else if(tmpRpm <= -100){
                tmpRpm+=50;
            }else if(tmpRpm < 0){
                tmpRpm+=10;
                if(tmpRpm > 0) tmpRpm = 0;
            }
            setMotVal(F_L, tmpRpm);
        }
    }

    if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
        gf_MtReqU[1].req=true;
        tmpRpm = getCurMotVal(F_R);
        if(tmpRpm < 4000){
            setMotVal(F_R, 4000);
        }
    } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
        if(gf_MtReqU[1].req==true){
            setMotVal(F_R, 4000);
        }
    } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
        if(gf_MtReqU[1].req){
            tmpRpm = getCurMotVal(F_R);
            if(tmpRpm == 0){
                gf_MtReqU[1].req = false;
            }else if(tmpRpm > 1000){
                tmpRpm-=100;
            } else if(tmpRpm >= 100){
                tmpRpm-=50;
            } else if(tmpRpm > 0){
                tmpRpm-=10;
                if(tmpRpm < 0) tmpRpm = 0;
            } else if(tmpRpm < -1000){
                tmpRpm+=100;
            } else if(tmpRpm <= -100){
                tmpRpm+=50;
            }else if(tmpRpm < 0){
                tmpRpm+=10;
                if(tmpRpm > 0) tmpRpm = 0;
            }
            setMotVal(F_R, tmpRpm);
        }
    }

    if(chkSWUserOpeRE(HbUserOpe::MOT_STOP)){
        // EMG STOP だが、テスト用のため、モーターのオフセットを0にして回転を止める
        for(int i = 0; i < 4; ++i){
            gf_MtReq[i].req = true;
            gf_MtReq[i].val = 0;
            gf_MtReqOfs[i].req = true;
            gf_MtReqOfs[i].val = 0;
            // gf_MtReqU[i].bf.req = true;
            // gf_MtReqU[i].bf.val
        }
    }
    
    if(chkSWUserOpeRE(HbUserOpe::R_1)){
        gf_AxReq[0].bf.req = true;
        gf_AxReq[1].bf.req = true;
        //sp.printf("R_1 Push \r\n");
    }
    if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
        for(int i = 0; i < 4; ++i){
            gf_MtReq[i].req = true;
            gf_MtReq[i].val = 0;
            gf_MtReqOfs[i].req = true;
            gf_MtReqOfs[i].val = 0;
        }
    }        
}
// ユーザー操作関連end
//------------------------------------------------------

//======================================================
//コンストラクタ
//======================================================
HbManager::HbManager(){
    //メンバクラスインスタンス
    eng[0]  = new HbEngine(0);
    eng[1]  = new HbEngine(1);
    att     = new HbAttitude(2.0 , 0 , 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);

    subProp[0] = new HbSubProp(F_L);
    subProp[0]->setCoef(IN, 0.00004,    0.0401, 29.262);
    subProp[0]->setCoef(OUT,0.00004,    0.1336, -69.184);
    //subProp[0]->makeTbl();

    subProp[1] = new HbSubProp(F_R);
    subProp[1]->setCoef(IN, 0.00004,    0.0151, 70.947);
    subProp[1]->setCoef(OUT,0.00004,    0.1336, -69.184);
    //subProp[1]->makeTbl();
    
    subProp[2] = new HbSubProp(R_L);
    subProp[2]->setCoef(IN, 0.00004,    0.0244, 53.954);
    subProp[2]->setCoef(OUT,0.00003,    0.1579, -114.22);
    //subProp[2]->makeTbl();
    
    subProp[3] = new HbSubProp(R_R);
    subProp[3]->setCoef(IN, 0.00004,    0.0111, 82.044);
    subProp[3]->setCoef(OUT,0.00003,    0.1617, -123.27);
    //subProp[3]->makeTbl();

    imu     = new Imu(p28,p27);

    ope = new HbUserOpe();

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