teamALI / Mbed 2 deprecated HB2018

ジャイロ追加前

Dependencies:   mbed FreeRTOS

HbManager.cpp@19:4b0fe9a5ec38, 2018-12-05 (annotated)

Committer:
takeru0x1103
Date:
Wed Dec 05 00:12:38 2018 +0000
Revision:
19:4b0fe9a5ec38
Parent:
18:5aa48aec9cae
Child:
20:0394e15412c3
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 17:f9610f3cfa1b 1 #include "HbManager.h"
takeru0x1103 17:f9610f3cfa1b 2 #include "globalFlags.h"
takeru0x1103 17:f9610f3cfa1b 3 #include "uart.h"
takeru0x1103 18:5aa48aec9cae 4
takeru0x1103 18:5aa48aec9cae 5 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 6 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 7 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 8 void HbManager::getAttitude(){
takeru0x1103 18:5aa48aec9cae 9 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 10 nowAngle = imu->GetYaw();
takeru0x1103 18:5aa48aec9cae 11 nowRate = imu->GetGyroZ();
takeru0x1103 18:5aa48aec9cae 12 //ヨー角キャリブレーション
takeru0x1103 18:5aa48aec9cae 13 if(gf_Cal.bf.yaw==true){
takeru0x1103 18:5aa48aec9cae 14 imu->CalYaw();
takeru0x1103 18:5aa48aec9cae 15 gf_Cal.bf.yaw=false;
takeru0x1103 18:5aa48aec9cae 16 sp.printf("Yaw caribration %f\t",nowAngle);
takeru0x1103 18:5aa48aec9cae 17 nowAngle=0;
takeru0x1103 18:5aa48aec9cae 18 }
takeru0x1103 18:5aa48aec9cae 19 //ジャイロオフセットキャリブレーション
takeru0x1103 18:5aa48aec9cae 20 if(gf_Cal.bf.gy==true){
takeru0x1103 18:5aa48aec9cae 21 imu->CalGyro();
takeru0x1103 18:5aa48aec9cae 22 gf_Cal.bf.gy=false;
takeru0x1103 18:5aa48aec9cae 23 sp.printf("Gyro caribration %d\t",nowRate);
takeru0x1103 18:5aa48aec9cae 24 nowRate=0;
takeru0x1103 17:f9610f3cfa1b 25 }
takeru0x1103 17:f9610f3cfa1b 26
takeru0x1103 18:5aa48aec9cae 27 if(gf_Print.bf.yaw) {sp.printf("Ang\t%f\t",nowAngle); }
takeru0x1103 18:5aa48aec9cae 28 if(gf_Print.bf.gy) {sp.printf("Gyr\t%f\t",nowRate); }
takeru0x1103 18:5aa48aec9cae 29 }
takeru0x1103 18:5aa48aec9cae 30 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 31 //姿勢制御
takeru0x1103 18:5aa48aec9cae 32 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 33 void HbManager::controlAttitude(){
takeru0x1103 18:5aa48aec9cae 34 //
takeru0x1103 19:4b0fe9a5ec38 35 float pidRtn;
takeru0x1103 19:4b0fe9a5ec38 36
takeru0x1103 19:4b0fe9a5ec38 37 if(gf_AttCntEna==true){
takeru0x1103 19:4b0fe9a5ec38 38 pidRtn= att->pid(0,nowAngle,nowRate);
takeru0x1103 19:4b0fe9a5ec38 39 }else{
takeru0x1103 19:4b0fe9a5ec38 40 pidRtn =0.0;
takeru0x1103 19:4b0fe9a5ec38 41 }
takeru0x1103 19:4b0fe9a5ec38 42
takeru0x1103 18:5aa48aec9cae 43 //
takeru0x1103 18:5aa48aec9cae 44 if(gf_Print.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
takeru0x1103 18:5aa48aec9cae 45 if(gf_Print.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
takeru0x1103 18:5aa48aec9cae 46 if(gf_Print.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
takeru0x1103 18:5aa48aec9cae 47 if(gf_Print.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
takeru0x1103 18:5aa48aec9cae 48 if(gf_Print.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
takeru0x1103 18:5aa48aec9cae 49
takeru0x1103 19:4b0fe9a5ec38 50 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
takeru0x1103 19:4b0fe9a5ec38 51 float fl = pidRtn * -1;
takeru0x1103 19:4b0fe9a5ec38 52 float fr = pidRtn;
takeru0x1103 19:4b0fe9a5ec38 53 float bl = pidRtn;
takeru0x1103 19:4b0fe9a5ec38 54 float br = pidRtn * -1;
takeru0x1103 18:5aa48aec9cae 55
takeru0x1103 19:4b0fe9a5ec38 56 motorVal[0] = (INT16)fl;//前左
takeru0x1103 19:4b0fe9a5ec38 57 motorVal[1] = (INT16)fr;//前右
takeru0x1103 19:4b0fe9a5ec38 58 motorVal[2] = (INT16)bl;//後左
takeru0x1103 19:4b0fe9a5ec38 59 motorVal[3] = (INT16)br;//後右
takeru0x1103 17:f9610f3cfa1b 60 }
takeru0x1103 18:5aa48aec9cae 61
takeru0x1103 18:5aa48aec9cae 62 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 63 //モーター制御
takeru0x1103 18:5aa48aec9cae 64 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 65 void HbManager::controlMotor(){
takeru0x1103 18:5aa48aec9cae 66
takeru0x1103 18:5aa48aec9cae 67 mot[0]->setValue(motorVal[0]);
takeru0x1103 18:5aa48aec9cae 68 mot[1]->setValue(motorVal[1]);
takeru0x1103 18:5aa48aec9cae 69 mot[2]->setValue(motorVal[2]);
takeru0x1103 18:5aa48aec9cae 70 mot[3]->setValue(motorVal[3]);
takeru0x1103 19:4b0fe9a5ec38 71
takeru0x1103 19:4b0fe9a5ec38 72 if(gf_Print.bf.m1){ sp.printf("m1=%d,",motorVal[0]); }
takeru0x1103 19:4b0fe9a5ec38 73 if(gf_Print.bf.m2){ sp.printf("m2=%d,",motorVal[0]); }
takeru0x1103 19:4b0fe9a5ec38 74 if(gf_Print.bf.m3){ sp.printf("m3=%d,",motorVal[0]); }
takeru0x1103 19:4b0fe9a5ec38 75 if(gf_Print.bf.m4){ sp.printf("m4=%d,",motorVal[0]); }
takeru0x1103 19:4b0fe9a5ec38 76
takeru0x1103 18:5aa48aec9cae 77 }
takeru0x1103 17:f9610f3cfa1b 78
takeru0x1103 17:f9610f3cfa1b 79 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 80 //エンジン制御
takeru0x1103 17:f9610f3cfa1b 81 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 82 void HbManager::controlEngine(){
takeru0x1103 18:5aa48aec9cae 83 UINT16 rpm[2];
takeru0x1103 18:5aa48aec9cae 84 int i;
takeru0x1103 17:f9610f3cfa1b 85
takeru0x1103 18:5aa48aec9cae 86 //エンジン回転数読み出し
takeru0x1103 18:5aa48aec9cae 87 rpm[0] = eng[0]->getRpm();
takeru0x1103 18:5aa48aec9cae 88 rpm[1] = eng[1]->getRpm();
takeru0x1103 17:f9610f3cfa1b 89
takeru0x1103 17:f9610f3cfa1b 90 if(gf_Print.bf.e1){ sp.printf("%d",rpm[0]); }
takeru0x1103 17:f9610f3cfa1b 91 if(gf_Print.bf.e2){ sp.printf("%d,",rpm[1]); }
takeru0x1103 17:f9610f3cfa1b 92
takeru0x1103 18:5aa48aec9cae 93 //アクセル設定
takeru0x1103 18:5aa48aec9cae 94 for(i=0; i<2; i++){
takeru0x1103 18:5aa48aec9cae 95 if(gf_AxReq[0].bf.req==true){
takeru0x1103 18:5aa48aec9cae 96 sp.printf("val=%d(%X)\r\n" , gf_AxReq[0].bf.val , gf_AxReq[0].dt);
takeru0x1103 18:5aa48aec9cae 97 accelVal[i] = gf_AxReq[i].bf.val;
takeru0x1103 18:5aa48aec9cae 98 eng[i]->setAccell(accelVal[i]);
takeru0x1103 18:5aa48aec9cae 99 gf_AxReq[i].bf.req=false;
takeru0x1103 18:5aa48aec9cae 100 }
takeru0x1103 17:f9610f3cfa1b 101 }
takeru0x1103 17:f9610f3cfa1b 102
takeru0x1103 17:f9610f3cfa1b 103 }
takeru0x1103 17:f9610f3cfa1b 104
takeru0x1103 17:f9610f3cfa1b 105 //======================================================
takeru0x1103 17:f9610f3cfa1b 106 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 107 //======================================================
takeru0x1103 17:f9610f3cfa1b 108 HbManager::HbManager(){
takeru0x1103 18:5aa48aec9cae 109 //メンバクラスインスタンス
takeru0x1103 18:5aa48aec9cae 110 eng[0] = new HbEngine(0);
takeru0x1103 18:5aa48aec9cae 111 eng[1] = new HbEngine(1);
takeru0x1103 18:5aa48aec9cae 112 att = new HbAttitude(4.0 , 4 , 0 , 0);//パラメータ outP P I D
takeru0x1103 18:5aa48aec9cae 113 mot[0] = new HbMotor(0);
takeru0x1103 18:5aa48aec9cae 114 mot[1] = new HbMotor(1);
takeru0x1103 18:5aa48aec9cae 115 mot[2] = new HbMotor(2);
takeru0x1103 18:5aa48aec9cae 116 mot[3] = new HbMotor(3);
takeru0x1103 18:5aa48aec9cae 117 #include "mbed.h"
takeru0x1103 18:5aa48aec9cae 118 imu = new Imu(p28,p27);
takeru0x1103 18:5aa48aec9cae 119
takeru0x1103 18:5aa48aec9cae 120 //初期化
takeru0x1103 17:f9610f3cfa1b 121 motorVal[0]=0;
takeru0x1103 17:f9610f3cfa1b 122 motorVal[1]=0;
takeru0x1103 17:f9610f3cfa1b 123 motorVal[2]=0;
takeru0x1103 17:f9610f3cfa1b 124 motorVal[3]=0;
takeru0x1103 17:f9610f3cfa1b 125
takeru0x1103 18:5aa48aec9cae 126 //フラグ初期化
takeru0x1103 18:5aa48aec9cae 127 gf_Print.flg=0;
takeru0x1103 18:5aa48aec9cae 128 gf_Mon.flg=0;
takeru0x1103 18:5aa48aec9cae 129 gf_Cal.flg=0;
takeru0x1103 18:5aa48aec9cae 130 gf_Chk.flg=0;
takeru0x1103 18:5aa48aec9cae 131
takeru0x1103 17:f9610f3cfa1b 132 }