teamALI
/
HB2018
ジャイロ追加前
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?
User | Revision | Line number | New 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 | } |