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;
+
 }