teamALI / Mbed 2 deprecated HB2018

ジャイロ追加前

Dependencies:   mbed FreeRTOS

HbManager.cpp@41:45c982b1c5b6, 2019-01-21 (annotated)

Committer:
MasashiNomura
Date:
Mon Jan 21 11:57:35 2019 +0000
Revision:
41:45c982b1c5b6
Parent:
40:debe99e228d3
Child:
42:cc8501b824ba
20190121 Modify for 45inc. HbMotor,HbManager,GF 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"
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();
takeru0x1103 18:5aa48aec9cae 20 //ヨー角キャリブレーション
takeru0x1103 18:5aa48aec9cae 21 if(gf_Cal.bf.yaw==true){
takeru0x1103 18:5aa48aec9cae 22 imu->CalYaw();
takeru0x1103 18:5aa48aec9cae 23 gf_Cal.bf.yaw=false;
takeru0x1103 18:5aa48aec9cae 24 sp.printf("Yaw caribration %f\t",nowAngle);
takeru0x1103 18:5aa48aec9cae 25 nowAngle=0;
takeru0x1103 18:5aa48aec9cae 26 }
takeru0x1103 18:5aa48aec9cae 27 //ジャイロオフセットキャリブレーション
takeru0x1103 18:5aa48aec9cae 28 if(gf_Cal.bf.gy==true){
takeru0x1103 18:5aa48aec9cae 29 imu->CalGyro();
takeru0x1103 18:5aa48aec9cae 30 gf_Cal.bf.gy=false;
takeru0x1103 18:5aa48aec9cae 31 sp.printf("Gyro caribration %d\t",nowRate);
takeru0x1103 18:5aa48aec9cae 32 nowRate=0;
takeru0x1103 17:f9610f3cfa1b 33 }
takeru0x1103 17:f9610f3cfa1b 34
MasashiNomura 41:45c982b1c5b6 35 if(gf_Print.d1.bf.yaw) {
MasashiNomura 25:f3a6e7eec9c3 36 tmpf = imu->GetYawRef();
MasashiNomura 25:f3a6e7eec9c3 37 sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf);
MasashiNomura 25:f3a6e7eec9c3 38 }
MasashiNomura 41:45c982b1c5b6 39 if(gf_Print.d1.bf.gy) {
MasashiNomura 25:f3a6e7eec9c3 40 tmpi = imu->GetGyroRef();
MasashiNomura 25:f3a6e7eec9c3 41 sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi);
MasashiNomura 25:f3a6e7eec9c3 42 }
takeru0x1103 18:5aa48aec9cae 43 }
MasashiNomura 25:f3a6e7eec9c3 44 void HbManager::calAtt(){
MasashiNomura 25:f3a6e7eec9c3 45 if(att == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 46 if(imu == NULL) return;
MasashiNomura 25:f3a6e7eec9c3 47 imu->CalYaw();
MasashiNomura 25:f3a6e7eec9c3 48 sp.printf("Yaw auto caribration %f\t",nowAngle);
MasashiNomura 25:f3a6e7eec9c3 49 nowAngle=0;
MasashiNomura 25:f3a6e7eec9c3 50 imu->CalGyro();
MasashiNomura 25:f3a6e7eec9c3 51 sp.printf("Gyro auto caribration %d\t\r\n",nowRate);
MasashiNomura 25:f3a6e7eec9c3 52 nowRate=0;
MasashiNomura 25:f3a6e7eec9c3 53 }
MasashiNomura 25:f3a6e7eec9c3 54
takeru0x1103 18:5aa48aec9cae 55 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 56 //姿勢制御
takeru0x1103 18:5aa48aec9cae 57 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 58 void HbManager::controlAttitude(){
takeru0x1103 19:4b0fe9a5ec38 59
MasashiNomura 36:2cc739c7e4cb 60 float pidRtn;
MasashiNomura 36:2cc739c7e4cb 61
MasashiNomura 36:2cc739c7e4cb 62 pidRtn = att->pid2(0,nowAngle,nowRate);
takeru0x1103 19:4b0fe9a5ec38 63
MasashiNomura 41:45c982b1c5b6 64 if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 41:45c982b1c5b6 65 if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 41:45c982b1c5b6 66 if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 41:45c982b1c5b6 67 if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 41:45c982b1c5b6 68 if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
MasashiNomura 41:45c982b1c5b6 69 if(gf_Print.d1.bf.fb){
MasashiNomura 31:56c554c560c1 70 sp.printf("PID FB,%f,",pidRtn);
MasashiNomura 31:56c554c560c1 71 sp.printf("%f,",nowAngle);
MasashiNomura 31:56c554c560c1 72 sp.printf("%f,",nowRate);
MasashiNomura 31:56c554c560c1 73 }
takeru0x1103 18:5aa48aec9cae 74
takeru0x1103 19:4b0fe9a5ec38 75 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 32:7f4145cc3551 76 // float fl = pidRtn * -1;
MasashiNomura 32:7f4145cc3551 77 // float fr = pidRtn;
MasashiNomura 32:7f4145cc3551 78 // float bl = pidRtn;
MasashiNomura 32:7f4145cc3551 79 // float br = pidRtn * -1;
MasashiNomura 32:7f4145cc3551 80 // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
MasashiNomura 32:7f4145cc3551 81 float fl = pidRtn * 2.25 * -1;
MasashiNomura 32:7f4145cc3551 82 float fr = pidRtn * 2.25;
MasashiNomura 32:7f4145cc3551 83 float bl = pidRtn * 2.25;
MasashiNomura 32:7f4145cc3551 84 float br = pidRtn * 2.25 * -1;
MasashiNomura 32:7f4145cc3551 85 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 86 gf_MtReq[i].req = true;
MasashiNomura 32:7f4145cc3551 87 }
MasashiNomura 32:7f4145cc3551 88 gf_MtReq[0].val = (INT16)fl;
MasashiNomura 32:7f4145cc3551 89 gf_MtReq[1].val = (INT16)fr;
MasashiNomura 32:7f4145cc3551 90 gf_MtReq[2].val = (INT16)bl;
MasashiNomura 32:7f4145cc3551 91 gf_MtReq[3].val = (INT16)br;
takeru0x1103 17:f9610f3cfa1b 92 }
MasashiNomura 24:c5945aaae777 93
MasashiNomura 24:c5945aaae777 94 void HbManager::controlAttitude(float cmdAngle){
MasashiNomura 24:c5945aaae777 95 float pidRtn;
MasashiNomura 24:c5945aaae777 96
MasashiNomura 36:2cc739c7e4cb 97 pidRtn = att->pid2(cmdAngle,nowAngle,nowRate);
MasashiNomura 36:2cc739c7e4cb 98
MasashiNomura 41:45c982b1c5b6 99 if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 41:45c982b1c5b6 100 if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 41:45c982b1c5b6 101 if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 41:45c982b1c5b6 102 if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 41:45c982b1c5b6 103 if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
MasashiNomura 41:45c982b1c5b6 104 if(gf_Print.d1.bf.fb){
MasashiNomura 36:2cc739c7e4cb 105 sp.printf("PID FB,%f,",pidRtn);
MasashiNomura 36:2cc739c7e4cb 106 sp.printf("%f,",nowAngle);
MasashiNomura 36:2cc739c7e4cb 107 sp.printf("%f,",nowRate);
MasashiNomura 36:2cc739c7e4cb 108 }
MasashiNomura 24:c5945aaae777 109
MasashiNomura 24:c5945aaae777 110 //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 36:2cc739c7e4cb 111 // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
MasashiNomura 36:2cc739c7e4cb 112 float fl = pidRtn * 2.25 * -1;
MasashiNomura 36:2cc739c7e4cb 113 float fr = pidRtn * 2.25;
MasashiNomura 36:2cc739c7e4cb 114 float bl = pidRtn * 2.25;
MasashiNomura 36:2cc739c7e4cb 115 float br = pidRtn * 2.25 * -1;
MasashiNomura 36:2cc739c7e4cb 116 for(int i = 0; i < 4; ++i){
MasashiNomura 36:2cc739c7e4cb 117 gf_MtReq[i].req = true;
MasashiNomura 36:2cc739c7e4cb 118 }
MasashiNomura 36:2cc739c7e4cb 119 gf_MtReq[0].val = (INT16)fl;
MasashiNomura 36:2cc739c7e4cb 120 gf_MtReq[1].val = (INT16)fr;
MasashiNomura 36:2cc739c7e4cb 121 gf_MtReq[2].val = (INT16)bl;
MasashiNomura 36:2cc739c7e4cb 122 gf_MtReq[3].val = (INT16)br;
MasashiNomura 36:2cc739c7e4cb 123
MasashiNomura 36:2cc739c7e4cb 124 // float pidRtn;
MasashiNomura 36:2cc739c7e4cb 125
MasashiNomura 36:2cc739c7e4cb 126 // pidRtn= att->pid(cmdAngle,nowAngle,nowRate);
MasashiNomura 36:2cc739c7e4cb 127 // //
MasashiNomura 41:45c982b1c5b6 128 // if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
MasashiNomura 41:45c982b1c5b6 129 // if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
MasashiNomura 41:45c982b1c5b6 130 // if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
MasashiNomura 41:45c982b1c5b6 131 // if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
MasashiNomura 41:45c982b1c5b6 132 // if(gf_Print.d1.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
MasashiNomura 24:c5945aaae777 133
MasashiNomura 36:2cc739c7e4cb 134 // //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
MasashiNomura 36:2cc739c7e4cb 135 // float fl = pidRtn * -1;
MasashiNomura 36:2cc739c7e4cb 136 // float fr = pidRtn;
MasashiNomura 36:2cc739c7e4cb 137 // float bl = pidRtn;
MasashiNomura 36:2cc739c7e4cb 138 // float br = pidRtn * -1;
MasashiNomura 36:2cc739c7e4cb 139
MasashiNomura 36:2cc739c7e4cb 140 // motorVal[0] = (INT16)fl;//前左
MasashiNomura 36:2cc739c7e4cb 141 // motorVal[1] = (INT16)fr;//前右
MasashiNomura 36:2cc739c7e4cb 142 // motorVal[2] = (INT16)bl;//後左
MasashiNomura 36:2cc739c7e4cb 143 // motorVal[3] = (INT16)br;//後右
MasashiNomura 24:c5945aaae777 144 }
MasashiNomura 24:c5945aaae777 145
MasashiNomura 24:c5945aaae777 146 void HbManager::setAttPara(typPidPara para){
MasashiNomura 24:c5945aaae777 147 if(att == NULL) return;
MasashiNomura 24:c5945aaae777 148 att->setPp(para.PP);
MasashiNomura 24:c5945aaae777 149 att->setP(para.P);
MasashiNomura 24:c5945aaae777 150 att->setI(para.I);
MasashiNomura 24:c5945aaae777 151 att->setD(para.D);
MasashiNomura 24:c5945aaae777 152 att->setIMax(para.IMax);
MasashiNomura 24:c5945aaae777 153 att->setIMin(para.IMin);
MasashiNomura 31:56c554c560c1 154 att->setV(para.V);
MasashiNomura 24:c5945aaae777 155 }
MasashiNomura 24:c5945aaae777 156
takeru0x1103 18:5aa48aec9cae 157 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 158 //モーター制御
takeru0x1103 18:5aa48aec9cae 159 //------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 160 void HbManager::controlMotor(){
MasashiNomura 25:f3a6e7eec9c3 161 INT16 tmp,dif;
MasashiNomura 29:eb3d72dd94aa 162 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 163 if(gf_MtReqOfs[i].req){
MasashiNomura 32:7f4145cc3551 164 tmp = subProp[i]->getValue(OFS);
MasashiNomura 32:7f4145cc3551 165 dif = tmp - (INT16)gf_MtReqOfs[i].val;
MasashiNomura 29:eb3d72dd94aa 166 if(dif == 0){
MasashiNomura 40:debe99e228d3 167 subProp[i]->setValue(OFS, (INT16)gf_MtReqOfs[i].val);
MasashiNomura 32:7f4145cc3551 168 gf_MtReqOfs[i].req = false;
MasashiNomura 29:eb3d72dd94aa 169 }
MasashiNomura 29:eb3d72dd94aa 170 else {
MasashiNomura 29:eb3d72dd94aa 171 //if(dif > 0){
MasashiNomura 29:eb3d72dd94aa 172 // --tmp;
MasashiNomura 29:eb3d72dd94aa 173 //}
MasashiNomura 29:eb3d72dd94aa 174 //else{
MasashiNomura 29:eb3d72dd94aa 175 // ++tmp;
MasashiNomura 29:eb3d72dd94aa 176 //}
MasashiNomura 29:eb3d72dd94aa 177 if(1000 < dif){
MasashiNomura 29:eb3d72dd94aa 178 tmp-=100;
MasashiNomura 29:eb3d72dd94aa 179 }
MasashiNomura 29:eb3d72dd94aa 180 else if(100 < dif){
MasashiNomura 29:eb3d72dd94aa 181 tmp-= 10;
MasashiNomura 29:eb3d72dd94aa 182 }
MasashiNomura 40:debe99e228d3 183 else if(100 >= dif && 10 < dif){
MasashiNomura 29:eb3d72dd94aa 184 tmp-= 5;
MasashiNomura 29:eb3d72dd94aa 185 }
MasashiNomura 29:eb3d72dd94aa 186 else if ( 10 >= dif && 1 <= dif){
MasashiNomura 29:eb3d72dd94aa 187 tmp-=1;
MasashiNomura 29:eb3d72dd94aa 188 }
MasashiNomura 29:eb3d72dd94aa 189 else if ( -1000 > dif){
MasashiNomura 29:eb3d72dd94aa 190 tmp+=100;
MasashiNomura 29:eb3d72dd94aa 191 }
MasashiNomura 40:debe99e228d3 192 else if ( -100 >= dif){
MasashiNomura 29:eb3d72dd94aa 193 tmp+=10;
MasashiNomura 29:eb3d72dd94aa 194 }
MasashiNomura 40:debe99e228d3 195 else if(-100 <= dif && -10 > dif){
MasashiNomura 29:eb3d72dd94aa 196 tmp+= 5;
MasashiNomura 29:eb3d72dd94aa 197 }
MasashiNomura 29:eb3d72dd94aa 198 else if ( -10 <= dif && -1 >= dif){
MasashiNomura 29:eb3d72dd94aa 199 tmp+=1;
MasashiNomura 29:eb3d72dd94aa 200 }
MasashiNomura 32:7f4145cc3551 201 subProp[i]->setValue(OFS, tmp);
MasashiNomura 29:eb3d72dd94aa 202 }
MasashiNomura 29:eb3d72dd94aa 203 }
MasashiNomura 29:eb3d72dd94aa 204 }
MasashiNomura 28:fdb3b144e342 205
MasashiNomura 32:7f4145cc3551 206 for(int i = 0; i< 4;++i){
MasashiNomura 32:7f4145cc3551 207 if(gf_MtReq[i].req){
MasashiNomura 33:eb260dbfc22a 208 subProp[i]->setValue(ATT,gf_MtReq[i].val);
MasashiNomura 36:2cc739c7e4cb 209 gf_MtReq[i].req = false;
MasashiNomura 29:eb3d72dd94aa 210 }
MasashiNomura 29:eb3d72dd94aa 211 }
takeru0x1103 19:4b0fe9a5ec38 212
MasashiNomura 41:45c982b1c5b6 213 if(gf_Print.d1.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 214 if(gf_Print.d1.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 215 if(gf_Print.d1.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }
MasashiNomura 41:45c982b1c5b6 216 if(gf_Print.d1.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); }
MasashiNomura 40:debe99e228d3 217
MasashiNomura 40:debe99e228d3 218 //DEBUG用FPGAに直接指示
MasashiNomura 40:debe99e228d3 219 INT16 pos;
MasashiNomura 40:debe99e228d3 220 INT16 typ;
MasashiNomura 40:debe99e228d3 221 if(gf_State == SLEEP){
MasashiNomura 40:debe99e228d3 222 for(int i = 0; i < 8; ++i){
MasashiNomura 40:debe99e228d3 223 pos = i / 2;
MasashiNomura 40:debe99e228d3 224 typ = i % 2;
MasashiNomura 40:debe99e228d3 225 if(gf_MtReqDct[i].req){
MasashiNomura 40:debe99e228d3 226 tmp = subProp[pos]->getValueFpgaMot((eMotType)typ);
MasashiNomura 40:debe99e228d3 227 dif = tmp - (INT16)gf_MtReqDct[i].val;
MasashiNomura 40:debe99e228d3 228 if(dif == 0){
MasashiNomura 40:debe99e228d3 229 subProp[pos]->setValueFpgaMot((eMotType)typ, (INT16)gf_MtReqDct[i].val);
MasashiNomura 40:debe99e228d3 230 gf_MtReqDct[i].req = false;
MasashiNomura 40:debe99e228d3 231 }
MasashiNomura 40:debe99e228d3 232 else {
MasashiNomura 40:debe99e228d3 233 if(1000 < dif){
MasashiNomura 40:debe99e228d3 234 tmp-=100;
MasashiNomura 40:debe99e228d3 235 }
MasashiNomura 40:debe99e228d3 236 else if(100 < dif){
MasashiNomura 40:debe99e228d3 237 tmp-= 10;
MasashiNomura 40:debe99e228d3 238 }
MasashiNomura 40:debe99e228d3 239 else if(100 >= dif && 10 < dif){
MasashiNomura 40:debe99e228d3 240 tmp-= 5;
MasashiNomura 40:debe99e228d3 241 }
MasashiNomura 40:debe99e228d3 242 else if ( 10 >= dif && 1 <= dif){
MasashiNomura 40:debe99e228d3 243 tmp-=1;
MasashiNomura 40:debe99e228d3 244 }
MasashiNomura 40:debe99e228d3 245 else if ( -1000 > dif){
MasashiNomura 40:debe99e228d3 246 tmp+=100;
MasashiNomura 40:debe99e228d3 247 }
MasashiNomura 40:debe99e228d3 248 else if ( -100 >= dif){
MasashiNomura 40:debe99e228d3 249 tmp+=10;
MasashiNomura 40:debe99e228d3 250 }
MasashiNomura 40:debe99e228d3 251 else if(-100 <= dif && -10 > dif){
MasashiNomura 40:debe99e228d3 252 tmp+= 5;
MasashiNomura 40:debe99e228d3 253 }
MasashiNomura 40:debe99e228d3 254 else if ( -10 <= dif && -1 >= dif){
MasashiNomura 40:debe99e228d3 255 tmp+=1;
MasashiNomura 40:debe99e228d3 256 }
MasashiNomura 40:debe99e228d3 257 subProp[pos]->setValueFpgaMot((eMotType)typ, tmp);
MasashiNomura 40:debe99e228d3 258 }
MasashiNomura 40:debe99e228d3 259 }
MasashiNomura 40:debe99e228d3 260 }
MasashiNomura 40:debe99e228d3 261 }
MasashiNomura 26:732bc37fbefd 262 }
MasashiNomura 26:732bc37fbefd 263
MasashiNomura 30:13ada1a24c59 264 INT16 HbManager::getCurMotVal(eMotPos pos){
MasashiNomura 30:13ada1a24c59 265 if(subProp[(int)pos] == NULL) return 0;
MasashiNomura 32:7f4145cc3551 266 return subProp[(int)pos]->getValue(USER);
MasashiNomura 30:13ada1a24c59 267 }
MasashiNomura 30:13ada1a24c59 268 void HbManager::setMotVal(eMotPos pos, INT16 val){
MasashiNomura 30:13ada1a24c59 269 if(subProp[(int)pos] == NULL) return;
MasashiNomura 32:7f4145cc3551 270 subProp[(int)pos]->setValue(USER, val);
MasashiNomura 30:13ada1a24c59 271 }
MasashiNomura 25:f3a6e7eec9c3 272
MasashiNomura 40:debe99e228d3 273 void HbManager::setFpgaMot(eMotPos pos,eMotType type, INT16 val){
MasashiNomura 40:debe99e228d3 274 if(subProp[(int)pos] == NULL) return;
MasashiNomura 40:debe99e228d3 275 subProp[(int)pos]->setValueFpgaMot(type, val);
MasashiNomura 40:debe99e228d3 276 }
MasashiNomura 40:debe99e228d3 277
MasashiNomura 36:2cc739c7e4cb 278 // void HbManager::setMotFPGA(UCHAR num, INT16 val){
MasashiNomura 36:2cc739c7e4cb 279 // //UCHAR tmpN = num / 2;
MasashiNomura 36:2cc739c7e4cb 280 // //UCHAR tmp = num % 2;
MasashiNomura 36:2cc739c7e4cb 281 // // mot[tmpN]->setValueFPGA(tmp, val);
MasashiNomura 36:2cc739c7e4cb 282 // // sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val);
MasashiNomura 36:2cc739c7e4cb 283 // }
MasashiNomura 27:ff63c23bc689 284
takeru0x1103 17:f9610f3cfa1b 285 //------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 286 //エンジン制御
takeru0x1103 17:f9610f3cfa1b 287 //------------------------------------------------------
MasashiNomura 39:1b76f7df8804 288 void HbManager::getEngine(){
takeru0x1103 18:5aa48aec9cae 289 UINT16 rpm[2];
takeru0x1103 18:5aa48aec9cae 290 int i;
takeru0x1103 17:f9610f3cfa1b 291
takeru0x1103 18:5aa48aec9cae 292 //エンジン回転数読み出し
takeru0x1103 18:5aa48aec9cae 293 rpm[0] = eng[0]->getRpm();
takeru0x1103 18:5aa48aec9cae 294 rpm[1] = eng[1]->getRpm();
takeru0x1103 17:f9610f3cfa1b 295
MasashiNomura 41:45c982b1c5b6 296 if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); }
MasashiNomura 41:45c982b1c5b6 297 if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); }
MasashiNomura 39:1b76f7df8804 298 }
MasashiNomura 39:1b76f7df8804 299
MasashiNomura 39:1b76f7df8804 300 void HbManager::controlEngine(){
MasashiNomura 39:1b76f7df8804 301 UINT16 rpm[2];
MasashiNomura 39:1b76f7df8804 302 int i;
MasashiNomura 41:45c982b1c5b6 303 INT16 tmp, dif;
MasashiNomura 39:1b76f7df8804 304 // //エンジン回転数読み出し
MasashiNomura 39:1b76f7df8804 305 // rpm[0] = eng[0]->getRpm();
MasashiNomura 39:1b76f7df8804 306 // rpm[1] = eng[1]->getRpm();
MasashiNomura 39:1b76f7df8804 307
MasashiNomura 41:45c982b1c5b6 308 // if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); }
MasashiNomura 41:45c982b1c5b6 309 // if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); }
takeru0x1103 17:f9610f3cfa1b 310
MasashiNomura 41:45c982b1c5b6 311 INT16 step = 100;
takeru0x1103 18:5aa48aec9cae 312 //アクセル設定
takeru0x1103 18:5aa48aec9cae 313 for(i=0; i<2; i++){
MasashiNomura 38:24ee50452755 314 if(gf_AxReq[i].bf.req==true){
MasashiNomura 41:45c982b1c5b6 315 tmp = eng[i]->getAccel();
MasashiNomura 41:45c982b1c5b6 316 dif = tmp - (INT16)gf_AxReq[i].bf.val;
MasashiNomura 41:45c982b1c5b6 317 if(dif == 0){
MasashiNomura 41:45c982b1c5b6 318 eng[i]->setAccel((INT16)gf_AxReq[i].bf.val);
MasashiNomura 41:45c982b1c5b6 319 gf_AxReq[i].bf.req=false;
MasashiNomura 41:45c982b1c5b6 320 }
MasashiNomura 41:45c982b1c5b6 321 else {
MasashiNomura 41:45c982b1c5b6 322 if(dif > 0){
MasashiNomura 41:45c982b1c5b6 323 if(dif > step){
MasashiNomura 41:45c982b1c5b6 324 tmp -= step;
MasashiNomura 41:45c982b1c5b6 325 }
MasashiNomura 41:45c982b1c5b6 326 else
MasashiNomura 41:45c982b1c5b6 327 {
MasashiNomura 41:45c982b1c5b6 328 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 329 }
MasashiNomura 41:45c982b1c5b6 330 }
MasashiNomura 41:45c982b1c5b6 331 else {
MasashiNomura 41:45c982b1c5b6 332 if( abs(dif) > step){
MasashiNomura 41:45c982b1c5b6 333 tmp += step;
MasashiNomura 41:45c982b1c5b6 334 }
MasashiNomura 41:45c982b1c5b6 335 else
MasashiNomura 41:45c982b1c5b6 336 {
MasashiNomura 41:45c982b1c5b6 337 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 338 }
MasashiNomura 41:45c982b1c5b6 339 }
MasashiNomura 41:45c982b1c5b6 340 eng[i]->setAccel(tmp);
MasashiNomura 41:45c982b1c5b6 341 // if(1000 < dif){
MasashiNomura 41:45c982b1c5b6 342 // tmp-=100;
MasashiNomura 41:45c982b1c5b6 343 // }
MasashiNomura 41:45c982b1c5b6 344 // else if(100 < dif){
MasashiNomura 41:45c982b1c5b6 345 // tmp-= 10;
MasashiNomura 41:45c982b1c5b6 346 // }
MasashiNomura 41:45c982b1c5b6 347 // else if(100 >= dif && 10 < dif){
MasashiNomura 41:45c982b1c5b6 348 // tmp-= 5;
MasashiNomura 41:45c982b1c5b6 349 // }
MasashiNomura 41:45c982b1c5b6 350 // else if ( 10 >= dif && 1 <= dif){
MasashiNomura 41:45c982b1c5b6 351 // tmp-=1;
MasashiNomura 41:45c982b1c5b6 352 // }
MasashiNomura 41:45c982b1c5b6 353 // else if ( -1000 > dif){
MasashiNomura 41:45c982b1c5b6 354 // tmp+=100;
MasashiNomura 41:45c982b1c5b6 355 // }
MasashiNomura 41:45c982b1c5b6 356 // else if ( -100 >= dif){
MasashiNomura 41:45c982b1c5b6 357 // tmp+=10;
MasashiNomura 41:45c982b1c5b6 358 // }
MasashiNomura 41:45c982b1c5b6 359 // else if(-100 <= dif && -10 > dif){
MasashiNomura 41:45c982b1c5b6 360 // tmp+= 5;
MasashiNomura 41:45c982b1c5b6 361 // }
MasashiNomura 41:45c982b1c5b6 362 // else if ( -10 <= dif && -1 >= dif){
MasashiNomura 41:45c982b1c5b6 363 // tmp+=1;
MasashiNomura 41:45c982b1c5b6 364 // }
MasashiNomura 41:45c982b1c5b6 365 // eng[i]->setAccel(tmp);
MasashiNomura 41:45c982b1c5b6 366 }
MasashiNomura 41:45c982b1c5b6 367 //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
MasashiNomura 41:45c982b1c5b6 368 // accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 41:45c982b1c5b6 369 // eng[i]->setAccel(accelVal[i]);
MasashiNomura 41:45c982b1c5b6 370 // gf_AxReq[i].bf.req=false;
takeru0x1103 18:5aa48aec9cae 371 }
takeru0x1103 17:f9610f3cfa1b 372 }
MasashiNomura 36:2cc739c7e4cb 373 }
MasashiNomura 36:2cc739c7e4cb 374 void HbManager::clearHvAxl(){
MasashiNomura 36:2cc739c7e4cb 375 for(int i = 0; i < 2; ++i){
MasashiNomura 36:2cc739c7e4cb 376 eng[i]->clearHoverAccel();
MasashiNomura 36:2cc739c7e4cb 377 }
MasashiNomura 36:2cc739c7e4cb 378 }
MasashiNomura 36:2cc739c7e4cb 379 bool HbManager::chkSetHvAxl(eEgPos pos){
MasashiNomura 36:2cc739c7e4cb 380 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 381 return eng[0]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 382 }
MasashiNomura 36:2cc739c7e4cb 383 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 384 return eng[1]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 385 }
takeru0x1103 17:f9610f3cfa1b 386 }
MasashiNomura 36:2cc739c7e4cb 387 void HbManager::setHvAxl(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 388 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 389 eng[0]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 390 }
MasashiNomura 36:2cc739c7e4cb 391 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 392 eng[1]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 393 }
MasashiNomura 36:2cc739c7e4cb 394 }
MasashiNomura 38:24ee50452755 395 INT16 HbManager::getHvAxl(eEgPos pos){
MasashiNomura 38:24ee50452755 396 if(pos == FRONT){
MasashiNomura 38:24ee50452755 397 return eng[0]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 398 }
MasashiNomura 38:24ee50452755 399 else/* if(pos == REAR)*/{
MasashiNomura 38:24ee50452755 400 return eng[1]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 401 }
MasashiNomura 38:24ee50452755 402 }
MasashiNomura 36:2cc739c7e4cb 403 void HbManager::setAccelVal(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 404 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 405 gf_AxReq[0].bf.req = true;
MasashiNomura 38:24ee50452755 406 gf_AxReq[0].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 407 } else {
MasashiNomura 36:2cc739c7e4cb 408 gf_AxReq[1].bf.req = true;
MasashiNomura 38:24ee50452755 409 gf_AxReq[1].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 410 }
MasashiNomura 36:2cc739c7e4cb 411 }
MasashiNomura 39:1b76f7df8804 412 INT16 HbManager::getAccelVal(eEgPos pos){
MasashiNomura 39:1b76f7df8804 413 if(pos == FRONT){
MasashiNomura 39:1b76f7df8804 414 return eng[0]->getAccel();
MasashiNomura 39:1b76f7df8804 415 }
MasashiNomura 39:1b76f7df8804 416 else/* if(pos == REAR)*/{
MasashiNomura 39:1b76f7df8804 417 return eng[1]->getAccel();
MasashiNomura 39:1b76f7df8804 418 }
MasashiNomura 39:1b76f7df8804 419 }
MasashiNomura 23:79e20be4bc5b 420 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 421 //ステート遷移関連
MasashiNomura 23:79e20be4bc5b 422 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 423 bool HbManager::chkOverIDLE(){
MasashiNomura 23:79e20be4bc5b 424 return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
MasashiNomura 23:79e20be4bc5b 425 }
MasashiNomura 23:79e20be4bc5b 426
MasashiNomura 23:79e20be4bc5b 427 bool HbManager::chkInRangeIDLE(){
MasashiNomura 23:79e20be4bc5b 428 return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
MasashiNomura 23:79e20be4bc5b 429 }
MasashiNomura 23:79e20be4bc5b 430 // ステート遷移関連end
MasashiNomura 23:79e20be4bc5b 431 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 432
MasashiNomura 23:79e20be4bc5b 433 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 434 //ユーザー操作関連
MasashiNomura 23:79e20be4bc5b 435 //------------------------------------------------------
MasashiNomura 22:24c9c2dedca9 436 void HbManager::getUserCommand(){
MasashiNomura 22:24c9c2dedca9 437 usrSW = ope->GetUserOpe();
MasashiNomura 23:79e20be4bc5b 438 //sp.printf("SW Val=%X\r\n", usrSW.w);
MasashiNomura 22:24c9c2dedca9 439 }
takeru0x1103 17:f9610f3cfa1b 440
MasashiNomura 38:24ee50452755 441 float HbManager::getUserMotAxlRaw(){
MasashiNomura 38:24ee50452755 442 return ope->GetAinAccelRaw();
MasashiNomura 38:24ee50452755 443 }
MasashiNomura 38:24ee50452755 444
MasashiNomura 33:eb260dbfc22a 445 INT16 HbManager::getUserMotAxl(){
MasashiNomura 33:eb260dbfc22a 446 return ope->GetAinAccel();
MasashiNomura 33:eb260dbfc22a 447 }
MasashiNomura 33:eb260dbfc22a 448
MasashiNomura 38:24ee50452755 449 float HbManager::getUserEngTrottleRaw(){
MasashiNomura 38:24ee50452755 450 return ope->GetAinThrottleRaw();
MasashiNomura 38:24ee50452755 451 }
MasashiNomura 38:24ee50452755 452
MasashiNomura 36:2cc739c7e4cb 453 INT16 HbManager::getUserEngTrottle(){
MasashiNomura 36:2cc739c7e4cb 454 return ope->GetAinThrottle();
MasashiNomura 36:2cc739c7e4cb 455 }
MasashiNomura 36:2cc739c7e4cb 456
MasashiNomura 23:79e20be4bc5b 457 bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
MasashiNomura 23:79e20be4bc5b 458 return ope->ChkCtrlSW(stype);
MasashiNomura 23:79e20be4bc5b 459 }
MasashiNomura 30:13ada1a24c59 460 bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
MasashiNomura 26:732bc37fbefd 461 return ope->ChkCtrlSwRiseEdge(stype);
MasashiNomura 26:732bc37fbefd 462 }
MasashiNomura 23:79e20be4bc5b 463 bool HbManager::chkSWUserOpeAny(){
MasashiNomura 23:79e20be4bc5b 464 return ope->ChkCtrlSwAny();
MasashiNomura 23:79e20be4bc5b 465 }
MasashiNomura 36:2cc739c7e4cb 466
MasashiNomura 36:2cc739c7e4cb 467 bool HbManager::chkSWUserOpeBoth(HbUserOpe::SW_TYPE stype1,HbUserOpe::SW_TYPE stype2){
MasashiNomura 36:2cc739c7e4cb 468 return ope->ChkCtrlSwBoth(stype1,stype2);
MasashiNomura 36:2cc739c7e4cb 469 }
MasashiNomura 36:2cc739c7e4cb 470
MasashiNomura 23:79e20be4bc5b 471 typUserSw HbManager::getUserSw(){
MasashiNomura 23:79e20be4bc5b 472 return usrSW;
MasashiNomura 23:79e20be4bc5b 473 }
MasashiNomura 31:56c554c560c1 474
MasashiNomura 31:56c554c560c1 475 void HbManager::chkSW(enmHbState stat){
MasashiNomura 31:56c554c560c1 476
MasashiNomura 39:1b76f7df8804 477 INT16 tmpRpm,tmpAxl;
MasashiNomura 31:56c554c560c1 478 // Front Left
MasashiNomura 31:56c554c560c1 479 if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 480 gf_MtReqU[0].req=true;
MasashiNomura 31:56c554c560c1 481 tmpRpm = getCurMotVal(F_L);
MasashiNomura 35:3779201b4c73 482 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 483 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 484 }
MasashiNomura 31:56c554c560c1 485 } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 486 if(gf_MtReqU[0].req==true){
MasashiNomura 35:3779201b4c73 487 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 488 }
MasashiNomura 31:56c554c560c1 489 } else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 490 if(gf_MtReqU[0].req){
MasashiNomura 31:56c554c560c1 491 tmpRpm = getCurMotVal(F_L);
MasashiNomura 31:56c554c560c1 492 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 493 gf_MtReqU[0].req = false;
MasashiNomura 31:56c554c560c1 494 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 495 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 496 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 497 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 498 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 499 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 500 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 501 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 502 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 503 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 504 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 505 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 506 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 507 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 508 }
MasashiNomura 31:56c554c560c1 509 setMotVal(F_L, tmpRpm);
MasashiNomura 31:56c554c560c1 510 }
MasashiNomura 31:56c554c560c1 511 }
MasashiNomura 31:56c554c560c1 512
MasashiNomura 31:56c554c560c1 513 if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 514 gf_MtReqU[1].req=true;
MasashiNomura 31:56c554c560c1 515 tmpRpm = getCurMotVal(F_R);
MasashiNomura 35:3779201b4c73 516 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 517 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 518 }
MasashiNomura 31:56c554c560c1 519 } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 520 if(gf_MtReqU[1].req==true){
MasashiNomura 35:3779201b4c73 521 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 522 }
MasashiNomura 31:56c554c560c1 523 } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 524 if(gf_MtReqU[1].req){
MasashiNomura 31:56c554c560c1 525 tmpRpm = getCurMotVal(F_R);
MasashiNomura 31:56c554c560c1 526 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 527 gf_MtReqU[1].req = false;
MasashiNomura 31:56c554c560c1 528 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 529 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 530 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 531 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 532 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 533 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 534 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 535 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 536 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 537 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 538 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 539 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 540 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 541 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 542 }
MasashiNomura 31:56c554c560c1 543 setMotVal(F_R, tmpRpm);
MasashiNomura 31:56c554c560c1 544 }
MasashiNomura 31:56c554c560c1 545 }
MasashiNomura 31:56c554c560c1 546
MasashiNomura 34:234b87f3e6ce 547 if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
MasashiNomura 39:1b76f7df8804 548 if(stat == IDLE){
MasashiNomura 39:1b76f7df8804 549 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 550 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 551 gf_AxReq[0].bf.val = getHvAxl(FRONT);
MasashiNomura 39:1b76f7df8804 552 gf_AxReq[1].bf.val = getHvAxl(REAR);
MasashiNomura 39:1b76f7df8804 553 setStateF(TAKE_OFF);
MasashiNomura 39:1b76f7df8804 554 }
MasashiNomura 34:234b87f3e6ce 555 sp.printf("FLT_ON Push \r\n");
MasashiNomura 34:234b87f3e6ce 556 }
MasashiNomura 34:234b87f3e6ce 557 if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){
MasashiNomura 39:1b76f7df8804 558 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 39:1b76f7df8804 559 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 560 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 561 gf_AxReq[0].bf.val = 0;
MasashiNomura 39:1b76f7df8804 562 gf_AxReq[1].bf.val = 0;
MasashiNomura 39:1b76f7df8804 563 if(stat == DRIVE){
MasashiNomura 39:1b76f7df8804 564 setStateF(EMGGND);
MasashiNomura 39:1b76f7df8804 565 } else {
MasashiNomura 39:1b76f7df8804 566 setStateF(GROUND);
MasashiNomura 39:1b76f7df8804 567 }
MasashiNomura 39:1b76f7df8804 568 }
MasashiNomura 34:234b87f3e6ce 569 sp.printf("FLT_OFF Push \r\n");
MasashiNomura 31:56c554c560c1 570 }
MasashiNomura 39:1b76f7df8804 571 if(chkSWUserOpe(HbUserOpe::F_ENG_UP)){
MasashiNomura 39:1b76f7df8804 572 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 39:1b76f7df8804 573 tmpAxl = getAccelVal(FRONT);
MasashiNomura 39:1b76f7df8804 574 if(tmpAxl < MAX_VAL_12BIT){
MasashiNomura 39:1b76f7df8804 575 ++tmpAxl;
MasashiNomura 39:1b76f7df8804 576 }
MasashiNomura 39:1b76f7df8804 577 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 578 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 579 }
MasashiNomura 39:1b76f7df8804 580 }
MasashiNomura 39:1b76f7df8804 581 if(chkSWUserOpe(HbUserOpe::F_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 582 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 39:1b76f7df8804 583 tmpAxl = getAccelVal(FRONT);
MasashiNomura 39:1b76f7df8804 584 if(tmpAxl > 0){
MasashiNomura 39:1b76f7df8804 585 --tmpAxl;
MasashiNomura 39:1b76f7df8804 586 }
MasashiNomura 39:1b76f7df8804 587 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 588 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 31:56c554c560c1 589 }
MasashiNomura 39:1b76f7df8804 590 }
MasashiNomura 39:1b76f7df8804 591 if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
MasashiNomura 39:1b76f7df8804 592 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 39:1b76f7df8804 593 tmpAxl = getAccelVal(REAR);
MasashiNomura 39:1b76f7df8804 594 if(tmpAxl < MAX_VAL_12BIT){
MasashiNomura 39:1b76f7df8804 595 ++tmpAxl;
MasashiNomura 39:1b76f7df8804 596 }
MasashiNomura 39:1b76f7df8804 597 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 598 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 36:2cc739c7e4cb 599 }
MasashiNomura 39:1b76f7df8804 600 }
MasashiNomura 39:1b76f7df8804 601 if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 602 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 39:1b76f7df8804 603 tmpAxl = getAccelVal(REAR);
MasashiNomura 39:1b76f7df8804 604 if(tmpAxl > 0){
MasashiNomura 39:1b76f7df8804 605 --tmpAxl;
MasashiNomura 39:1b76f7df8804 606 }
MasashiNomura 39:1b76f7df8804 607 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 608 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 609 }
MasashiNomura 39:1b76f7df8804 610 }
MasashiNomura 39:1b76f7df8804 611 // if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
MasashiNomura 39:1b76f7df8804 612 // // モーター
MasashiNomura 39:1b76f7df8804 613 // for(int i = 0; i < 4; ++i){
MasashiNomura 39:1b76f7df8804 614 // gf_MtReq[i].req = true;
MasashiNomura 39:1b76f7df8804 615 // gf_MtReq[i].val = 0;
MasashiNomura 39:1b76f7df8804 616 // gf_MtReqOfs[i].req = true;
MasashiNomura 39:1b76f7df8804 617 // gf_MtReqOfs[i].val = 0;
MasashiNomura 39:1b76f7df8804 618 // }
MasashiNomura 39:1b76f7df8804 619 // // エンジン用サーボ
MasashiNomura 39:1b76f7df8804 620 // for(int i = 0; i < 2; ++i){
MasashiNomura 39:1b76f7df8804 621 // gf_AxReq[i].bf.req = true;
MasashiNomura 39:1b76f7df8804 622 // gf_AxReq[i].bf.val = 0;
MasashiNomura 39:1b76f7df8804 623 // }
MasashiNomura 39:1b76f7df8804 624 // gf_FromActiveStat = isActiveState();
MasashiNomura 39:1b76f7df8804 625 // setStateF(MOT_STOP);
MasashiNomura 39:1b76f7df8804 626 // }
MasashiNomura 31:56c554c560c1 627 }
MasashiNomura 23:79e20be4bc5b 628 // ユーザー操作関連end
MasashiNomura 23:79e20be4bc5b 629 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 630
takeru0x1103 17:f9610f3cfa1b 631 //======================================================
takeru0x1103 17:f9610f3cfa1b 632 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 633 //======================================================
takeru0x1103 17:f9610f3cfa1b 634 HbManager::HbManager(){
takeru0x1103 18:5aa48aec9cae 635 //メンバクラスインスタンス
takeru0x1103 18:5aa48aec9cae 636 eng[0] = new HbEngine(0);
takeru0x1103 18:5aa48aec9cae 637 eng[1] = new HbEngine(1);
MasashiNomura 32:7f4145cc3551 638 att = new HbAttitude(2.0 , 0 , 0 , 0);//パラメータ outP P I D
MasashiNomura 28:fdb3b144e342 639 // mot[0] = new HbMotor(0);
MasashiNomura 28:fdb3b144e342 640 // mot[1] = new HbMotor(1);
MasashiNomura 28:fdb3b144e342 641 // mot[2] = new HbMotor(2);
MasashiNomura 28:fdb3b144e342 642 // mot[3] = new HbMotor(3);
MasashiNomura 28:fdb3b144e342 643
MasashiNomura 28:fdb3b144e342 644 subProp[0] = new HbSubProp(F_L);
MasashiNomura 28:fdb3b144e342 645 subProp[0]->setCoef(IN, 0.00004, 0.0401, 29.262);
MasashiNomura 28:fdb3b144e342 646 subProp[0]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 22:24c9c2dedca9 647
MasashiNomura 28:fdb3b144e342 648 subProp[1] = new HbSubProp(F_R);
MasashiNomura 28:fdb3b144e342 649 subProp[1]->setCoef(IN, 0.00004, 0.0151, 70.947);
MasashiNomura 28:fdb3b144e342 650 subProp[1]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 28:fdb3b144e342 651
MasashiNomura 28:fdb3b144e342 652 subProp[2] = new HbSubProp(R_L);
MasashiNomura 28:fdb3b144e342 653 subProp[2]->setCoef(IN, 0.00004, 0.0244, 53.954);
MasashiNomura 28:fdb3b144e342 654 subProp[2]->setCoef(OUT,0.00003, 0.1579, -114.22);
MasashiNomura 28:fdb3b144e342 655
MasashiNomura 28:fdb3b144e342 656 subProp[3] = new HbSubProp(R_R);
MasashiNomura 28:fdb3b144e342 657 subProp[3]->setCoef(IN, 0.00004, 0.0111, 82.044);
MasashiNomura 28:fdb3b144e342 658 subProp[3]->setCoef(OUT,0.00003, 0.1617, -123.27);
MasashiNomura 28:fdb3b144e342 659
takeru0x1103 18:5aa48aec9cae 660 imu = new Imu(p28,p27);
takeru0x1103 18:5aa48aec9cae 661
MasashiNomura 22:24c9c2dedca9 662 ope = new HbUserOpe();
MasashiNomura 22:24c9c2dedca9 663
MasashiNomura 36:2cc739c7e4cb 664 // //初期化
MasashiNomura 36:2cc739c7e4cb 665 // motorVal[0]=0;
MasashiNomura 36:2cc739c7e4cb 666 // motorVal[1]=0;
MasashiNomura 36:2cc739c7e4cb 667 // motorVal[2]=0;
MasashiNomura 36:2cc739c7e4cb 668 // motorVal[3]=0;
takeru0x1103 17:f9610f3cfa1b 669 }