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@53:b09c062cc31c, 2019-03-04 (annotated)

Committer:
MasashiNomura
Date:
Mon Mar 04 04:03:23 2019 +0000
Revision:
53:b09c062cc31c
Parent:
51:9a994998607b
Child:
55:e245227d9fea
2019/03/04 add engine adjust step

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