![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ジャイロ追加前
Diff: HbManager.cpp
- Revision:
- 18:5aa48aec9cae
- Parent:
- 17:f9610f3cfa1b
- Child:
- 19:4b0fe9a5ec38
--- a/HbManager.cpp Fri Nov 30 05:24:27 2018 +0000 +++ b/HbManager.cpp Sat Dec 01 14:03:08 2018 +0000 @@ -1,100 +1,131 @@ #include "HbManager.h" #include "globalFlags.h" #include "uart.h" -//#include "command.h" -//#include "Gyro.h" + +//クリッピング +#define clip(val,min,max) ( val>max ? max : val<min ? min : val) -/* -static void xSlotlChange(UINT16 *tlt , UCHAR dir){ - int tmp; - tmp = *tlt; - - if( dir==0x1 ){ - tmp+=4;//加速 - }else{ - tmp-=14;//減速 +//------------------------------------------------------ +//姿勢状態取得 +//------------------------------------------------------ +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(tmp>=0xFFF){ - *tlt =0xFFF;//上限リミット - }else if(tmp<=0){ - *tlt =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 = att->pid(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.fb){sp.printf("PID feedback=%f,",pidRtn);} + + + + if(gf_AttCntEna==true){ + //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く) + float fl = pidRtn; + float fr = pidRtn * -1; + float bl = pidRtn * -1; + float br = pidRtn; + + motorVal[0] = (INT16)fl;//前左 + motorVal[1] = (INT16)fr;//前右 + motorVal[2] = (INT16)bl;//後左 + motorVal[3] = (INT16)br;//後右 }else{ - *tlt=tmp; + motorVal[0] = 0;//前左 + motorVal[1] = 0;//前右 + motorVal[2] = 0;//後左 + motorVal[3] = 0;//後右 } } + +//------------------------------------------------------ +//モーター制御 +//------------------------------------------------------ +void HbManager::controlMotor(){ + + mot[0]->setValue(motorVal[0]); + mot[1]->setValue(motorVal[1]); + mot[2]->setValue(motorVal[2]); + mot[3]->setValue(motorVal[3]); +} - -//------------------------------------------ -//ジャイロアクセス -//------------------------------------------ -//void HbManager::controlEngine(){ -// Gyro gy(p28,p27);//ジャイロセンサー用シリアルポートインスタンス -// float ang;//角度 -// float rate;//角速度 -//} - -//------------------------------------------ -//ユーザー指令状態取得 -//------------------------------------------ -void HbManager::getUserCommand(){ - typu_USER_SW sw; - sw.w = fpgaGetUserSw(); - - xSlotlChange(&motorVal[0] , sw.bf.axl_l); - xSlotlChange(&motorVal[1] , sw.bf.axl_r); - xSlotlChange(&motorVal[2] , sw.bf.axl_r); - xSlotlChange(&motorVal[3] , sw.bf.axl_l); - - fpgaSubProp(0,motorVal[0]); - fpgaSubProp(1,motorVal[1]); - fpgaSubProp(2,motorVal[2]); - fpgaSubProp(3,motorVal[3]); - - //sp.printf("Sw=%02X\r\n",sw.w); -} - -*/ //------------------------------------------------------ //エンジン制御 //------------------------------------------------------ void HbManager::controlEngine(){ - UINT16 rpm[2]; - + UINT16 rpm[2]; + int i; - rpm[0] = e1->getRpm(); - rpm[1] = e2->getRpm(); + //エンジン回転数読み出し + 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]); } - - if(gf_Print.flg!=0){ - gf_Print.flg=0; - sp.printf("\r\n"); + //アクセル設定 + 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; + } } - if(gf_AxReq[0].bf.req==true){ - sp.printf("val=%d(%X)\r\n" , gf_AxReq[0].bf.val , gf_AxReq[0].dt); - accelVal[0] = gf_AxReq[0].bf.val; - e1->setAccell(accelVal[0]); - gf_AxReq[0].bf.req=false; - } - - } //====================================================== //コンストラクタ //====================================================== HbManager::HbManager(){ - e1 = new HbEngine(0); - e2 = new HbEngine(1); - + //メンバクラスインスタンス + 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); + + //初期化 motorVal[0]=0; motorVal[1]=0; motorVal[2]=0; motorVal[3]=0; - chk=0; + //フラグ初期化 + gf_Print.flg=0; + gf_Mon.flg=0; + gf_Cal.flg=0; + gf_Chk.flg=0; + }