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@35:3779201b4c73, 2018-12-25 (annotated)

Committer:
MasashiNomura
Date:
Tue Dec 25 08:20:17 2018 +0000
Revision:
35:3779201b4c73
Parent:
34:234b87f3e6ce
Child:
36:2cc739c7e4cb
12/25 Remove no use commentout, add cchecking motAxl etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 17:f9610f3cfa1b 1 #include "HbManager.h"
takeru0x1103 17:f9610f3cfa1b 2 #include "globalFlags.h"
takeru0x1103 17:f9610f3cfa1b 3 #include "uart.h"
takeru0x1103 18:5aa48aec9cae 4
takeru0x1103 18:5aa48aec9cae 5 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 6 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 7 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 8 void HbManager::getAttitude(){
MasashiNomura 25:f3a6e7eec9c3 9 float tmpf;
MasashiNomura 25:f3a6e7eec9c3 10 INT16 tmpi;
takeru0x1103 18:5aa48aec9cae 11 //姿勢状態取得
takeru0x1103 18:5aa48aec9cae 12 nowAngle = imu->GetYaw();
takeru0x1103 18:5aa48aec9cae 13 nowRate = imu->GetGyroZ();
takeru0x1103 18:5aa48aec9cae 14 //ヨー角キャリブレーション
takeru0x1103 18:5aa48aec9cae 15 if(gf_Cal.bf.yaw==true){
takeru0x1103 18:5aa48aec9cae 16 imu->CalYaw();
takeru0x1103 18:5aa48aec9cae 17 gf_Cal.bf.yaw=false;
takeru0x1103 18:5aa48aec9cae 18 sp.printf("Yaw caribration %f\t",nowAngle);
takeru0x1103 18:5aa48aec9cae 19 nowAngle=0;
takeru0x1103 18:5aa48aec9cae 20 }
takeru0x1103 18:5aa48aec9cae 21 //ジャイロオフセットキャリブレーション
takeru0x1103 18:5aa48aec9cae 22 if(gf_Cal.bf.gy==true){
takeru0x1103 18:5aa48aec9cae 23 imu->CalGyro();
takeru0x1103 18:5aa48aec9cae 24 gf_Cal.bf.gy=false;
takeru0x1103 18:5aa48aec9cae 25 sp.printf("Gyro caribration %d\t",nowRate);
takeru0x1103 18:5aa48aec9cae 26 nowRate=0;
takeru0x1103 17:f9610f3cfa1b 27 }
takeru0x1103 17:f9610f3cfa1b 28
MasashiNomura 25:f3a6e7eec9c3 29 if(gf_Print.bf.yaw) {
MasashiNomura 25:f3a6e7eec9c3 30 tmpf = imu->GetYawRef();
MasashiNomura 25:f3a6e7eec9c3 31 sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf);
MasashiNomura 25:f3a6e7eec9c3 32 }
MasashiNomura 25:f3a6e7eec9c3 33 if(gf_Print.bf.gy) {
MasashiNomura 25:f3a6e7eec9c3 34 tmpi = imu->GetGyroRef();
MasashiNomura 25:f3a6e7eec9c3 35 sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi);
MasashiNomura 25:f3a6e7eec9c3 36 }
takeru0x1103 18:5aa48aec9cae 37 }
MasashiNomura 25:f3a6e7eec9c3 38 void HbManager::calAtt(){
MasashiNomura 25:f3a6e7eec9c3 39 if(att == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 40 if(imu == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 41 imu->CalYaw();
MasashiNomura 25:f3a6e7eec9c3 42 sp.printf("Yaw auto caribration %f\t",nowAngle);
MasashiNomura 25:f3a6e7eec9c3 43 nowAngle=0;
MasashiNomura 25:f3a6e7eec9c3 44 imu->CalGyro();
MasashiNomura 25:f3a6e7eec9c3 45 sp.printf("Gyro auto caribration %d\t\r\n",nowRate);
MasashiNomura 25:f3a6e7eec9c3 46 nowRate=0;
MasashiNomura 25:f3a6e7eec9c3 47 }
MasashiNomura 25:f3a6e7eec9c3 48
takeru0x1103 18:5aa48aec9cae 49 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 50 //姿勢制御
takeru0x1103 18:5aa48aec9cae 51 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 52 void HbManager::controlAttitude(){
takeru0x1103 18:5aa48aec9cae 53 //
takeru0x1103 19:4b0fe9a5ec38 54 float pidRtn;
MasashiNomura 31:56c554c560c1 55 float pidRtn2;
takeru0x1103 19:4b0fe9a5ec38 56
takeru0x1103 21:78302ecdb661 57 // if(gf.state == CHK_ATT){
MasashiNomura 32:7f4145cc3551 58 //pidRtn = att->pid(0,nowAngle,nowRate);
MasashiNomura 32:7f4145cc3551 59 pidRtn = att->pid2(0,nowAngle,nowRate);
takeru0x1103 21:78302ecdb661 60 // }else{
takeru0x1103 21:78302ecdb661 61 // pidRtn =0.0;
takeru0x1103 21:78302ecdb661 62 // }
MasashiNomura 32:7f4145cc3551 63 // pidRtn2 = att->pid2(0,nowAngle,nowRate);
MasashiNomura 31:56c554c560c1 64
takeru0x1103 19:4b0fe9a5ec38 65
takeru0x1103 18:5aa48aec9cae 66 //
takeru0x1103 18:5aa48aec9cae 67 if(gf_Print.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
takeru0x1103 18:5aa48aec9cae 68 if(gf_Print.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
takeru0x1103 18:5aa48aec9cae 69 if(gf_Print.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
takeru0x1103 18:5aa48aec9cae 70 if(gf_Print.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 32:7f4145cc3551 71 if(gf_Print.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
MasashiNomura 31:56c554c560c1 72 if(gf_Print.bf.fb){
MasashiNomura 31:56c554c560c1 73 sp.printf("PID FB,%f,",pidRtn);
MasashiNomura 31:56c554c560c1 74 sp.printf("%f,",pidRtn2);
MasashiNomura 31:56c554c560c1 75 sp.printf("%f,",nowAngle);
MasashiNomura 31:56c554c560c1 76 sp.printf("%f,",nowRate);
MasashiNomura 31:56c554c560c1 77 }
takeru0x1103 18:5aa48aec9cae 78
takeru0x1103 19:4b0fe9a5ec38 79 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 32:7f4145cc3551 80 // float fl = pidRtn * -1;
MasashiNomura 32:7f4145cc3551 81 // float fr = pidRtn;
MasashiNomura 32:7f4145cc3551 82 // float bl = pidRtn;
MasashiNomura 32:7f4145cc3551 83 // float br = pidRtn * -1;
MasashiNomura 32:7f4145cc3551 84 // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
MasashiNomura 32:7f4145cc3551 85 float fl = pidRtn * 2.25 * -1;
MasashiNomura 32:7f4145cc3551 86 float fr = pidRtn * 2.25;
MasashiNomura 32:7f4145cc3551 87 float bl = pidRtn * 2.25;
MasashiNomura 32:7f4145cc3551 88 float br = pidRtn * 2.25 * -1;
MasashiNomura 32:7f4145cc3551 89 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 90 gf_MtReq[i].req = true;
MasashiNomura 32:7f4145cc3551 91 //gf_MtReq[i].req = false;
MasashiNomura 32:7f4145cc3551 92 }
MasashiNomura 32:7f4145cc3551 93 gf_MtReq[0].val = (INT16)fl;
MasashiNomura 32:7f4145cc3551 94 gf_MtReq[1].val = (INT16)fr;
MasashiNomura 32:7f4145cc3551 95 gf_MtReq[2].val = (INT16)bl;
MasashiNomura 32:7f4145cc3551 96 gf_MtReq[3].val = (INT16)br;
takeru0x1103 17:f9610f3cfa1b 97 }
MasashiNomura 24:c5945aaae777 98
MasashiNomura 24:c5945aaae777 99 void HbManager::controlAttitude(float cmdAngle){
MasashiNomura 24:c5945aaae777 100 //
MasashiNomura 24:c5945aaae777 101 float pidRtn;
MasashiNomura 24:c5945aaae777 102
MasashiNomura 24:c5945aaae777 103 pidRtn= att->pid(cmdAngle,nowAngle,nowRate);
MasashiNomura 24:c5945aaae777 104 //
MasashiNomura 24:c5945aaae777 105 if(gf_Print.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 24:c5945aaae777 106 if(gf_Print.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 24:c5945aaae777 107 if(gf_Print.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 24:c5945aaae777 108 if(gf_Print.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 24:c5945aaae777 109 if(gf_Print.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
MasashiNomura 24:c5945aaae777 110
MasashiNomura 24:c5945aaae777 111 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 24:c5945aaae777 112 float fl = pidRtn * -1;
MasashiNomura 24:c5945aaae777 113 float fr = pidRtn;
MasashiNomura 24:c5945aaae777 114 float bl = pidRtn;
MasashiNomura 24:c5945aaae777 115 float br = pidRtn * -1;
MasashiNomura 24:c5945aaae777 116
MasashiNomura 24:c5945aaae777 117 motorVal[0] = (INT16)fl;//前左
MasashiNomura 24:c5945aaae777 118 motorVal[1] = (INT16)fr;//前右
MasashiNomura 24:c5945aaae777 119 motorVal[2] = (INT16)bl;//後左
MasashiNomura 24:c5945aaae777 120 motorVal[3] = (INT16)br;//後右
MasashiNomura 24:c5945aaae777 121 }
MasashiNomura 24:c5945aaae777 122
MasashiNomura 24:c5945aaae777 123 void HbManager::setAttPara(typPidPara para){
MasashiNomura 24:c5945aaae777 124 if(att == NULL) return;
MasashiNomura 24:c5945aaae777 125 att->setPp(para.PP);
MasashiNomura 24:c5945aaae777 126 att->setP(para.P);
MasashiNomura 24:c5945aaae777 127 att->setI(para.I);
MasashiNomura 24:c5945aaae777 128 att->setD(para.D);
MasashiNomura 24:c5945aaae777 129 att->setIMax(para.IMax);
MasashiNomura 24:c5945aaae777 130 att->setIMin(para.IMin);
MasashiNomura 31:56c554c560c1 131 att->setV(para.V);
MasashiNomura 24:c5945aaae777 132 }
MasashiNomura 24:c5945aaae777 133
takeru0x1103 18:5aa48aec9cae 134 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 135 //モーター制御
takeru0x1103 18:5aa48aec9cae 136 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 137 void HbManager::controlMotor(){
MasashiNomura 25:f3a6e7eec9c3 138 INT16 tmp,dif;
MasashiNomura 29:eb3d72dd94aa 139 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 140 if(gf_MtReqOfs[i].req){
MasashiNomura 32:7f4145cc3551 141 tmp = subProp[i]->getValue(OFS);
MasashiNomura 32:7f4145cc3551 142 dif = tmp - (INT16)gf_MtReqOfs[i].val;
MasashiNomura 29:eb3d72dd94aa 143 if(dif == 0){
MasashiNomura 32:7f4145cc3551 144 gf_MtReqOfs[i].req = false;
MasashiNomura 29:eb3d72dd94aa 145 }
MasashiNomura 29:eb3d72dd94aa 146 else {
MasashiNomura 29:eb3d72dd94aa 147 //if(dif > 0){
MasashiNomura 29:eb3d72dd94aa 148 // --tmp;
MasashiNomura 29:eb3d72dd94aa 149 //}
MasashiNomura 29:eb3d72dd94aa 150 //else{
MasashiNomura 29:eb3d72dd94aa 151 // ++tmp;
MasashiNomura 29:eb3d72dd94aa 152 //}
MasashiNomura 29:eb3d72dd94aa 153 if(1000 < dif){
MasashiNomura 29:eb3d72dd94aa 154 tmp-=100;
MasashiNomura 29:eb3d72dd94aa 155 }
MasashiNomura 29:eb3d72dd94aa 156 else if(100 < dif){
MasashiNomura 29:eb3d72dd94aa 157 tmp-= 10;
MasashiNomura 29:eb3d72dd94aa 158 }
MasashiNomura 29:eb3d72dd94aa 159 else if(100 > dif && 10 < dif){
MasashiNomura 29:eb3d72dd94aa 160 tmp-= 5;
MasashiNomura 29:eb3d72dd94aa 161 }
MasashiNomura 29:eb3d72dd94aa 162 else if ( 10 >= dif && 1 <= dif){
MasashiNomura 29:eb3d72dd94aa 163 tmp-=1;
MasashiNomura 29:eb3d72dd94aa 164 }
MasashiNomura 29:eb3d72dd94aa 165 else if ( -1000 > dif){
MasashiNomura 29:eb3d72dd94aa 166 tmp+=100;
MasashiNomura 29:eb3d72dd94aa 167 }
MasashiNomura 29:eb3d72dd94aa 168 else if ( -100 > dif){
MasashiNomura 29:eb3d72dd94aa 169 tmp+=10;
MasashiNomura 29:eb3d72dd94aa 170 }
MasashiNomura 29:eb3d72dd94aa 171 else if(-100 < dif && -10 > dif){
MasashiNomura 29:eb3d72dd94aa 172 tmp+= 5;
MasashiNomura 29:eb3d72dd94aa 173 }
MasashiNomura 29:eb3d72dd94aa 174 else if ( -10 <= dif && -1 >= dif){
MasashiNomura 29:eb3d72dd94aa 175 tmp+=1;
MasashiNomura 29:eb3d72dd94aa 176 }
MasashiNomura 32:7f4145cc3551 177 subProp[i]->setValue(OFS, tmp);
MasashiNomura 29:eb3d72dd94aa 178 }
MasashiNomura 29:eb3d72dd94aa 179 }
MasashiNomura 29:eb3d72dd94aa 180 }
MasashiNomura 28:fdb3b144e342 181
MasashiNomura 32:7f4145cc3551 182 for(int i = 0; i< 4;++i){
MasashiNomura 32:7f4145cc3551 183 if(gf_MtReq[i].req){
MasashiNomura 33:eb260dbfc22a 184 subProp[i]->setValue(ATT,gf_MtReq[i].val);
MasashiNomura 29:eb3d72dd94aa 185 }
MasashiNomura 29:eb3d72dd94aa 186 }
takeru0x1103 19:4b0fe9a5ec38 187
MasashiNomura 32:7f4145cc3551 188 if(gf_Print.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
MasashiNomura 32:7f4145cc3551 189 if(gf_Print.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
MasashiNomura 32:7f4145cc3551 190 if(gf_Print.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }
MasashiNomura 32:7f4145cc3551 191 if(gf_Print.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); }
MasashiNomura 26:732bc37fbefd 192 }
MasashiNomura 26:732bc37fbefd 193
MasashiNomura 32:7f4145cc3551 194 // void HbManager::addMotVal(eMotPos pos, INT16 add){
MasashiNomura 29:eb3d72dd94aa 195 // INT16 tmp;
MasashiNomura 32:7f4145cc3551 196 // if(pos == F_L){
MasashiNomura 29:eb3d72dd94aa 197 // tmp = subProp[0]->getValue();
MasashiNomura 32:7f4145cc3551 198 // subProp[0]->setValue(tmp + add);
MasashiNomura 29:eb3d72dd94aa 199 // }
MasashiNomura 32:7f4145cc3551 200 // else if(pos == F_R){
MasashiNomura 29:eb3d72dd94aa 201 // tmp = subProp[1]->getValue();
MasashiNomura 32:7f4145cc3551 202 // subProp[1]->setValue(tmp + add);
MasashiNomura 29:eb3d72dd94aa 203 // }
MasashiNomura 32:7f4145cc3551 204 // else if(pos == R_L){
MasashiNomura 29:eb3d72dd94aa 205 // tmp = subProp[2]->getValue();
MasashiNomura 32:7f4145cc3551 206 // subProp[2]->setValue(tmp + add);
MasashiNomura 32:7f4145cc3551 207 // }
MasashiNomura 32:7f4145cc3551 208 // else if(pos == R_R){
MasashiNomura 32:7f4145cc3551 209 // tmp = subProp[3]->getValue();
MasashiNomura 32:7f4145cc3551 210 // subProp[3]->setValue(tmp + add);
MasashiNomura 29:eb3d72dd94aa 211 // }
MasashiNomura 32:7f4145cc3551 212 // }
MasashiNomura 32:7f4145cc3551 213
MasashiNomura 32:7f4145cc3551 214 // void HbManager::subMotVal(eMotPos pos, INT16 sub){
MasashiNomura 32:7f4145cc3551 215 // INT16 tmp;
MasashiNomura 32:7f4145cc3551 216 // if(pos == F_L){
MasashiNomura 32:7f4145cc3551 217 // tmp = subProp[0]->getValue();
MasashiNomura 32:7f4145cc3551 218 // subProp[0]->setValue(tmp - sub);
MasashiNomura 32:7f4145cc3551 219 // }
MasashiNomura 32:7f4145cc3551 220 // else if(pos == F_R){
MasashiNomura 32:7f4145cc3551 221 // tmp = subProp[1]->getValue();
MasashiNomura 32:7f4145cc3551 222 // subProp[1]->setValue(tmp - sub);
MasashiNomura 32:7f4145cc3551 223 // }
MasashiNomura 32:7f4145cc3551 224 // else if(pos == R_L){
MasashiNomura 32:7f4145cc3551 225 // tmp = subProp[2]->getValue();
MasashiNomura 32:7f4145cc3551 226 // subProp[2]->setValue(tmp - sub);
MasashiNomura 32:7f4145cc3551 227 // }
MasashiNomura 32:7f4145cc3551 228 // else if(pos == R_R){
MasashiNomura 29:eb3d72dd94aa 229 // tmp = subProp[3]->getValue();
MasashiNomura 32:7f4145cc3551 230 // subProp[3]->setValue(tmp - sub);
MasashiNomura 29:eb3d72dd94aa 231 // }
MasashiNomura 29:eb3d72dd94aa 232 // }
MasashiNomura 29:eb3d72dd94aa 233
MasashiNomura 30:13ada1a24c59 234 INT16 HbManager::getCurMotVal(eMotPos pos){
MasashiNomura 30:13ada1a24c59 235 if(subProp[(int)pos] == NULL) return 0;
MasashiNomura 32:7f4145cc3551 236 return subProp[(int)pos]->getValue(USER);
MasashiNomura 30:13ada1a24c59 237 }
MasashiNomura 30:13ada1a24c59 238 void HbManager::setMotVal(eMotPos pos, INT16 val){
MasashiNomura 30:13ada1a24c59 239 if(subProp[(int)pos] == NULL) return;
MasashiNomura 32:7f4145cc3551 240 subProp[(int)pos]->setValue(USER, val);
MasashiNomura 30:13ada1a24c59 241 }
MasashiNomura 25:f3a6e7eec9c3 242
MasashiNomura 27:ff63c23bc689 243 void HbManager::setMotFPGA(UCHAR num, INT16 val){
MasashiNomura 31:56c554c560c1 244 //UCHAR tmpN = num / 2;
MasashiNomura 31:56c554c560c1 245 //UCHAR tmp = num % 2;
MasashiNomura 28:fdb3b144e342 246 // mot[tmpN]->setValueFPGA(tmp, val);
MasashiNomura 28:fdb3b144e342 247 // sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val);
MasashiNomura 27:ff63c23bc689 248 }
MasashiNomura 27:ff63c23bc689 249
takeru0x1103 17:f9610f3cfa1b 250 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 251 //エンジン制御
takeru0x1103 17:f9610f3cfa1b 252 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 253 void HbManager::controlEngine(){
takeru0x1103 18:5aa48aec9cae 254 UINT16 rpm[2];
takeru0x1103 18:5aa48aec9cae 255 int i;
takeru0x1103 17:f9610f3cfa1b 256
takeru0x1103 18:5aa48aec9cae 257 //エンジン回転数読み出し
takeru0x1103 18:5aa48aec9cae 258 rpm[0] = eng[0]->getRpm();
takeru0x1103 18:5aa48aec9cae 259 rpm[1] = eng[1]->getRpm();
takeru0x1103 17:f9610f3cfa1b 260
takeru0x1103 17:f9610f3cfa1b 261 if(gf_Print.bf.e1){ sp.printf("%d",rpm[0]); }
takeru0x1103 17:f9610f3cfa1b 262 if(gf_Print.bf.e2){ sp.printf("%d,",rpm[1]); }
takeru0x1103 17:f9610f3cfa1b 263
takeru0x1103 18:5aa48aec9cae 264 //アクセル設定
takeru0x1103 18:5aa48aec9cae 265 for(i=0; i<2; i++){
takeru0x1103 18:5aa48aec9cae 266 if(gf_AxReq[0].bf.req==true){
takeru0x1103 18:5aa48aec9cae 267 sp.printf("val=%d(%X)\r\n" , gf_AxReq[0].bf.val , gf_AxReq[0].dt);
takeru0x1103 18:5aa48aec9cae 268 accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 35:3779201b4c73 269 eng[i]->setAccel(accelVal[i]);
takeru0x1103 18:5aa48aec9cae 270 gf_AxReq[i].bf.req=false;
takeru0x1103 18:5aa48aec9cae 271 }
takeru0x1103 17:f9610f3cfa1b 272 }
takeru0x1103 17:f9610f3cfa1b 273
takeru0x1103 17:f9610f3cfa1b 274 }
MasashiNomura 23:79e20be4bc5b 275
MasashiNomura 23:79e20be4bc5b 276 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 277 //ステート遷移関連
MasashiNomura 23:79e20be4bc5b 278 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 279 bool HbManager::chkOverIDLE(){
MasashiNomura 23:79e20be4bc5b 280 return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
MasashiNomura 23:79e20be4bc5b 281 }
MasashiNomura 23:79e20be4bc5b 282
MasashiNomura 23:79e20be4bc5b 283 bool HbManager::chkInRangeIDLE(){
MasashiNomura 23:79e20be4bc5b 284 return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
MasashiNomura 23:79e20be4bc5b 285 }
MasashiNomura 23:79e20be4bc5b 286 // ステート遷移関連end
MasashiNomura 23:79e20be4bc5b 287 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 288
MasashiNomura 23:79e20be4bc5b 289 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 290 //ユーザー操作関連
MasashiNomura 23:79e20be4bc5b 291 //------------------------------------------------------
MasashiNomura 22:24c9c2dedca9 292 void HbManager::getUserCommand(){
MasashiNomura 22:24c9c2dedca9 293 usrSW = ope->GetUserOpe();
MasashiNomura 23:79e20be4bc5b 294 //sp.printf("SW Val=%X\r\n", usrSW.w);
MasashiNomura 22:24c9c2dedca9 295 }
takeru0x1103 17:f9610f3cfa1b 296
MasashiNomura 33:eb260dbfc22a 297 INT16 HbManager::getUserMotAxl(){
MasashiNomura 33:eb260dbfc22a 298 return ope->GetAinAccel();
MasashiNomura 33:eb260dbfc22a 299 }
MasashiNomura 33:eb260dbfc22a 300
MasashiNomura 23:79e20be4bc5b 301 bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
MasashiNomura 23:79e20be4bc5b 302 return ope->ChkCtrlSW(stype);
MasashiNomura 23:79e20be4bc5b 303 }
MasashiNomura 30:13ada1a24c59 304 bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
MasashiNomura 26:732bc37fbefd 305 return ope->ChkCtrlSwRiseEdge(stype);
MasashiNomura 26:732bc37fbefd 306 }
MasashiNomura 23:79e20be4bc5b 307 bool HbManager::chkSWUserOpeAny(){
MasashiNomura 23:79e20be4bc5b 308 return ope->ChkCtrlSwAny();
MasashiNomura 23:79e20be4bc5b 309 }
MasashiNomura 23:79e20be4bc5b 310 typUserSw HbManager::getUserSw(){
MasashiNomura 23:79e20be4bc5b 311 return usrSW;
MasashiNomura 23:79e20be4bc5b 312 }
MasashiNomura 31:56c554c560c1 313
MasashiNomura 31:56c554c560c1 314 void HbManager::chkSW(enmHbState stat){
MasashiNomura 31:56c554c560c1 315
MasashiNomura 31:56c554c560c1 316 INT16 tmpRpm;
MasashiNomura 31:56c554c560c1 317 // Front Left
MasashiNomura 31:56c554c560c1 318 if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 319 gf_MtReqU[0].req=true;
MasashiNomura 31:56c554c560c1 320 tmpRpm = getCurMotVal(F_L);
MasashiNomura 35:3779201b4c73 321 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 322 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 323 }
MasashiNomura 31:56c554c560c1 324 } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 325 if(gf_MtReqU[0].req==true){
MasashiNomura 35:3779201b4c73 326 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 327 }
MasashiNomura 31:56c554c560c1 328 } else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 329 if(gf_MtReqU[0].req){
MasashiNomura 31:56c554c560c1 330 tmpRpm = getCurMotVal(F_L);
MasashiNomura 31:56c554c560c1 331 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 332 gf_MtReqU[0].req = false;
MasashiNomura 31:56c554c560c1 333 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 334 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 335 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 336 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 337 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 338 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 339 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 340 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 341 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 342 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 343 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 344 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 345 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 346 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 347 }
MasashiNomura 31:56c554c560c1 348 setMotVal(F_L, tmpRpm);
MasashiNomura 31:56c554c560c1 349 }
MasashiNomura 31:56c554c560c1 350 }
MasashiNomura 31:56c554c560c1 351
MasashiNomura 31:56c554c560c1 352 if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 353 gf_MtReqU[1].req=true;
MasashiNomura 31:56c554c560c1 354 tmpRpm = getCurMotVal(F_R);
MasashiNomura 35:3779201b4c73 355 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 356 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 357 }
MasashiNomura 31:56c554c560c1 358 } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 359 if(gf_MtReqU[1].req==true){
MasashiNomura 35:3779201b4c73 360 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 361 }
MasashiNomura 31:56c554c560c1 362 } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 363 if(gf_MtReqU[1].req){
MasashiNomura 31:56c554c560c1 364 tmpRpm = getCurMotVal(F_R);
MasashiNomura 31:56c554c560c1 365 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 366 gf_MtReqU[1].req = false;
MasashiNomura 31:56c554c560c1 367 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 368 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 369 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 370 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 371 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 372 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 373 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 374 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 375 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 376 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 377 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 378 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 379 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 380 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 381 }
MasashiNomura 31:56c554c560c1 382 setMotVal(F_R, tmpRpm);
MasashiNomura 31:56c554c560c1 383 }
MasashiNomura 31:56c554c560c1 384 }
MasashiNomura 31:56c554c560c1 385
MasashiNomura 31:56c554c560c1 386 if(chkSWUserOpeRE(HbUserOpe::MOT_STOP)){
MasashiNomura 31:56c554c560c1 387 // EMG STOP だが、テスト用のため、モーターのオフセットを0にして回転を止める
MasashiNomura 31:56c554c560c1 388 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 389 gf_MtReq[i].req = true;
MasashiNomura 32:7f4145cc3551 390 gf_MtReq[i].val = 0;
MasashiNomura 32:7f4145cc3551 391 gf_MtReqOfs[i].req = true;
MasashiNomura 32:7f4145cc3551 392 gf_MtReqOfs[i].val = 0;
MasashiNomura 31:56c554c560c1 393 }
MasashiNomura 31:56c554c560c1 394 }
MasashiNomura 31:56c554c560c1 395
MasashiNomura 34:234b87f3e6ce 396 if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
MasashiNomura 31:56c554c560c1 397 gf_AxReq[0].bf.req = true;
MasashiNomura 31:56c554c560c1 398 gf_AxReq[1].bf.req = true;
MasashiNomura 35:3779201b4c73 399 //gf_AxReq[0].bf.val = MAX_VAL_12BIT;
MasashiNomura 35:3779201b4c73 400 //gf_AxReq[1].bf.val = MAX_VAL_12BIT;
MasashiNomura 34:234b87f3e6ce 401 sp.printf("FLT_ON Push \r\n");
MasashiNomura 34:234b87f3e6ce 402 }
MasashiNomura 34:234b87f3e6ce 403 if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){
MasashiNomura 34:234b87f3e6ce 404 gf_AxReq[0].bf.req = true;
MasashiNomura 34:234b87f3e6ce 405 gf_AxReq[1].bf.req = true;
MasashiNomura 35:3779201b4c73 406 gf_AxReq[0].bf.val = 0;
MasashiNomura 35:3779201b4c73 407 gf_AxReq[1].bf.val = 0;
MasashiNomura 34:234b87f3e6ce 408 sp.printf("FLT_OFF Push \r\n");
MasashiNomura 31:56c554c560c1 409 }
MasashiNomura 31:56c554c560c1 410 if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
MasashiNomura 31:56c554c560c1 411 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 412 gf_MtReq[i].req = true;
MasashiNomura 32:7f4145cc3551 413 gf_MtReq[i].val = 0;
MasashiNomura 32:7f4145cc3551 414 gf_MtReqOfs[i].req = true;
MasashiNomura 32:7f4145cc3551 415 gf_MtReqOfs[i].val = 0;
MasashiNomura 31:56c554c560c1 416 }
MasashiNomura 32:7f4145cc3551 417 }
MasashiNomura 31:56c554c560c1 418 }
MasashiNomura 23:79e20be4bc5b 419 // ユーザー操作関連end
MasashiNomura 23:79e20be4bc5b 420 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 421
takeru0x1103 17:f9610f3cfa1b 422 //======================================================
takeru0x1103 17:f9610f3cfa1b 423 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 424 //======================================================
takeru0x1103 17:f9610f3cfa1b 425 HbManager::HbManager(){
takeru0x1103 18:5aa48aec9cae 426 //メンバクラスインスタンス
takeru0x1103 18:5aa48aec9cae 427 eng[0] = new HbEngine(0);
takeru0x1103 18:5aa48aec9cae 428 eng[1] = new HbEngine(1);
MasashiNomura 32:7f4145cc3551 429 att = new HbAttitude(2.0 , 0 , 0 , 0);//パラメータ outP P I D
MasashiNomura 28:fdb3b144e342 430 // mot[0] = new HbMotor(0);
MasashiNomura 28:fdb3b144e342 431 // mot[1] = new HbMotor(1);
MasashiNomura 28:fdb3b144e342 432 // mot[2] = new HbMotor(2);
MasashiNomura 28:fdb3b144e342 433 // mot[3] = new HbMotor(3);
MasashiNomura 28:fdb3b144e342 434
MasashiNomura 28:fdb3b144e342 435 subProp[0] = new HbSubProp(F_L);
MasashiNomura 28:fdb3b144e342 436 subProp[0]->setCoef(IN, 0.00004, 0.0401, 29.262);
MasashiNomura 28:fdb3b144e342 437 subProp[0]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 28:fdb3b144e342 438 //subProp[0]->makeTbl();
MasashiNomura 22:24c9c2dedca9 439
MasashiNomura 28:fdb3b144e342 440 subProp[1] = new HbSubProp(F_R);
MasashiNomura 28:fdb3b144e342 441 subProp[1]->setCoef(IN, 0.00004, 0.0151, 70.947);
MasashiNomura 28:fdb3b144e342 442 subProp[1]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 28:fdb3b144e342 443 //subProp[1]->makeTbl();
MasashiNomura 28:fdb3b144e342 444
MasashiNomura 28:fdb3b144e342 445 subProp[2] = new HbSubProp(R_L);
MasashiNomura 28:fdb3b144e342 446 subProp[2]->setCoef(IN, 0.00004, 0.0244, 53.954);
MasashiNomura 28:fdb3b144e342 447 subProp[2]->setCoef(OUT,0.00003, 0.1579, -114.22);
MasashiNomura 28:fdb3b144e342 448 //subProp[2]->makeTbl();
MasashiNomura 28:fdb3b144e342 449
MasashiNomura 28:fdb3b144e342 450 subProp[3] = new HbSubProp(R_R);
MasashiNomura 28:fdb3b144e342 451 subProp[3]->setCoef(IN, 0.00004, 0.0111, 82.044);
MasashiNomura 28:fdb3b144e342 452 subProp[3]->setCoef(OUT,0.00003, 0.1617, -123.27);
MasashiNomura 28:fdb3b144e342 453 //subProp[3]->makeTbl();
MasashiNomura 28:fdb3b144e342 454
takeru0x1103 18:5aa48aec9cae 455 imu = new Imu(p28,p27);
takeru0x1103 18:5aa48aec9cae 456
MasashiNomura 22:24c9c2dedca9 457 ope = new HbUserOpe();
MasashiNomura 22:24c9c2dedca9 458
takeru0x1103 18:5aa48aec9cae 459 //初期化
takeru0x1103 17:f9610f3cfa1b 460 motorVal[0]=0;
takeru0x1103 17:f9610f3cfa1b 461 motorVal[1]=0;
takeru0x1103 17:f9610f3cfa1b 462 motorVal[2]=0;
takeru0x1103 17:f9610f3cfa1b 463 motorVal[3]=0;
MasashiNomura 25:f3a6e7eec9c3 464 motorValD[0]=0;
MasashiNomura 25:f3a6e7eec9c3 465 motorValD[1]=0;
MasashiNomura 25:f3a6e7eec9c3 466 motorValD[2]=0;
MasashiNomura 25:f3a6e7eec9c3 467 motorValD[3]=0;
takeru0x1103 17:f9610f3cfa1b 468 }