teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

HbManager.cpp@49:048966a8264a, 2019-02-25 (annotated)

Committer:
MasashiNomura
Date:
Mon Feb 25 12:51:35 2019 +0000
Revision:
49:048966a8264a
Parent:
48:71aec693a7dc
Child:
50:76413e8e073d
2019/02/25  for share

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"
MasashiNomura 39:1b76f7df8804 4 #include "fpga.h"
MasashiNomura 39:1b76f7df8804 5
MasashiNomura 39:1b76f7df8804 6 UINT16 HbManager::proofOfSurvival()
MasashiNomura 39:1b76f7df8804 7 {
MasashiNomura 39:1b76f7df8804 8 return fpgaTest(1);
MasashiNomura 39:1b76f7df8804 9 }
takeru0x1103 18:5aa48aec9cae 10
takeru0x1103 18:5aa48aec9cae 11 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 12 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 13 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 14 void HbManager::getAttitude(){
MasashiNomura 25:f3a6e7eec9c3 15 float tmpf;
MasashiNomura 25:f3a6e7eec9c3 16 INT16 tmpi;
takeru0x1103 18:5aa48aec9cae 17 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 18 nowAngle = imu->GetYaw();
takeru0x1103 18:5aa48aec9cae 19 nowRate = imu->GetGyroZ();
MasashiNomura 47:d3fa874f336e 20
MasashiNomura 47:d3fa874f336e 21 if(gf_Cal.bf.yaw==true && gf_Cal.bf.gy==true){
MasashiNomura 47:d3fa874f336e 22 nowTrgtAng = 0;
MasashiNomura 47:d3fa874f336e 23 }
takeru0x1103 18:5aa48aec9cae 24 //ヨー角キャリブレーション
takeru0x1103 18:5aa48aec9cae 25 if(gf_Cal.bf.yaw==true){
takeru0x1103 18:5aa48aec9cae 26 imu->CalYaw();
takeru0x1103 18:5aa48aec9cae 27 gf_Cal.bf.yaw=false;
takeru0x1103 18:5aa48aec9cae 28 sp.printf("Yaw caribration %f\t",nowAngle);
takeru0x1103 18:5aa48aec9cae 29 nowAngle=0;
takeru0x1103 18:5aa48aec9cae 30 }
takeru0x1103 18:5aa48aec9cae 31 //ジャイロオフセットキャリブレーション
takeru0x1103 18:5aa48aec9cae 32 if(gf_Cal.bf.gy==true){
takeru0x1103 18:5aa48aec9cae 33 imu->CalGyro();
takeru0x1103 18:5aa48aec9cae 34 gf_Cal.bf.gy=false;
takeru0x1103 18:5aa48aec9cae 35 sp.printf("Gyro caribration %d\t",nowRate);
takeru0x1103 18:5aa48aec9cae 36 nowRate=0;
takeru0x1103 17:f9610f3cfa1b 37 }
takeru0x1103 17:f9610f3cfa1b 38
MasashiNomura 41:45c982b1c5b6 39 if(gf_Print.d1.bf.yaw) {
MasashiNomura 25:f3a6e7eec9c3 40 tmpf = imu->GetYawRef();
MasashiNomura 25:f3a6e7eec9c3 41 sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf);
MasashiNomura 25:f3a6e7eec9c3 42 }
MasashiNomura 41:45c982b1c5b6 43 if(gf_Print.d1.bf.gy) {
MasashiNomura 25:f3a6e7eec9c3 44 tmpi = imu->GetGyroRef();
MasashiNomura 25:f3a6e7eec9c3 45 sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi);
MasashiNomura 25:f3a6e7eec9c3 46 }
takeru0x1103 18:5aa48aec9cae 47 }
MasashiNomura 25:f3a6e7eec9c3 48 void HbManager::calAtt(){
MasashiNomura 25:f3a6e7eec9c3 49 if(att == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 50 if(imu == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 51 imu->CalYaw();
MasashiNomura 25:f3a6e7eec9c3 52 sp.printf("Yaw auto caribration %f\t",nowAngle);
MasashiNomura 25:f3a6e7eec9c3 53 nowAngle=0;
MasashiNomura 25:f3a6e7eec9c3 54 imu->CalGyro();
MasashiNomura 25:f3a6e7eec9c3 55 sp.printf("Gyro auto caribration %d\t\r\n",nowRate);
MasashiNomura 25:f3a6e7eec9c3 56 nowRate=0;
MasashiNomura 47:d3fa874f336e 57
MasashiNomura 47:d3fa874f336e 58 nowTrgtAng = 0;
MasashiNomura 25:f3a6e7eec9c3 59 }
MasashiNomura 25:f3a6e7eec9c3 60
takeru0x1103 18:5aa48aec9cae 61 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 62 //姿勢制御
takeru0x1103 18:5aa48aec9cae 63 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 64 void HbManager::controlAttitude(){
takeru0x1103 19:4b0fe9a5ec38 65
MasashiNomura 36:2cc739c7e4cb 66 float pidRtn;
MasashiNomura 36:2cc739c7e4cb 67
MasashiNomura 48:71aec693a7dc 68 if(g_PidPara.mode == PID_0_OFF || g_PidPara.mode == PID_0_ON){
MasashiNomura 48:71aec693a7dc 69 pidRtn = att->pid(nowTrgtAng,nowAngle,nowRate);
MasashiNomura 48:71aec693a7dc 70 } else {
MasashiNomura 48:71aec693a7dc 71 pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate);
MasashiNomura 48:71aec693a7dc 72 }
takeru0x1103 19:4b0fe9a5ec38 73
MasashiNomura 47:d3fa874f336e 74 if(att != NULL){
MasashiNomura 48:71aec693a7dc 75 if(gf_Print.d1.bf.pp){sp.printf("PID:PP=%f,",att->getPp());}
MasashiNomura 47:d3fa874f336e 76 if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 47:d3fa874f336e 77 if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 47:d3fa874f336e 78 if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 47:d3fa874f336e 79 if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
MasashiNomura 47:d3fa874f336e 80 if(gf_Print.d1.bf.fb){
MasashiNomura 49:048966a8264a 81 //sp.printf("PID FB:%f, %f, %f, ",pidRtn, nowAngle, nowRate);
MasashiNomura 48:71aec693a7dc 82 sp.printf("PID FB:%f, %f, %f, ",pidRtn, nowAngle, nowRate);
MasashiNomura 48:71aec693a7dc 83 //sp.printf("%f,",nowAngle);
MasashiNomura 48:71aec693a7dc 84 //sp.printf("%f,",nowRate);
MasashiNomura 47:d3fa874f336e 85 }
MasashiNomura 46:5074781a28dd 86 }
takeru0x1103 18:5aa48aec9cae 87
takeru0x1103 19:4b0fe9a5ec38 88 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 46:5074781a28dd 89 // 現在、望む角速度から正確なプロペラ回転数に変換する方法を持たないため、得られた値から決まった係数で回転数に変換
MasashiNomura 46:5074781a28dd 90 // // float fl = pidRtn * -1;
MasashiNomura 46:5074781a28dd 91 // // float fr = pidRtn;
MasashiNomura 46:5074781a28dd 92 // // float bl = pidRtn;
MasashiNomura 46:5074781a28dd 93 // // float br = pidRtn * -1;
MasashiNomura 46:5074781a28dd 94 // // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
MasashiNomura 46:5074781a28dd 95 // float fl = pidRtn * 2.25 * -1;
MasashiNomura 46:5074781a28dd 96 // float fr = pidRtn * 2.25;
MasashiNomura 46:5074781a28dd 97 // float bl = pidRtn * 2.25;
MasashiNomura 46:5074781a28dd 98 // float br = pidRtn * 2.25 * -1;
MasashiNomura 46:5074781a28dd 99
MasashiNomura 48:71aec693a7dc 100 // // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする
MasashiNomura 48:71aec693a7dc 101 // float tmpRtn = abs(pidRtn);
MasashiNomura 48:71aec693a7dc 102 // tmpRtn = sqrtf(tmpRtn);
MasashiNomura 48:71aec693a7dc 103 // if(pidRtn < 0){
MasashiNomura 48:71aec693a7dc 104 // tmpRtn *= -1;
MasashiNomura 48:71aec693a7dc 105 // }
MasashiNomura 48:71aec693a7dc 106 // pidRtn = tmpRtn;
MasashiNomura 48:71aec693a7dc 107 float fl, fr, bl, br;
MasashiNomura 48:71aec693a7dc 108 //float fl = pidRtn * 2.25 * -1;
MasashiNomura 48:71aec693a7dc 109 //float fr = pidRtn * 2.25;
MasashiNomura 48:71aec693a7dc 110 //float bl = pidRtn * 2.25;
MasashiNomura 48:71aec693a7dc 111 //float br = pidRtn * 2.25 * -1;
MasashiNomura 48:71aec693a7dc 112 // float fl = pidRtn;
MasashiNomura 48:71aec693a7dc 113 // float fr = -pidRtn;
MasashiNomura 48:71aec693a7dc 114 // float bl = -pidRtn;
MasashiNomura 48:71aec693a7dc 115 // float br = pidRtn;
MasashiNomura 48:71aec693a7dc 116 if(g_PidPara.mode == PID_0_ON){
MasashiNomura 48:71aec693a7dc 117 fl = pidRtn;
MasashiNomura 48:71aec693a7dc 118 fr = -pidRtn;
MasashiNomura 48:71aec693a7dc 119 bl = -pidRtn;
MasashiNomura 48:71aec693a7dc 120 br = pidRtn;
MasashiNomura 48:71aec693a7dc 121 } else if(g_PidPara.mode == PID_1_ON){
MasashiNomura 48:71aec693a7dc 122 fl = pidRtn * 2.25 * -1;
MasashiNomura 48:71aec693a7dc 123 fr = pidRtn * 2.25;
MasashiNomura 48:71aec693a7dc 124 bl = pidRtn * 2.25;
MasashiNomura 48:71aec693a7dc 125 br = pidRtn * 2.25 * -1;
MasashiNomura 48:71aec693a7dc 126 }else{
MasashiNomura 48:71aec693a7dc 127 fl = 0;
MasashiNomura 48:71aec693a7dc 128 fr = 0;
MasashiNomura 48:71aec693a7dc 129 bl = 0;
MasashiNomura 48:71aec693a7dc 130 br = 0;
MasashiNomura 46:5074781a28dd 131 }
MasashiNomura 32:7f4145cc3551 132 gf_MtReq[0].val = (INT16)fl;
MasashiNomura 32:7f4145cc3551 133 gf_MtReq[1].val = (INT16)fr;
MasashiNomura 32:7f4145cc3551 134 gf_MtReq[2].val = (INT16)bl;
MasashiNomura 32:7f4145cc3551 135 gf_MtReq[3].val = (INT16)br;
MasashiNomura 46:5074781a28dd 136 for(int i = 0; i < 4; ++i){
MasashiNomura 46:5074781a28dd 137 gf_MtReq[i].req = true;
MasashiNomura 46:5074781a28dd 138 }
MasashiNomura 48:71aec693a7dc 139 // if(gf_Print.d1.bf.fb){
MasashiNomura 48:71aec693a7dc 140 // sp.printf("REQ RPM FL:%d, FR:%d, RL:%d, RR:%d",gf_MtReq[0].val,gf_MtReq[1].val,gf_MtReq[2].val,gf_MtReq[3].val);
MasashiNomura 48:71aec693a7dc 141 // }
takeru0x1103 17:f9610f3cfa1b 142 }
MasashiNomura 24:c5945aaae777 143
MasashiNomura 24:c5945aaae777 144 void HbManager::controlAttitude(float cmdAngle){
MasashiNomura 24:c5945aaae777 145 float pidRtn;
MasashiNomura 24:c5945aaae777 146
MasashiNomura 36:2cc739c7e4cb 147 pidRtn = att->pid2(cmdAngle,nowAngle,nowRate);
MasashiNomura 36:2cc739c7e4cb 148
MasashiNomura 41:45c982b1c5b6 149 if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 41:45c982b1c5b6 150 if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 41:45c982b1c5b6 151 if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 41:45c982b1c5b6 152 if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 41:45c982b1c5b6 153 if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
MasashiNomura 41:45c982b1c5b6 154 if(gf_Print.d1.bf.fb){
MasashiNomura 36:2cc739c7e4cb 155 sp.printf("PID FB,%f,",pidRtn);
MasashiNomura 36:2cc739c7e4cb 156 sp.printf("%f,",nowAngle);
MasashiNomura 36:2cc739c7e4cb 157 sp.printf("%f,",nowRate);
MasashiNomura 36:2cc739c7e4cb 158 }
MasashiNomura 24:c5945aaae777 159
MasashiNomura 24:c5945aaae777 160 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 36:2cc739c7e4cb 161 // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
MasashiNomura 36:2cc739c7e4cb 162 float fl = pidRtn * 2.25 * -1;
MasashiNomura 36:2cc739c7e4cb 163 float fr = pidRtn * 2.25;
MasashiNomura 36:2cc739c7e4cb 164 float bl = pidRtn * 2.25;
MasashiNomura 36:2cc739c7e4cb 165 float br = pidRtn * 2.25 * -1;
MasashiNomura 36:2cc739c7e4cb 166 gf_MtReq[0].val = (INT16)fl;
MasashiNomura 36:2cc739c7e4cb 167 gf_MtReq[1].val = (INT16)fr;
MasashiNomura 36:2cc739c7e4cb 168 gf_MtReq[2].val = (INT16)bl;
MasashiNomura 36:2cc739c7e4cb 169 gf_MtReq[3].val = (INT16)br;
MasashiNomura 46:5074781a28dd 170 for(int i = 0; i < 4; ++i){
MasashiNomura 46:5074781a28dd 171 gf_MtReq[i].req = true;
MasashiNomura 46:5074781a28dd 172 }
MasashiNomura 36:2cc739c7e4cb 173
MasashiNomura 36:2cc739c7e4cb 174 // float pidRtn;
MasashiNomura 36:2cc739c7e4cb 175
MasashiNomura 36:2cc739c7e4cb 176 // pidRtn= att->pid(cmdAngle,nowAngle,nowRate);
MasashiNomura 36:2cc739c7e4cb 177 // //
MasashiNomura 41:45c982b1c5b6 178 // if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 41:45c982b1c5b6 179 // if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 41:45c982b1c5b6 180 // if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 41:45c982b1c5b6 181 // if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 41:45c982b1c5b6 182 // if(gf_Print.d1.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
MasashiNomura 24:c5945aaae777 183
MasashiNomura 36:2cc739c7e4cb 184 // //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 36:2cc739c7e4cb 185 // float fl = pidRtn * -1;
MasashiNomura 36:2cc739c7e4cb 186 // float fr = pidRtn;
MasashiNomura 36:2cc739c7e4cb 187 // float bl = pidRtn;
MasashiNomura 36:2cc739c7e4cb 188 // float br = pidRtn * -1;
MasashiNomura 36:2cc739c7e4cb 189
MasashiNomura 36:2cc739c7e4cb 190 // motorVal[0] = (INT16)fl;//前左
MasashiNomura 36:2cc739c7e4cb 191 // motorVal[1] = (INT16)fr;//前右
MasashiNomura 36:2cc739c7e4cb 192 // motorVal[2] = (INT16)bl;//後左
MasashiNomura 36:2cc739c7e4cb 193 // motorVal[3] = (INT16)br;//後右
MasashiNomura 24:c5945aaae777 194 }
MasashiNomura 24:c5945aaae777 195
MasashiNomura 24:c5945aaae777 196 void HbManager::setAttPara(typPidPara para){
MasashiNomura 24:c5945aaae777 197 if(att == NULL) return;
MasashiNomura 24:c5945aaae777 198 att->setPp(para.PP);
MasashiNomura 24:c5945aaae777 199 att->setP(para.P);
MasashiNomura 24:c5945aaae777 200 att->setI(para.I);
MasashiNomura 24:c5945aaae777 201 att->setD(para.D);
MasashiNomura 24:c5945aaae777 202 att->setIMax(para.IMax);
MasashiNomura 24:c5945aaae777 203 att->setIMin(para.IMin);
MasashiNomura 31:56c554c560c1 204 att->setV(para.V);
MasashiNomura 24:c5945aaae777 205 }
MasashiNomura 24:c5945aaae777 206
takeru0x1103 18:5aa48aec9cae 207 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 208 //モーター制御
takeru0x1103 18:5aa48aec9cae 209 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 210 void HbManager::controlMotor(){
MasashiNomura 25:f3a6e7eec9c3 211 INT16 tmp,dif;
MasashiNomura 29:eb3d72dd94aa 212 for(int i = 0; i < 4; ++i){
MasashiNomura 45:3b51dd26b579 213 tmp = subProp[i]->getValue(OFS);
MasashiNomura 32:7f4145cc3551 214 if(gf_MtReqOfs[i].req){
MasashiNomura 32:7f4145cc3551 215 dif = tmp - (INT16)gf_MtReqOfs[i].val;
MasashiNomura 29:eb3d72dd94aa 216 if(dif == 0){
MasashiNomura 40:debe99e228d3 217 subProp[i]->setValue(OFS, (INT16)gf_MtReqOfs[i].val);
MasashiNomura 32:7f4145cc3551 218 gf_MtReqOfs[i].req = false;
MasashiNomura 29:eb3d72dd94aa 219 }
MasashiNomura 29:eb3d72dd94aa 220 else {
MasashiNomura 29:eb3d72dd94aa 221 //if(dif > 0){
MasashiNomura 29:eb3d72dd94aa 222 // --tmp;
MasashiNomura 29:eb3d72dd94aa 223 //}
MasashiNomura 29:eb3d72dd94aa 224 //else{
MasashiNomura 29:eb3d72dd94aa 225 // ++tmp;
MasashiNomura 29:eb3d72dd94aa 226 //}
MasashiNomura 29:eb3d72dd94aa 227 if(1000 < dif){
MasashiNomura 29:eb3d72dd94aa 228 tmp-=100;
MasashiNomura 29:eb3d72dd94aa 229 }
MasashiNomura 29:eb3d72dd94aa 230 else if(100 < dif){
MasashiNomura 29:eb3d72dd94aa 231 tmp-= 10;
MasashiNomura 29:eb3d72dd94aa 232 }
MasashiNomura 40:debe99e228d3 233 else if(100 >= dif && 10 < dif){
MasashiNomura 29:eb3d72dd94aa 234 tmp-= 5;
MasashiNomura 29:eb3d72dd94aa 235 }
MasashiNomura 29:eb3d72dd94aa 236 else if ( 10 >= dif && 1 <= dif){
MasashiNomura 29:eb3d72dd94aa 237 tmp-=1;
MasashiNomura 29:eb3d72dd94aa 238 }
MasashiNomura 29:eb3d72dd94aa 239 else if ( -1000 > dif){
MasashiNomura 29:eb3d72dd94aa 240 tmp+=100;
MasashiNomura 29:eb3d72dd94aa 241 }
MasashiNomura 40:debe99e228d3 242 else if ( -100 >= dif){
MasashiNomura 29:eb3d72dd94aa 243 tmp+=10;
MasashiNomura 29:eb3d72dd94aa 244 }
MasashiNomura 40:debe99e228d3 245 else if(-100 <= dif && -10 > dif){
MasashiNomura 29:eb3d72dd94aa 246 tmp+= 5;
MasashiNomura 29:eb3d72dd94aa 247 }
MasashiNomura 29:eb3d72dd94aa 248 else if ( -10 <= dif && -1 >= dif){
MasashiNomura 29:eb3d72dd94aa 249 tmp+=1;
MasashiNomura 29:eb3d72dd94aa 250 }
MasashiNomura 32:7f4145cc3551 251 subProp[i]->setValue(OFS, tmp);
MasashiNomura 29:eb3d72dd94aa 252 }
MasashiNomura 29:eb3d72dd94aa 253 }
MasashiNomura 45:3b51dd26b579 254 else
MasashiNomura 45:3b51dd26b579 255 {
MasashiNomura 45:3b51dd26b579 256 subProp[i]->setValue(OFS, tmp);
MasashiNomura 45:3b51dd26b579 257 }
MasashiNomura 29:eb3d72dd94aa 258 }
MasashiNomura 28:fdb3b144e342 259
MasashiNomura 32:7f4145cc3551 260 for(int i = 0; i< 4;++i){
MasashiNomura 32:7f4145cc3551 261 if(gf_MtReq[i].req){
MasashiNomura 33:eb260dbfc22a 262 subProp[i]->setValue(ATT,gf_MtReq[i].val);
MasashiNomura 36:2cc739c7e4cb 263 gf_MtReq[i].req = false;
MasashiNomura 29:eb3d72dd94aa 264 }
MasashiNomura 45:3b51dd26b579 265 else
MasashiNomura 45:3b51dd26b579 266 {
MasashiNomura 45:3b51dd26b579 267 tmp = subProp[i]->getValue(ATT);
MasashiNomura 45:3b51dd26b579 268 subProp[i]->setValue(ATT,tmp);
MasashiNomura 45:3b51dd26b579 269 }
MasashiNomura 29:eb3d72dd94aa 270 }
takeru0x1103 19:4b0fe9a5ec38 271
MasashiNomura 48:71aec693a7dc 272 for(int i = 0; i < 4; ++i){
MasashiNomura 48:71aec693a7dc 273 subProp[i]->setValue();
MasashiNomura 48:71aec693a7dc 274 }
MasashiNomura 48:71aec693a7dc 275
MasashiNomura 41:45c982b1c5b6 276 if(gf_Print.d1.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 277 if(gf_Print.d1.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 278 if(gf_Print.d1.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 279 if(gf_Print.d1.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); }
MasashiNomura 40:debe99e228d3 280
MasashiNomura 40:debe99e228d3 281 //DEBUG用FPGAに直接指示
MasashiNomura 40:debe99e228d3 282 INT16 pos;
MasashiNomura 40:debe99e228d3 283 INT16 typ;
MasashiNomura 40:debe99e228d3 284 if(gf_State == SLEEP){
MasashiNomura 40:debe99e228d3 285 for(int i = 0; i < 8; ++i){
MasashiNomura 40:debe99e228d3 286 pos = i / 2;
MasashiNomura 40:debe99e228d3 287 typ = i % 2;
MasashiNomura 40:debe99e228d3 288 if(gf_MtReqDct[i].req){
MasashiNomura 40:debe99e228d3 289 tmp = subProp[pos]->getValueFpgaMot((eMotType)typ);
MasashiNomura 40:debe99e228d3 290 dif = tmp - (INT16)gf_MtReqDct[i].val;
MasashiNomura 40:debe99e228d3 291 if(dif == 0){
MasashiNomura 40:debe99e228d3 292 subProp[pos]->setValueFpgaMot((eMotType)typ, (INT16)gf_MtReqDct[i].val);
MasashiNomura 40:debe99e228d3 293 gf_MtReqDct[i].req = false;
MasashiNomura 40:debe99e228d3 294 }
MasashiNomura 40:debe99e228d3 295 else {
MasashiNomura 40:debe99e228d3 296 if(1000 < dif){
MasashiNomura 40:debe99e228d3 297 tmp-=100;
MasashiNomura 40:debe99e228d3 298 }
MasashiNomura 40:debe99e228d3 299 else if(100 < dif){
MasashiNomura 40:debe99e228d3 300 tmp-= 10;
MasashiNomura 40:debe99e228d3 301 }
MasashiNomura 40:debe99e228d3 302 else if(100 >= dif && 10 < dif){
MasashiNomura 40:debe99e228d3 303 tmp-= 5;
MasashiNomura 40:debe99e228d3 304 }
MasashiNomura 40:debe99e228d3 305 else if ( 10 >= dif && 1 <= dif){
MasashiNomura 40:debe99e228d3 306 tmp-=1;
MasashiNomura 40:debe99e228d3 307 }
MasashiNomura 40:debe99e228d3 308 else if ( -1000 > dif){
MasashiNomura 40:debe99e228d3 309 tmp+=100;
MasashiNomura 40:debe99e228d3 310 }
MasashiNomura 40:debe99e228d3 311 else if ( -100 >= dif){
MasashiNomura 40:debe99e228d3 312 tmp+=10;
MasashiNomura 40:debe99e228d3 313 }
MasashiNomura 40:debe99e228d3 314 else if(-100 <= dif && -10 > dif){
MasashiNomura 40:debe99e228d3 315 tmp+= 5;
MasashiNomura 40:debe99e228d3 316 }
MasashiNomura 40:debe99e228d3 317 else if ( -10 <= dif && -1 >= dif){
MasashiNomura 40:debe99e228d3 318 tmp+=1;
MasashiNomura 40:debe99e228d3 319 }
MasashiNomura 40:debe99e228d3 320 subProp[pos]->setValueFpgaMot((eMotType)typ, tmp);
MasashiNomura 40:debe99e228d3 321 }
MasashiNomura 40:debe99e228d3 322 }
MasashiNomura 40:debe99e228d3 323 }
MasashiNomura 40:debe99e228d3 324 }
MasashiNomura 26:732bc37fbefd 325 }
MasashiNomura 26:732bc37fbefd 326
MasashiNomura 30:13ada1a24c59 327 INT16 HbManager::getCurMotVal(eMotPos pos){
MasashiNomura 30:13ada1a24c59 328 if(subProp[(int)pos] == NULL) return 0;
MasashiNomura 32:7f4145cc3551 329 return subProp[(int)pos]->getValue(USER);
MasashiNomura 30:13ada1a24c59 330 }
MasashiNomura 46:5074781a28dd 331
MasashiNomura 30:13ada1a24c59 332 void HbManager::setMotVal(eMotPos pos, INT16 val){
MasashiNomura 30:13ada1a24c59 333 if(subProp[(int)pos] == NULL) return;
MasashiNomura 32:7f4145cc3551 334 subProp[(int)pos]->setValue(USER, val);
MasashiNomura 30:13ada1a24c59 335 }
MasashiNomura 25:f3a6e7eec9c3 336
MasashiNomura 46:5074781a28dd 337 void HbManager::setMotValOfs(eMotPos pos, INT16 val){
MasashiNomura 46:5074781a28dd 338 if(subProp[(int)pos] == NULL) return;
MasashiNomura 46:5074781a28dd 339 subProp[(int)pos]->setValue(OFS, val);
MasashiNomura 46:5074781a28dd 340 }
MasashiNomura 46:5074781a28dd 341
MasashiNomura 40:debe99e228d3 342 void HbManager::setFpgaMot(eMotPos pos,eMotType type, INT16 val){
MasashiNomura 40:debe99e228d3 343 if(subProp[(int)pos] == NULL) return;
MasashiNomura 40:debe99e228d3 344 subProp[(int)pos]->setValueFpgaMot(type, val);
MasashiNomura 40:debe99e228d3 345 }
MasashiNomura 40:debe99e228d3 346
MasashiNomura 36:2cc739c7e4cb 347 // void HbManager::setMotFPGA(UCHAR num, INT16 val){
MasashiNomura 36:2cc739c7e4cb 348 // //UCHAR tmpN = num / 2;
MasashiNomura 36:2cc739c7e4cb 349 // //UCHAR tmp = num % 2;
MasashiNomura 36:2cc739c7e4cb 350 // // mot[tmpN]->setValueFPGA(tmp, val);
MasashiNomura 36:2cc739c7e4cb 351 // // sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val);
MasashiNomura 36:2cc739c7e4cb 352 // }
MasashiNomura 27:ff63c23bc689 353
takeru0x1103 17:f9610f3cfa1b 354 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 355 //エンジン制御
takeru0x1103 17:f9610f3cfa1b 356 //------------------------------------------------------
MasashiNomura 39:1b76f7df8804 357 void HbManager::getEngine(){
takeru0x1103 18:5aa48aec9cae 358 UINT16 rpm[2];
takeru0x1103 18:5aa48aec9cae 359 int i;
takeru0x1103 17:f9610f3cfa1b 360
takeru0x1103 18:5aa48aec9cae 361 //エンジン回転数読み出し
takeru0x1103 18:5aa48aec9cae 362 rpm[0] = eng[0]->getRpm();
takeru0x1103 18:5aa48aec9cae 363 rpm[1] = eng[1]->getRpm();
takeru0x1103 17:f9610f3cfa1b 364
MasashiNomura 41:45c982b1c5b6 365 if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); }
MasashiNomura 41:45c982b1c5b6 366 if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); }
MasashiNomura 42:cc8501b824ba 367 if(gf_Print.d2.bf.ep1){sp.printf("set %d hover %d ", getAccelVal(FRONT), getHvAxl(FRONT));}
MasashiNomura 42:cc8501b824ba 368 if(gf_Print.d2.bf.ep2){sp.printf("set %d hover %d ", getAccelVal(REAR), getHvAxl(REAR));}
MasashiNomura 42:cc8501b824ba 369
MasashiNomura 42:cc8501b824ba 370 if(gf_AxReqH[0].bf.req){
MasashiNomura 42:cc8501b824ba 371 setHvAxl(FRONT,gf_AxReqH[0].bf.val);
MasashiNomura 42:cc8501b824ba 372 gf_AxReqH[0].bf.req = false;
MasashiNomura 42:cc8501b824ba 373 }
MasashiNomura 42:cc8501b824ba 374 if(gf_AxReqH[1].bf.req){
MasashiNomura 42:cc8501b824ba 375 setHvAxl(REAR,gf_AxReqH[1].bf.val);
MasashiNomura 42:cc8501b824ba 376 gf_AxReqH[1].bf.req = false;
MasashiNomura 42:cc8501b824ba 377 }
MasashiNomura 43:156199c2f9b6 378 for(int i = 0; i < 2; ++i){
MasashiNomura 43:156199c2f9b6 379 if(gf_AxStepReq[i].bf.req){
MasashiNomura 43:156199c2f9b6 380 en_srv_step[i] = gf_AxStepReq[i].bf.val;
MasashiNomura 43:156199c2f9b6 381 gf_AxStepReq[i].bf.req = false;
MasashiNomura 43:156199c2f9b6 382 }
MasashiNomura 43:156199c2f9b6 383 }
MasashiNomura 39:1b76f7df8804 384 }
MasashiNomura 39:1b76f7df8804 385
MasashiNomura 39:1b76f7df8804 386 void HbManager::controlEngine(){
MasashiNomura 39:1b76f7df8804 387 int i;
MasashiNomura 41:45c982b1c5b6 388 INT16 tmp, dif;
MasashiNomura 45:3b51dd26b579 389 INT16 step = 100;
takeru0x1103 18:5aa48aec9cae 390 //アクセル設定
takeru0x1103 18:5aa48aec9cae 391 for(i=0; i<2; i++){
MasashiNomura 38:24ee50452755 392 if(gf_AxReq[i].bf.req==true){
MasashiNomura 45:3b51dd26b579 393 step = en_srv_step[i];
MasashiNomura 41:45c982b1c5b6 394 tmp = eng[i]->getAccel();
MasashiNomura 41:45c982b1c5b6 395 dif = tmp - (INT16)gf_AxReq[i].bf.val;
MasashiNomura 41:45c982b1c5b6 396 if(dif == 0){
MasashiNomura 45:3b51dd26b579 397 accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 45:3b51dd26b579 398 eng[i]->setAccel(accelVal[i]);
MasashiNomura 41:45c982b1c5b6 399 gf_AxReq[i].bf.req=false;
MasashiNomura 42:cc8501b824ba 400 //sp.printf("Servo Moving FINISH!!\r\n");
MasashiNomura 41:45c982b1c5b6 401 }
MasashiNomura 41:45c982b1c5b6 402 else {
MasashiNomura 41:45c982b1c5b6 403 if(dif > 0){
MasashiNomura 41:45c982b1c5b6 404 if(dif > step){
MasashiNomura 41:45c982b1c5b6 405 tmp -= step;
MasashiNomura 41:45c982b1c5b6 406 }
MasashiNomura 41:45c982b1c5b6 407 else
MasashiNomura 41:45c982b1c5b6 408 {
MasashiNomura 41:45c982b1c5b6 409 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 410 }
MasashiNomura 41:45c982b1c5b6 411 }
MasashiNomura 41:45c982b1c5b6 412 else {
MasashiNomura 41:45c982b1c5b6 413 if( abs(dif) > step){
MasashiNomura 41:45c982b1c5b6 414 tmp += step;
MasashiNomura 41:45c982b1c5b6 415 }
MasashiNomura 41:45c982b1c5b6 416 else
MasashiNomura 41:45c982b1c5b6 417 {
MasashiNomura 41:45c982b1c5b6 418 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 419 }
MasashiNomura 41:45c982b1c5b6 420 }
MasashiNomura 41:45c982b1c5b6 421 eng[i]->setAccel(tmp);
MasashiNomura 41:45c982b1c5b6 422 }
MasashiNomura 41:45c982b1c5b6 423 //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
MasashiNomura 45:3b51dd26b579 424 }
MasashiNomura 45:3b51dd26b579 425 else
MasashiNomura 45:3b51dd26b579 426 {// 直前でセットされた値をセット
MasashiNomura 45:3b51dd26b579 427 eng[i]->setAccel(accelVal[i]);
takeru0x1103 18:5aa48aec9cae 428 }
takeru0x1103 17:f9610f3cfa1b 429 }
MasashiNomura 36:2cc739c7e4cb 430 }
MasashiNomura 46:5074781a28dd 431
MasashiNomura 46:5074781a28dd 432 void HbManager::controlEngine(enmHbState stat){
MasashiNomura 46:5074781a28dd 433 int i;
MasashiNomura 46:5074781a28dd 434 INT16 tmp, dif;
MasashiNomura 46:5074781a28dd 435 INT16 step = 100;
MasashiNomura 46:5074781a28dd 436 //アクセル設定
MasashiNomura 46:5074781a28dd 437 if(stat == CHK_EG_F || stat == CHK_EG_R){
MasashiNomura 46:5074781a28dd 438 for(i=0; i<2; i++){
MasashiNomura 46:5074781a28dd 439 if(gf_AxReq[i].bf.req==true){
MasashiNomura 46:5074781a28dd 440 accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 46:5074781a28dd 441 eng[i]->setAccel(accelVal[i]);
MasashiNomura 46:5074781a28dd 442 gf_AxReq[i].bf.req=false;
MasashiNomura 46:5074781a28dd 443 //sp.printf("val=%d\r\n" , accelVal[i]);
MasashiNomura 46:5074781a28dd 444 }
MasashiNomura 46:5074781a28dd 445 else
MasashiNomura 46:5074781a28dd 446 {// 直前でセットされた値をセット
MasashiNomura 46:5074781a28dd 447 eng[i]->setAccel(accelVal[i]);
MasashiNomura 46:5074781a28dd 448 }
MasashiNomura 46:5074781a28dd 449 }
MasashiNomura 46:5074781a28dd 450 }else {
MasashiNomura 46:5074781a28dd 451 for(i=0; i<2; i++){
MasashiNomura 46:5074781a28dd 452 if(gf_AxReq[i].bf.req==true){
MasashiNomura 46:5074781a28dd 453 step = en_srv_step[i];
MasashiNomura 46:5074781a28dd 454 tmp = eng[i]->getAccel();
MasashiNomura 46:5074781a28dd 455 dif = tmp - (INT16)gf_AxReq[i].bf.val;
MasashiNomura 46:5074781a28dd 456 if(dif == 0){
MasashiNomura 46:5074781a28dd 457 accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 46:5074781a28dd 458 eng[i]->setAccel(accelVal[i]);
MasashiNomura 46:5074781a28dd 459 gf_AxReq[i].bf.req=false;
MasashiNomura 46:5074781a28dd 460 //sp.printf("Servo Moving FINISH!!\r\n");
MasashiNomura 46:5074781a28dd 461 }
MasashiNomura 46:5074781a28dd 462 else {
MasashiNomura 46:5074781a28dd 463 if(dif > 0){
MasashiNomura 46:5074781a28dd 464 if(dif > step){
MasashiNomura 46:5074781a28dd 465 tmp -= step;
MasashiNomura 46:5074781a28dd 466 }
MasashiNomura 46:5074781a28dd 467 else
MasashiNomura 46:5074781a28dd 468 {
MasashiNomura 46:5074781a28dd 469 tmp -= dif;
MasashiNomura 46:5074781a28dd 470 }
MasashiNomura 46:5074781a28dd 471 }
MasashiNomura 46:5074781a28dd 472 else {
MasashiNomura 46:5074781a28dd 473 if( abs(dif) > step){
MasashiNomura 46:5074781a28dd 474 tmp += step;
MasashiNomura 46:5074781a28dd 475 }
MasashiNomura 46:5074781a28dd 476 else
MasashiNomura 46:5074781a28dd 477 {
MasashiNomura 46:5074781a28dd 478 tmp -= dif;
MasashiNomura 46:5074781a28dd 479 }
MasashiNomura 46:5074781a28dd 480 }
MasashiNomura 46:5074781a28dd 481 eng[i]->setAccel(tmp);
MasashiNomura 46:5074781a28dd 482 }
MasashiNomura 46:5074781a28dd 483 //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
MasashiNomura 46:5074781a28dd 484 }
MasashiNomura 46:5074781a28dd 485 else
MasashiNomura 46:5074781a28dd 486 {// 直前でセットされた値をセット
MasashiNomura 46:5074781a28dd 487 eng[i]->setAccel(accelVal[i]);
MasashiNomura 46:5074781a28dd 488 }
MasashiNomura 46:5074781a28dd 489 }
MasashiNomura 46:5074781a28dd 490 }
MasashiNomura 46:5074781a28dd 491 }
MasashiNomura 46:5074781a28dd 492
MasashiNomura 36:2cc739c7e4cb 493 void HbManager::clearHvAxl(){
MasashiNomura 36:2cc739c7e4cb 494 for(int i = 0; i < 2; ++i){
MasashiNomura 36:2cc739c7e4cb 495 eng[i]->clearHoverAccel();
MasashiNomura 36:2cc739c7e4cb 496 }
MasashiNomura 36:2cc739c7e4cb 497 }
MasashiNomura 36:2cc739c7e4cb 498 bool HbManager::chkSetHvAxl(eEgPos pos){
MasashiNomura 36:2cc739c7e4cb 499 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 500 return eng[0]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 501 }
MasashiNomura 36:2cc739c7e4cb 502 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 503 return eng[1]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 504 }
takeru0x1103 17:f9610f3cfa1b 505 }
MasashiNomura 36:2cc739c7e4cb 506 void HbManager::setHvAxl(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 507 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 508 eng[0]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 509 }
MasashiNomura 36:2cc739c7e4cb 510 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 511 eng[1]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 512 }
MasashiNomura 36:2cc739c7e4cb 513 }
MasashiNomura 38:24ee50452755 514 INT16 HbManager::getHvAxl(eEgPos pos){
MasashiNomura 38:24ee50452755 515 if(pos == FRONT){
MasashiNomura 38:24ee50452755 516 return eng[0]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 517 }
MasashiNomura 38:24ee50452755 518 else/* if(pos == REAR)*/{
MasashiNomura 38:24ee50452755 519 return eng[1]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 520 }
MasashiNomura 38:24ee50452755 521 }
MasashiNomura 36:2cc739c7e4cb 522 void HbManager::setAccelVal(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 523 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 524 gf_AxReq[0].bf.req = true;
MasashiNomura 38:24ee50452755 525 gf_AxReq[0].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 526 } else {
MasashiNomura 36:2cc739c7e4cb 527 gf_AxReq[1].bf.req = true;
MasashiNomura 38:24ee50452755 528 gf_AxReq[1].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 529 }
MasashiNomura 36:2cc739c7e4cb 530 }
MasashiNomura 39:1b76f7df8804 531 INT16 HbManager::getAccelVal(eEgPos pos){
MasashiNomura 39:1b76f7df8804 532 if(pos == FRONT){
MasashiNomura 39:1b76f7df8804 533 return eng[0]->getAccel();
MasashiNomura 39:1b76f7df8804 534 }
MasashiNomura 39:1b76f7df8804 535 else/* if(pos == REAR)*/{
MasashiNomura 39:1b76f7df8804 536 return eng[1]->getAccel();
MasashiNomura 39:1b76f7df8804 537 }
MasashiNomura 39:1b76f7df8804 538 }
MasashiNomura 23:79e20be4bc5b 539 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 540 //ステート遷移関連
MasashiNomura 23:79e20be4bc5b 541 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 542 bool HbManager::chkOverIDLE(){
MasashiNomura 23:79e20be4bc5b 543 return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
MasashiNomura 23:79e20be4bc5b 544 }
MasashiNomura 23:79e20be4bc5b 545
MasashiNomura 23:79e20be4bc5b 546 bool HbManager::chkInRangeIDLE(){
MasashiNomura 23:79e20be4bc5b 547 return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
MasashiNomura 23:79e20be4bc5b 548 }
MasashiNomura 23:79e20be4bc5b 549 // ステート遷移関連end
MasashiNomura 23:79e20be4bc5b 550 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 551
MasashiNomura 23:79e20be4bc5b 552 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 553 //ユーザー操作関連
MasashiNomura 23:79e20be4bc5b 554 //------------------------------------------------------
MasashiNomura 22:24c9c2dedca9 555 void HbManager::getUserCommand(){
MasashiNomura 22:24c9c2dedca9 556 usrSW = ope->GetUserOpe();
MasashiNomura 23:79e20be4bc5b 557 //sp.printf("SW Val=%X\r\n", usrSW.w);
MasashiNomura 22:24c9c2dedca9 558 }
takeru0x1103 17:f9610f3cfa1b 559
MasashiNomura 38:24ee50452755 560 float HbManager::getUserMotAxlRaw(){
MasashiNomura 38:24ee50452755 561 return ope->GetAinAccelRaw();
MasashiNomura 38:24ee50452755 562 }
MasashiNomura 38:24ee50452755 563
MasashiNomura 33:eb260dbfc22a 564 INT16 HbManager::getUserMotAxl(){
MasashiNomura 33:eb260dbfc22a 565 return ope->GetAinAccel();
MasashiNomura 33:eb260dbfc22a 566 }
MasashiNomura 33:eb260dbfc22a 567
MasashiNomura 38:24ee50452755 568 float HbManager::getUserEngTrottleRaw(){
MasashiNomura 38:24ee50452755 569 return ope->GetAinThrottleRaw();
MasashiNomura 38:24ee50452755 570 }
MasashiNomura 38:24ee50452755 571
MasashiNomura 36:2cc739c7e4cb 572 INT16 HbManager::getUserEngTrottle(){
MasashiNomura 36:2cc739c7e4cb 573 return ope->GetAinThrottle();
MasashiNomura 36:2cc739c7e4cb 574 }
MasashiNomura 36:2cc739c7e4cb 575
MasashiNomura 23:79e20be4bc5b 576 bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
MasashiNomura 23:79e20be4bc5b 577 return ope->ChkCtrlSW(stype);
MasashiNomura 23:79e20be4bc5b 578 }
MasashiNomura 30:13ada1a24c59 579 bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
MasashiNomura 26:732bc37fbefd 580 return ope->ChkCtrlSwRiseEdge(stype);
MasashiNomura 26:732bc37fbefd 581 }
MasashiNomura 23:79e20be4bc5b 582 bool HbManager::chkSWUserOpeAny(){
MasashiNomura 23:79e20be4bc5b 583 return ope->ChkCtrlSwAny();
MasashiNomura 23:79e20be4bc5b 584 }
MasashiNomura 36:2cc739c7e4cb 585
MasashiNomura 36:2cc739c7e4cb 586 bool HbManager::chkSWUserOpeBoth(HbUserOpe::SW_TYPE stype1,HbUserOpe::SW_TYPE stype2){
MasashiNomura 36:2cc739c7e4cb 587 return ope->ChkCtrlSwBoth(stype1,stype2);
MasashiNomura 36:2cc739c7e4cb 588 }
MasashiNomura 36:2cc739c7e4cb 589
MasashiNomura 23:79e20be4bc5b 590 typUserSw HbManager::getUserSw(){
MasashiNomura 23:79e20be4bc5b 591 return usrSW;
MasashiNomura 23:79e20be4bc5b 592 }
MasashiNomura 31:56c554c560c1 593
MasashiNomura 31:56c554c560c1 594 void HbManager::chkSW(enmHbState stat){
MasashiNomura 31:56c554c560c1 595
MasashiNomura 39:1b76f7df8804 596 INT16 tmpRpm,tmpAxl;
MasashiNomura 31:56c554c560c1 597 // Front Left
MasashiNomura 31:56c554c560c1 598 if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 599 gf_MtReqU[0].req=true;
MasashiNomura 31:56c554c560c1 600 tmpRpm = getCurMotVal(F_L);
MasashiNomura 35:3779201b4c73 601 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 602 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 603 }
MasashiNomura 47:d3fa874f336e 604 //nowTrgtAng -= 1;
MasashiNomura 47:d3fa874f336e 605
MasashiNomura 31:56c554c560c1 606 } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 607 if(gf_MtReqU[0].req==true){
MasashiNomura 35:3779201b4c73 608 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 609 }
MasashiNomura 47:d3fa874f336e 610 //nowTrgtAng -= 0.1;
MasashiNomura 31:56c554c560c1 611 } else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 612 if(gf_MtReqU[0].req){
MasashiNomura 31:56c554c560c1 613 tmpRpm = getCurMotVal(F_L);
MasashiNomura 31:56c554c560c1 614 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 615 gf_MtReqU[0].req = false;
MasashiNomura 31:56c554c560c1 616 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 617 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 618 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 619 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 620 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 621 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 622 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 623 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 624 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 625 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 626 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 627 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 628 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 629 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 630 }
MasashiNomura 31:56c554c560c1 631 setMotVal(F_L, tmpRpm);
MasashiNomura 31:56c554c560c1 632 }
MasashiNomura 31:56c554c560c1 633 }
MasashiNomura 31:56c554c560c1 634
MasashiNomura 31:56c554c560c1 635 if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 636 gf_MtReqU[1].req=true;
MasashiNomura 31:56c554c560c1 637 tmpRpm = getCurMotVal(F_R);
MasashiNomura 35:3779201b4c73 638 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 639 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 640 }
MasashiNomura 47:d3fa874f336e 641 //nowTrgtAng += 1;
MasashiNomura 31:56c554c560c1 642 } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 643 if(gf_MtReqU[1].req==true){
MasashiNomura 35:3779201b4c73 644 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 645 }
MasashiNomura 47:d3fa874f336e 646 //nowTrgtAng += 0.1;
MasashiNomura 31:56c554c560c1 647 } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 648 if(gf_MtReqU[1].req){
MasashiNomura 31:56c554c560c1 649 tmpRpm = getCurMotVal(F_R);
MasashiNomura 31:56c554c560c1 650 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 651 gf_MtReqU[1].req = false;
MasashiNomura 31:56c554c560c1 652 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 653 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 654 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 655 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 656 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 657 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 658 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 659 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 660 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 661 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 662 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 663 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 664 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 665 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 666 }
MasashiNomura 31:56c554c560c1 667 setMotVal(F_R, tmpRpm);
MasashiNomura 31:56c554c560c1 668 }
MasashiNomura 31:56c554c560c1 669 }
MasashiNomura 31:56c554c560c1 670
MasashiNomura 34:234b87f3e6ce 671 if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
MasashiNomura 39:1b76f7df8804 672 if(stat == IDLE){
MasashiNomura 39:1b76f7df8804 673 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 674 gf_AxReq[1].bf.req = true;
MasashiNomura 47:d3fa874f336e 675 gf_AxReq[0].bf.val = getHvAxl(FRONT) / 2;
MasashiNomura 47:d3fa874f336e 676 gf_AxReq[1].bf.val = getHvAxl(REAR) / 2;
MasashiNomura 47:d3fa874f336e 677 setStateF(UPPER_IDLE);
MasashiNomura 47:d3fa874f336e 678 }
MasashiNomura 47:d3fa874f336e 679 else if(stat == UPPER_IDLE){
MasashiNomura 47:d3fa874f336e 680 gf_AxReq[0].bf.req = true;
MasashiNomura 47:d3fa874f336e 681 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 682 gf_AxReq[0].bf.val = getHvAxl(FRONT);
MasashiNomura 39:1b76f7df8804 683 gf_AxReq[1].bf.val = getHvAxl(REAR);
MasashiNomura 39:1b76f7df8804 684 setStateF(TAKE_OFF);
MasashiNomura 39:1b76f7df8804 685 }
MasashiNomura 34:234b87f3e6ce 686 sp.printf("FLT_ON Push \r\n");
MasashiNomura 34:234b87f3e6ce 687 }
MasashiNomura 34:234b87f3e6ce 688 if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){
MasashiNomura 47:d3fa874f336e 689 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == UPPER_IDLE || stat == IDLE){
MasashiNomura 42:cc8501b824ba 690 for(int i = 0; i < 2; ++i){
MasashiNomura 47:d3fa874f336e 691 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 47:d3fa874f336e 692 setHvAxl((eEgPos)i, gf_AxReq[i].bf.val);
MasashiNomura 47:d3fa874f336e 693 }
MasashiNomura 42:cc8501b824ba 694 gf_AxReq[i].bf.req = true;
MasashiNomura 42:cc8501b824ba 695 gf_AxReq[i].bf.val = 0;
MasashiNomura 42:cc8501b824ba 696 }
MasashiNomura 39:1b76f7df8804 697 if(stat == DRIVE){
MasashiNomura 39:1b76f7df8804 698 setStateF(EMGGND);
MasashiNomura 39:1b76f7df8804 699 } else {
MasashiNomura 39:1b76f7df8804 700 setStateF(GROUND);
MasashiNomura 39:1b76f7df8804 701 }
MasashiNomura 39:1b76f7df8804 702 }
MasashiNomura 34:234b87f3e6ce 703 sp.printf("FLT_OFF Push \r\n");
MasashiNomura 31:56c554c560c1 704 }
MasashiNomura 45:3b51dd26b579 705 //if(chkSWUserOpe(HbUserOpe::F_ENG_UP)){
MasashiNomura 45:3b51dd26b579 706 if(chkSWUserOpeRE(HbUserOpe::F_ENG_UP)){
MasashiNomura 39:1b76f7df8804 707 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 43:156199c2f9b6 708 gf_BlinkLED = true;
MasashiNomura 46:5074781a28dd 709 INT16 step = 20;
MasashiNomura 39:1b76f7df8804 710 tmpAxl = getAccelVal(FRONT);
MasashiNomura 42:cc8501b824ba 711 tmpAxl+=step;
MasashiNomura 42:cc8501b824ba 712 if(tmpAxl > MAX_VAL_12BIT){
MasashiNomura 42:cc8501b824ba 713 tmpAxl = MAX_VAL_12BIT;
MasashiNomura 39:1b76f7df8804 714 }
MasashiNomura 39:1b76f7df8804 715 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 716 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 717 }
MasashiNomura 39:1b76f7df8804 718 }
MasashiNomura 45:3b51dd26b579 719 //if(chkSWUserOpe(HbUserOpe::F_ENG_DOWN)){
MasashiNomura 45:3b51dd26b579 720 if(chkSWUserOpeRE(HbUserOpe::F_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 721 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 43:156199c2f9b6 722 gf_BlinkLED = true;
MasashiNomura 46:5074781a28dd 723 INT16 step = 20;
MasashiNomura 39:1b76f7df8804 724 tmpAxl = getAccelVal(FRONT);
MasashiNomura 42:cc8501b824ba 725 tmpAxl-=step;
MasashiNomura 42:cc8501b824ba 726 if(tmpAxl < 0){
MasashiNomura 42:cc8501b824ba 727 tmpAxl = 0;
MasashiNomura 39:1b76f7df8804 728 }
MasashiNomura 39:1b76f7df8804 729 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 730 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 31:56c554c560c1 731 }
MasashiNomura 39:1b76f7df8804 732 }
MasashiNomura 45:3b51dd26b579 733 //if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
MasashiNomura 45:3b51dd26b579 734 if(chkSWUserOpeRE(HbUserOpe::R_ENG_UP)){
MasashiNomura 39:1b76f7df8804 735 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 46:5074781a28dd 736 INT16 step = 20;
MasashiNomura 43:156199c2f9b6 737 gf_BlinkLED = true;
MasashiNomura 39:1b76f7df8804 738 tmpAxl = getAccelVal(REAR);
MasashiNomura 42:cc8501b824ba 739 tmpAxl+=step;
MasashiNomura 42:cc8501b824ba 740 if(tmpAxl > MAX_VAL_12BIT){
MasashiNomura 42:cc8501b824ba 741 tmpAxl = MAX_VAL_12BIT;
MasashiNomura 39:1b76f7df8804 742 }
MasashiNomura 39:1b76f7df8804 743 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 744 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 36:2cc739c7e4cb 745 }
MasashiNomura 39:1b76f7df8804 746 }
MasashiNomura 45:3b51dd26b579 747 //if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
MasashiNomura 45:3b51dd26b579 748 if(chkSWUserOpeRE(HbUserOpe::R_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 749 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 46:5074781a28dd 750 INT16 step = 20;
MasashiNomura 43:156199c2f9b6 751 gf_BlinkLED = true;
MasashiNomura 39:1b76f7df8804 752 tmpAxl = getAccelVal(REAR);
MasashiNomura 42:cc8501b824ba 753 tmpAxl-=step;
MasashiNomura 42:cc8501b824ba 754 if(tmpAxl < 0){
MasashiNomura 42:cc8501b824ba 755 tmpAxl = 0;
MasashiNomura 39:1b76f7df8804 756 }
MasashiNomura 39:1b76f7df8804 757 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 758 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 759 }
MasashiNomura 39:1b76f7df8804 760 }
MasashiNomura 39:1b76f7df8804 761 // if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
MasashiNomura 39:1b76f7df8804 762 // // モーター
MasashiNomura 39:1b76f7df8804 763 // for(int i = 0; i < 4; ++i){
MasashiNomura 39:1b76f7df8804 764 // gf_MtReq[i].req = true;
MasashiNomura 39:1b76f7df8804 765 // gf_MtReq[i].val = 0;
MasashiNomura 39:1b76f7df8804 766 // gf_MtReqOfs[i].req = true;
MasashiNomura 39:1b76f7df8804 767 // gf_MtReqOfs[i].val = 0;
MasashiNomura 39:1b76f7df8804 768 // }
MasashiNomura 39:1b76f7df8804 769 // // エンジン用サーボ
MasashiNomura 39:1b76f7df8804 770 // for(int i = 0; i < 2; ++i){
MasashiNomura 39:1b76f7df8804 771 // gf_AxReq[i].bf.req = true;
MasashiNomura 39:1b76f7df8804 772 // gf_AxReq[i].bf.val = 0;
MasashiNomura 39:1b76f7df8804 773 // }
MasashiNomura 39:1b76f7df8804 774 // gf_FromActiveStat = isActiveState();
MasashiNomura 39:1b76f7df8804 775 // setStateF(MOT_STOP);
MasashiNomura 39:1b76f7df8804 776 // }
MasashiNomura 31:56c554c560c1 777 }
MasashiNomura 23:79e20be4bc5b 778 // ユーザー操作関連end
MasashiNomura 23:79e20be4bc5b 779 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 780
takeru0x1103 17:f9610f3cfa1b 781 //======================================================
takeru0x1103 17:f9610f3cfa1b 782 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 783 //======================================================
takeru0x1103 17:f9610f3cfa1b 784 HbManager::HbManager(){
takeru0x1103 18:5aa48aec9cae 785 //メンバクラスインスタンス
takeru0x1103 18:5aa48aec9cae 786 eng[0] = new HbEngine(0);
takeru0x1103 18:5aa48aec9cae 787 eng[1] = new HbEngine(1);
MasashiNomura 42:cc8501b824ba 788 eng[0]->setHoverAccel(1500);
MasashiNomura 42:cc8501b824ba 789 eng[1]->setHoverAccel(1500);
MasashiNomura 47:d3fa874f336e 790 att = new HbAttitude(2.0 , 2.0 , 0 , 0);//パラメータ outP P I D
MasashiNomura 45:3b51dd26b579 791
MasashiNomura 45:3b51dd26b579 792 accelVal[0] = 0;
MasashiNomura 45:3b51dd26b579 793 accelVal[1] = 0;
MasashiNomura 28:fdb3b144e342 794 // mot[0] = new HbMotor(0);
MasashiNomura 28:fdb3b144e342 795 // mot[1] = new HbMotor(1);
MasashiNomura 28:fdb3b144e342 796 // mot[2] = new HbMotor(2);
MasashiNomura 28:fdb3b144e342 797 // mot[3] = new HbMotor(3);
MasashiNomura 28:fdb3b144e342 798
MasashiNomura 42:cc8501b824ba 799 // subProp[0] = new HbSubProp(F_L);
MasashiNomura 42:cc8501b824ba 800 // subProp[0]->setCoef(IN, 0.00004, 0.0401, 29.262);
MasashiNomura 42:cc8501b824ba 801 // subProp[0]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 42:cc8501b824ba 802
MasashiNomura 42:cc8501b824ba 803 // subProp[1] = new HbSubProp(F_R);
MasashiNomura 42:cc8501b824ba 804 // subProp[1]->setCoef(IN, 0.00004, 0.0151, 70.947);
MasashiNomura 42:cc8501b824ba 805 // subProp[1]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 42:cc8501b824ba 806
MasashiNomura 42:cc8501b824ba 807 // subProp[2] = new HbSubProp(R_L);
MasashiNomura 42:cc8501b824ba 808 // subProp[2]->setCoef(IN, 0.00004, 0.0244, 53.954);
MasashiNomura 42:cc8501b824ba 809 // subProp[2]->setCoef(OUT,0.00003, 0.1579, -114.22);
MasashiNomura 42:cc8501b824ba 810
MasashiNomura 42:cc8501b824ba 811 // subProp[3] = new HbSubProp(R_R);
MasashiNomura 42:cc8501b824ba 812 // subProp[3]->setCoef(IN, 0.00004, 0.0111, 82.044);
MasashiNomura 42:cc8501b824ba 813 // subProp[3]->setCoef(OUT,0.00003, 0.1617, -123.27);
MasashiNomura 42:cc8501b824ba 814
MasashiNomura 28:fdb3b144e342 815 subProp[0] = new HbSubProp(F_L);
MasashiNomura 42:cc8501b824ba 816 subProp[0]->setCoef(IN, 0.00004, 0.0786, -5.248);
MasashiNomura 42:cc8501b824ba 817 subProp[0]->setCoef(OUT,0.00005, 0.0112, 104.45);
MasashiNomura 22:24c9c2dedca9 818
MasashiNomura 28:fdb3b144e342 819 subProp[1] = new HbSubProp(F_R);
MasashiNomura 42:cc8501b824ba 820 subProp[1]->setCoef(IN, 0.00005, 0.0095, 126.11);
MasashiNomura 42:cc8501b824ba 821 subProp[1]->setCoef(OUT,0.00004, 0.0798, -2.1167);
MasashiNomura 28:fdb3b144e342 822
MasashiNomura 28:fdb3b144e342 823 subProp[2] = new HbSubProp(R_L);
MasashiNomura 42:cc8501b824ba 824 subProp[2]->setCoef(IN, 0.00004, 0.1039, -50.946);
MasashiNomura 42:cc8501b824ba 825 subProp[2]->setCoef(OUT,0.00004, 0.0886, -21.033);
MasashiNomura 28:fdb3b144e342 826
MasashiNomura 28:fdb3b144e342 827 subProp[3] = new HbSubProp(R_R);
MasashiNomura 42:cc8501b824ba 828 subProp[3]->setCoef(IN, 0.00005, 0.0108, 105.15);
MasashiNomura 42:cc8501b824ba 829 subProp[3]->setCoef(OUT,0.00004, 0.0456, 50.611);
MasashiNomura 28:fdb3b144e342 830
takeru0x1103 18:5aa48aec9cae 831 imu = new Imu(p28,p27);
MasashiNomura 47:d3fa874f336e 832 nowTrgtAng = 0.0f;
MasashiNomura 22:24c9c2dedca9 833 ope = new HbUserOpe();
MasashiNomura 22:24c9c2dedca9 834
MasashiNomura 47:d3fa874f336e 835 en_srv_step[0] = 30;
MasashiNomura 47:d3fa874f336e 836 en_srv_step[1] = 30;
MasashiNomura 36:2cc739c7e4cb 837 // //初期化
MasashiNomura 36:2cc739c7e4cb 838 // motorVal[0]=0;
MasashiNomura 36:2cc739c7e4cb 839 // motorVal[1]=0;
MasashiNomura 36:2cc739c7e4cb 840 // motorVal[2]=0;
MasashiNomura 36:2cc739c7e4cb 841 // motorVal[3]=0;
takeru0x1103 17:f9610f3cfa1b 842 }