teamALI / Mbed 2 deprecated HB2018

ジャイロ追加前

Dependencies:   mbed FreeRTOS

HbManager.cpp@43:156199c2f9b6, 2019-01-24 (annotated)

Committer:
MasashiNomura
Date:
Thu Jan 24 13:21:01 2019 +0000
Revision:
43:156199c2f9b6
Parent:
42:cc8501b824ba
Child:
44:14fe4bd10fdb
20190124 Modify for 45inc.; Add to varialble servo step val , Blink LED when pressing up-down sw in HOVER State.

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 42:cc8501b824ba 298 if(gf_Print.d2.bf.ep1){sp.printf("set %d hover %d ", getAccelVal(FRONT), getHvAxl(FRONT));}
MasashiNomura 42:cc8501b824ba 299 if(gf_Print.d2.bf.ep2){sp.printf("set %d hover %d ", getAccelVal(REAR), getHvAxl(REAR));}
MasashiNomura 42:cc8501b824ba 300
MasashiNomura 42:cc8501b824ba 301 if(gf_AxReqH[0].bf.req){
MasashiNomura 42:cc8501b824ba 302 setHvAxl(FRONT,gf_AxReqH[0].bf.val);
MasashiNomura 42:cc8501b824ba 303 gf_AxReqH[0].bf.req = false;
MasashiNomura 42:cc8501b824ba 304 }
MasashiNomura 42:cc8501b824ba 305 if(gf_AxReqH[1].bf.req){
MasashiNomura 42:cc8501b824ba 306 setHvAxl(REAR,gf_AxReqH[1].bf.val);
MasashiNomura 42:cc8501b824ba 307 gf_AxReqH[1].bf.req = false;
MasashiNomura 42:cc8501b824ba 308 }
MasashiNomura 43:156199c2f9b6 309 for(int i = 0; i < 2; ++i){
MasashiNomura 43:156199c2f9b6 310 if(gf_AxStepReq[i].bf.req){
MasashiNomura 43:156199c2f9b6 311 en_srv_step[i] = gf_AxStepReq[i].bf.val;
MasashiNomura 43:156199c2f9b6 312 gf_AxStepReq[i].bf.req = false;
MasashiNomura 43:156199c2f9b6 313 }
MasashiNomura 43:156199c2f9b6 314 }
MasashiNomura 39:1b76f7df8804 315 }
MasashiNomura 39:1b76f7df8804 316
MasashiNomura 39:1b76f7df8804 317 void HbManager::controlEngine(){
MasashiNomura 39:1b76f7df8804 318 UINT16 rpm[2];
MasashiNomura 39:1b76f7df8804 319 int i;
MasashiNomura 41:45c982b1c5b6 320 INT16 tmp, dif;
MasashiNomura 39:1b76f7df8804 321 // //エンジン回転数読み出し
MasashiNomura 39:1b76f7df8804 322 // rpm[0] = eng[0]->getRpm();
MasashiNomura 39:1b76f7df8804 323 // rpm[1] = eng[1]->getRpm();
MasashiNomura 39:1b76f7df8804 324
MasashiNomura 41:45c982b1c5b6 325 // if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); }
MasashiNomura 41:45c982b1c5b6 326 // if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); }
takeru0x1103 17:f9610f3cfa1b 327
MasashiNomura 41:45c982b1c5b6 328 INT16 step = 100;
takeru0x1103 18:5aa48aec9cae 329 //アクセル設定
takeru0x1103 18:5aa48aec9cae 330 for(i=0; i<2; i++){
MasashiNomura 43:156199c2f9b6 331 step = en_srv_step[i];
MasashiNomura 38:24ee50452755 332 if(gf_AxReq[i].bf.req==true){
MasashiNomura 41:45c982b1c5b6 333 tmp = eng[i]->getAccel();
MasashiNomura 41:45c982b1c5b6 334 dif = tmp - (INT16)gf_AxReq[i].bf.val;
MasashiNomura 41:45c982b1c5b6 335 if(dif == 0){
MasashiNomura 41:45c982b1c5b6 336 eng[i]->setAccel((INT16)gf_AxReq[i].bf.val);
MasashiNomura 41:45c982b1c5b6 337 gf_AxReq[i].bf.req=false;
MasashiNomura 42:cc8501b824ba 338 //sp.printf("Servo Moving FINISH!!\r\n");
MasashiNomura 41:45c982b1c5b6 339 }
MasashiNomura 41:45c982b1c5b6 340 else {
MasashiNomura 41:45c982b1c5b6 341 if(dif > 0){
MasashiNomura 41:45c982b1c5b6 342 if(dif > step){
MasashiNomura 41:45c982b1c5b6 343 tmp -= step;
MasashiNomura 41:45c982b1c5b6 344 }
MasashiNomura 41:45c982b1c5b6 345 else
MasashiNomura 41:45c982b1c5b6 346 {
MasashiNomura 41:45c982b1c5b6 347 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 348 }
MasashiNomura 41:45c982b1c5b6 349 }
MasashiNomura 41:45c982b1c5b6 350 else {
MasashiNomura 41:45c982b1c5b6 351 if( abs(dif) > step){
MasashiNomura 41:45c982b1c5b6 352 tmp += step;
MasashiNomura 41:45c982b1c5b6 353 }
MasashiNomura 41:45c982b1c5b6 354 else
MasashiNomura 41:45c982b1c5b6 355 {
MasashiNomura 41:45c982b1c5b6 356 tmp -= dif;
MasashiNomura 41:45c982b1c5b6 357 }
MasashiNomura 41:45c982b1c5b6 358 }
MasashiNomura 41:45c982b1c5b6 359 eng[i]->setAccel(tmp);
MasashiNomura 41:45c982b1c5b6 360 // if(1000 < dif){
MasashiNomura 41:45c982b1c5b6 361 // tmp-=100;
MasashiNomura 41:45c982b1c5b6 362 // }
MasashiNomura 41:45c982b1c5b6 363 // else if(100 < dif){
MasashiNomura 41:45c982b1c5b6 364 // tmp-= 10;
MasashiNomura 41:45c982b1c5b6 365 // }
MasashiNomura 41:45c982b1c5b6 366 // else if(100 >= dif && 10 < dif){
MasashiNomura 41:45c982b1c5b6 367 // tmp-= 5;
MasashiNomura 41:45c982b1c5b6 368 // }
MasashiNomura 41:45c982b1c5b6 369 // else if ( 10 >= dif && 1 <= dif){
MasashiNomura 41:45c982b1c5b6 370 // tmp-=1;
MasashiNomura 41:45c982b1c5b6 371 // }
MasashiNomura 41:45c982b1c5b6 372 // else if ( -1000 > dif){
MasashiNomura 41:45c982b1c5b6 373 // tmp+=100;
MasashiNomura 41:45c982b1c5b6 374 // }
MasashiNomura 41:45c982b1c5b6 375 // else if ( -100 >= dif){
MasashiNomura 41:45c982b1c5b6 376 // tmp+=10;
MasashiNomura 41:45c982b1c5b6 377 // }
MasashiNomura 41:45c982b1c5b6 378 // else if(-100 <= dif && -10 > dif){
MasashiNomura 41:45c982b1c5b6 379 // tmp+= 5;
MasashiNomura 41:45c982b1c5b6 380 // }
MasashiNomura 41:45c982b1c5b6 381 // else if ( -10 <= dif && -1 >= dif){
MasashiNomura 41:45c982b1c5b6 382 // tmp+=1;
MasashiNomura 41:45c982b1c5b6 383 // }
MasashiNomura 41:45c982b1c5b6 384 // eng[i]->setAccel(tmp);
MasashiNomura 41:45c982b1c5b6 385 }
MasashiNomura 41:45c982b1c5b6 386 //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
MasashiNomura 41:45c982b1c5b6 387 // accelVal[i] = gf_AxReq[i].bf.val;
MasashiNomura 41:45c982b1c5b6 388 // eng[i]->setAccel(accelVal[i]);
MasashiNomura 41:45c982b1c5b6 389 // gf_AxReq[i].bf.req=false;
takeru0x1103 18:5aa48aec9cae 390 }
takeru0x1103 17:f9610f3cfa1b 391 }
MasashiNomura 36:2cc739c7e4cb 392 }
MasashiNomura 36:2cc739c7e4cb 393 void HbManager::clearHvAxl(){
MasashiNomura 36:2cc739c7e4cb 394 for(int i = 0; i < 2; ++i){
MasashiNomura 36:2cc739c7e4cb 395 eng[i]->clearHoverAccel();
MasashiNomura 36:2cc739c7e4cb 396 }
MasashiNomura 36:2cc739c7e4cb 397 }
MasashiNomura 36:2cc739c7e4cb 398 bool HbManager::chkSetHvAxl(eEgPos pos){
MasashiNomura 36:2cc739c7e4cb 399 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 400 return eng[0]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 401 }
MasashiNomura 36:2cc739c7e4cb 402 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 403 return eng[1]->chkSetHoverAccel();
MasashiNomura 36:2cc739c7e4cb 404 }
takeru0x1103 17:f9610f3cfa1b 405 }
MasashiNomura 36:2cc739c7e4cb 406 void HbManager::setHvAxl(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 407 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 408 eng[0]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 409 }
MasashiNomura 36:2cc739c7e4cb 410 else/* if(pos == REAR)*/{
MasashiNomura 36:2cc739c7e4cb 411 eng[1]->setHoverAccel(val);
MasashiNomura 36:2cc739c7e4cb 412 }
MasashiNomura 36:2cc739c7e4cb 413 }
MasashiNomura 38:24ee50452755 414 INT16 HbManager::getHvAxl(eEgPos pos){
MasashiNomura 38:24ee50452755 415 if(pos == FRONT){
MasashiNomura 38:24ee50452755 416 return eng[0]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 417 }
MasashiNomura 38:24ee50452755 418 else/* if(pos == REAR)*/{
MasashiNomura 38:24ee50452755 419 return eng[1]->getHoverAccelVal();
MasashiNomura 38:24ee50452755 420 }
MasashiNomura 38:24ee50452755 421 }
MasashiNomura 36:2cc739c7e4cb 422 void HbManager::setAccelVal(eEgPos pos, INT16 val){
MasashiNomura 36:2cc739c7e4cb 423 if(pos == FRONT){
MasashiNomura 36:2cc739c7e4cb 424 gf_AxReq[0].bf.req = true;
MasashiNomura 38:24ee50452755 425 gf_AxReq[0].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 426 } else {
MasashiNomura 36:2cc739c7e4cb 427 gf_AxReq[1].bf.req = true;
MasashiNomura 38:24ee50452755 428 gf_AxReq[1].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
MasashiNomura 36:2cc739c7e4cb 429 }
MasashiNomura 36:2cc739c7e4cb 430 }
MasashiNomura 39:1b76f7df8804 431 INT16 HbManager::getAccelVal(eEgPos pos){
MasashiNomura 39:1b76f7df8804 432 if(pos == FRONT){
MasashiNomura 39:1b76f7df8804 433 return eng[0]->getAccel();
MasashiNomura 39:1b76f7df8804 434 }
MasashiNomura 39:1b76f7df8804 435 else/* if(pos == REAR)*/{
MasashiNomura 39:1b76f7df8804 436 return eng[1]->getAccel();
MasashiNomura 39:1b76f7df8804 437 }
MasashiNomura 39:1b76f7df8804 438 }
MasashiNomura 23:79e20be4bc5b 439 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 440 //ステート遷移関連
MasashiNomura 23:79e20be4bc5b 441 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 442 bool HbManager::chkOverIDLE(){
MasashiNomura 23:79e20be4bc5b 443 return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
MasashiNomura 23:79e20be4bc5b 444 }
MasashiNomura 23:79e20be4bc5b 445
MasashiNomura 23:79e20be4bc5b 446 bool HbManager::chkInRangeIDLE(){
MasashiNomura 23:79e20be4bc5b 447 return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
MasashiNomura 23:79e20be4bc5b 448 }
MasashiNomura 23:79e20be4bc5b 449 // ステート遷移関連end
MasashiNomura 23:79e20be4bc5b 450 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 451
MasashiNomura 23:79e20be4bc5b 452 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 453 //ユーザー操作関連
MasashiNomura 23:79e20be4bc5b 454 //------------------------------------------------------
MasashiNomura 22:24c9c2dedca9 455 void HbManager::getUserCommand(){
MasashiNomura 22:24c9c2dedca9 456 usrSW = ope->GetUserOpe();
MasashiNomura 23:79e20be4bc5b 457 //sp.printf("SW Val=%X\r\n", usrSW.w);
MasashiNomura 22:24c9c2dedca9 458 }
takeru0x1103 17:f9610f3cfa1b 459
MasashiNomura 38:24ee50452755 460 float HbManager::getUserMotAxlRaw(){
MasashiNomura 38:24ee50452755 461 return ope->GetAinAccelRaw();
MasashiNomura 38:24ee50452755 462 }
MasashiNomura 38:24ee50452755 463
MasashiNomura 33:eb260dbfc22a 464 INT16 HbManager::getUserMotAxl(){
MasashiNomura 33:eb260dbfc22a 465 return ope->GetAinAccel();
MasashiNomura 33:eb260dbfc22a 466 }
MasashiNomura 33:eb260dbfc22a 467
MasashiNomura 38:24ee50452755 468 float HbManager::getUserEngTrottleRaw(){
MasashiNomura 38:24ee50452755 469 return ope->GetAinThrottleRaw();
MasashiNomura 38:24ee50452755 470 }
MasashiNomura 38:24ee50452755 471
MasashiNomura 36:2cc739c7e4cb 472 INT16 HbManager::getUserEngTrottle(){
MasashiNomura 36:2cc739c7e4cb 473 return ope->GetAinThrottle();
MasashiNomura 36:2cc739c7e4cb 474 }
MasashiNomura 36:2cc739c7e4cb 475
MasashiNomura 23:79e20be4bc5b 476 bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
MasashiNomura 23:79e20be4bc5b 477 return ope->ChkCtrlSW(stype);
MasashiNomura 23:79e20be4bc5b 478 }
MasashiNomura 30:13ada1a24c59 479 bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
MasashiNomura 26:732bc37fbefd 480 return ope->ChkCtrlSwRiseEdge(stype);
MasashiNomura 26:732bc37fbefd 481 }
MasashiNomura 23:79e20be4bc5b 482 bool HbManager::chkSWUserOpeAny(){
MasashiNomura 23:79e20be4bc5b 483 return ope->ChkCtrlSwAny();
MasashiNomura 23:79e20be4bc5b 484 }
MasashiNomura 36:2cc739c7e4cb 485
MasashiNomura 36:2cc739c7e4cb 486 bool HbManager::chkSWUserOpeBoth(HbUserOpe::SW_TYPE stype1,HbUserOpe::SW_TYPE stype2){
MasashiNomura 36:2cc739c7e4cb 487 return ope->ChkCtrlSwBoth(stype1,stype2);
MasashiNomura 36:2cc739c7e4cb 488 }
MasashiNomura 36:2cc739c7e4cb 489
MasashiNomura 23:79e20be4bc5b 490 typUserSw HbManager::getUserSw(){
MasashiNomura 23:79e20be4bc5b 491 return usrSW;
MasashiNomura 23:79e20be4bc5b 492 }
MasashiNomura 31:56c554c560c1 493
MasashiNomura 31:56c554c560c1 494 void HbManager::chkSW(enmHbState stat){
MasashiNomura 31:56c554c560c1 495
MasashiNomura 39:1b76f7df8804 496 INT16 tmpRpm,tmpAxl;
MasashiNomura 31:56c554c560c1 497 // Front Left
MasashiNomura 31:56c554c560c1 498 if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 499 gf_MtReqU[0].req=true;
MasashiNomura 31:56c554c560c1 500 tmpRpm = getCurMotVal(F_L);
MasashiNomura 35:3779201b4c73 501 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 502 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 503 }
MasashiNomura 31:56c554c560c1 504 } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 505 if(gf_MtReqU[0].req==true){
MasashiNomura 35:3779201b4c73 506 setMotVal(F_L, BRK_RPM);
MasashiNomura 31:56c554c560c1 507 }
MasashiNomura 31:56c554c560c1 508 } else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 32:7f4145cc3551 509 if(gf_MtReqU[0].req){
MasashiNomura 31:56c554c560c1 510 tmpRpm = getCurMotVal(F_L);
MasashiNomura 31:56c554c560c1 511 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 512 gf_MtReqU[0].req = false;
MasashiNomura 31:56c554c560c1 513 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 514 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 515 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 516 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 517 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 518 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 519 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 520 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 521 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 522 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 523 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 524 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 525 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 526 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 527 }
MasashiNomura 31:56c554c560c1 528 setMotVal(F_L, tmpRpm);
MasashiNomura 31:56c554c560c1 529 }
MasashiNomura 31:56c554c560c1 530 }
MasashiNomura 31:56c554c560c1 531
MasashiNomura 31:56c554c560c1 532 if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 533 gf_MtReqU[1].req=true;
MasashiNomura 31:56c554c560c1 534 tmpRpm = getCurMotVal(F_R);
MasashiNomura 35:3779201b4c73 535 if(tmpRpm < BRK_RPM){
MasashiNomura 35:3779201b4c73 536 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 537 }
MasashiNomura 31:56c554c560c1 538 } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 539 if(gf_MtReqU[1].req==true){
MasashiNomura 35:3779201b4c73 540 setMotVal(F_R, BRK_RPM);
MasashiNomura 31:56c554c560c1 541 }
MasashiNomura 31:56c554c560c1 542 } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 32:7f4145cc3551 543 if(gf_MtReqU[1].req){
MasashiNomura 31:56c554c560c1 544 tmpRpm = getCurMotVal(F_R);
MasashiNomura 31:56c554c560c1 545 if(tmpRpm == 0){
MasashiNomura 32:7f4145cc3551 546 gf_MtReqU[1].req = false;
MasashiNomura 31:56c554c560c1 547 }else if(tmpRpm > 1000){
MasashiNomura 31:56c554c560c1 548 tmpRpm-=100;
MasashiNomura 31:56c554c560c1 549 } else if(tmpRpm >= 100){
MasashiNomura 31:56c554c560c1 550 tmpRpm-=50;
MasashiNomura 31:56c554c560c1 551 } else if(tmpRpm > 0){
MasashiNomura 31:56c554c560c1 552 tmpRpm-=10;
MasashiNomura 31:56c554c560c1 553 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 554 } else if(tmpRpm < -1000){
MasashiNomura 31:56c554c560c1 555 tmpRpm+=100;
MasashiNomura 31:56c554c560c1 556 } else if(tmpRpm <= -100){
MasashiNomura 31:56c554c560c1 557 tmpRpm+=50;
MasashiNomura 31:56c554c560c1 558 }else if(tmpRpm < 0){
MasashiNomura 31:56c554c560c1 559 tmpRpm+=10;
MasashiNomura 31:56c554c560c1 560 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 31:56c554c560c1 561 }
MasashiNomura 31:56c554c560c1 562 setMotVal(F_R, tmpRpm);
MasashiNomura 31:56c554c560c1 563 }
MasashiNomura 31:56c554c560c1 564 }
MasashiNomura 31:56c554c560c1 565
MasashiNomura 34:234b87f3e6ce 566 if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
MasashiNomura 39:1b76f7df8804 567 if(stat == IDLE){
MasashiNomura 39:1b76f7df8804 568 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 569 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 570 gf_AxReq[0].bf.val = getHvAxl(FRONT);
MasashiNomura 39:1b76f7df8804 571 gf_AxReq[1].bf.val = getHvAxl(REAR);
MasashiNomura 39:1b76f7df8804 572 setStateF(TAKE_OFF);
MasashiNomura 39:1b76f7df8804 573 }
MasashiNomura 34:234b87f3e6ce 574 sp.printf("FLT_ON Push \r\n");
MasashiNomura 34:234b87f3e6ce 575 }
MasashiNomura 34:234b87f3e6ce 576 if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){
MasashiNomura 39:1b76f7df8804 577 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 42:cc8501b824ba 578 for(int i = 0; i < 2; ++i){
MasashiNomura 42:cc8501b824ba 579 setHvAxl((eEgPos)i, gf_AxReq[i].bf.val);
MasashiNomura 42:cc8501b824ba 580 gf_AxReq[i].bf.req = true;
MasashiNomura 42:cc8501b824ba 581 gf_AxReq[i].bf.val = 0;
MasashiNomura 42:cc8501b824ba 582 }
MasashiNomura 42:cc8501b824ba 583 // gf_AxReq[0].bf.req = true;
MasashiNomura 42:cc8501b824ba 584 // gf_AxReq[1].bf.req = true;
MasashiNomura 42:cc8501b824ba 585 // gf_AxReq[0].bf.val = 0;
MasashiNomura 42:cc8501b824ba 586 // gf_AxReq[1].bf.val = 0;
MasashiNomura 39:1b76f7df8804 587 if(stat == DRIVE){
MasashiNomura 39:1b76f7df8804 588 setStateF(EMGGND);
MasashiNomura 39:1b76f7df8804 589 } else {
MasashiNomura 39:1b76f7df8804 590 setStateF(GROUND);
MasashiNomura 39:1b76f7df8804 591 }
MasashiNomura 39:1b76f7df8804 592 }
MasashiNomura 34:234b87f3e6ce 593 sp.printf("FLT_OFF Push \r\n");
MasashiNomura 31:56c554c560c1 594 }
MasashiNomura 39:1b76f7df8804 595 if(chkSWUserOpe(HbUserOpe::F_ENG_UP)){
MasashiNomura 39:1b76f7df8804 596 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 43:156199c2f9b6 597 gf_BlinkLED = true;
MasashiNomura 42:cc8501b824ba 598 INT16 step = 3;
MasashiNomura 39:1b76f7df8804 599 tmpAxl = getAccelVal(FRONT);
MasashiNomura 42:cc8501b824ba 600 tmpAxl+=step;
MasashiNomura 42:cc8501b824ba 601 if(tmpAxl > MAX_VAL_12BIT){
MasashiNomura 42:cc8501b824ba 602 tmpAxl = MAX_VAL_12BIT;
MasashiNomura 39:1b76f7df8804 603 }
MasashiNomura 39:1b76f7df8804 604 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 605 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 606 }
MasashiNomura 39:1b76f7df8804 607 }
MasashiNomura 39:1b76f7df8804 608 if(chkSWUserOpe(HbUserOpe::F_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 609 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 43:156199c2f9b6 610 gf_BlinkLED = true;
MasashiNomura 42:cc8501b824ba 611 INT16 step = 3;
MasashiNomura 39:1b76f7df8804 612 tmpAxl = getAccelVal(FRONT);
MasashiNomura 42:cc8501b824ba 613 tmpAxl-=step;
MasashiNomura 42:cc8501b824ba 614 if(tmpAxl < 0){
MasashiNomura 42:cc8501b824ba 615 tmpAxl = 0;
MasashiNomura 39:1b76f7df8804 616 }
MasashiNomura 39:1b76f7df8804 617 gf_AxReq[0].bf.req = true;
MasashiNomura 39:1b76f7df8804 618 gf_AxReq[0].bf.val = tmpAxl;
MasashiNomura 31:56c554c560c1 619 }
MasashiNomura 39:1b76f7df8804 620 }
MasashiNomura 39:1b76f7df8804 621 if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
MasashiNomura 39:1b76f7df8804 622 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 42:cc8501b824ba 623 INT16 step = 3;
MasashiNomura 43:156199c2f9b6 624 gf_BlinkLED = true;
MasashiNomura 39:1b76f7df8804 625 tmpAxl = getAccelVal(REAR);
MasashiNomura 42:cc8501b824ba 626 tmpAxl+=step;
MasashiNomura 42:cc8501b824ba 627 if(tmpAxl > MAX_VAL_12BIT){
MasashiNomura 42:cc8501b824ba 628 tmpAxl = MAX_VAL_12BIT;
MasashiNomura 39:1b76f7df8804 629 }
MasashiNomura 39:1b76f7df8804 630 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 631 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 36:2cc739c7e4cb 632 }
MasashiNomura 39:1b76f7df8804 633 }
MasashiNomura 39:1b76f7df8804 634 if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
MasashiNomura 39:1b76f7df8804 635 if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
MasashiNomura 42:cc8501b824ba 636 INT16 step = 3;
MasashiNomura 43:156199c2f9b6 637 gf_BlinkLED = true;
MasashiNomura 39:1b76f7df8804 638 tmpAxl = getAccelVal(REAR);
MasashiNomura 42:cc8501b824ba 639 tmpAxl-=step;
MasashiNomura 42:cc8501b824ba 640 if(tmpAxl < 0){
MasashiNomura 42:cc8501b824ba 641 tmpAxl = 0;
MasashiNomura 39:1b76f7df8804 642 }
MasashiNomura 39:1b76f7df8804 643 gf_AxReq[1].bf.req = true;
MasashiNomura 39:1b76f7df8804 644 gf_AxReq[1].bf.val = tmpAxl;
MasashiNomura 39:1b76f7df8804 645 }
MasashiNomura 39:1b76f7df8804 646 }
MasashiNomura 39:1b76f7df8804 647 // if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
MasashiNomura 39:1b76f7df8804 648 // // モーター
MasashiNomura 39:1b76f7df8804 649 // for(int i = 0; i < 4; ++i){
MasashiNomura 39:1b76f7df8804 650 // gf_MtReq[i].req = true;
MasashiNomura 39:1b76f7df8804 651 // gf_MtReq[i].val = 0;
MasashiNomura 39:1b76f7df8804 652 // gf_MtReqOfs[i].req = true;
MasashiNomura 39:1b76f7df8804 653 // gf_MtReqOfs[i].val = 0;
MasashiNomura 39:1b76f7df8804 654 // }
MasashiNomura 39:1b76f7df8804 655 // // エンジン用サーボ
MasashiNomura 39:1b76f7df8804 656 // for(int i = 0; i < 2; ++i){
MasashiNomura 39:1b76f7df8804 657 // gf_AxReq[i].bf.req = true;
MasashiNomura 39:1b76f7df8804 658 // gf_AxReq[i].bf.val = 0;
MasashiNomura 39:1b76f7df8804 659 // }
MasashiNomura 39:1b76f7df8804 660 // gf_FromActiveStat = isActiveState();
MasashiNomura 39:1b76f7df8804 661 // setStateF(MOT_STOP);
MasashiNomura 39:1b76f7df8804 662 // }
MasashiNomura 31:56c554c560c1 663 }
MasashiNomura 23:79e20be4bc5b 664 // ユーザー操作関連end
MasashiNomura 23:79e20be4bc5b 665 //------------------------------------------------------
MasashiNomura 23:79e20be4bc5b 666
takeru0x1103 17:f9610f3cfa1b 667 //======================================================
takeru0x1103 17:f9610f3cfa1b 668 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 669 //======================================================
takeru0x1103 17:f9610f3cfa1b 670 HbManager::HbManager(){
takeru0x1103 18:5aa48aec9cae 671 //メンバクラスインスタンス
takeru0x1103 18:5aa48aec9cae 672 eng[0] = new HbEngine(0);
takeru0x1103 18:5aa48aec9cae 673 eng[1] = new HbEngine(1);
MasashiNomura 42:cc8501b824ba 674 eng[0]->setHoverAccel(1500);
MasashiNomura 42:cc8501b824ba 675 eng[1]->setHoverAccel(1500);
MasashiNomura 32:7f4145cc3551 676 att = new HbAttitude(2.0 , 0 , 0 , 0);//パラメータ outP P I D
MasashiNomura 28:fdb3b144e342 677 // mot[0] = new HbMotor(0);
MasashiNomura 28:fdb3b144e342 678 // mot[1] = new HbMotor(1);
MasashiNomura 28:fdb3b144e342 679 // mot[2] = new HbMotor(2);
MasashiNomura 28:fdb3b144e342 680 // mot[3] = new HbMotor(3);
MasashiNomura 28:fdb3b144e342 681
MasashiNomura 42:cc8501b824ba 682 // subProp[0] = new HbSubProp(F_L);
MasashiNomura 42:cc8501b824ba 683 // subProp[0]->setCoef(IN, 0.00004, 0.0401, 29.262);
MasashiNomura 42:cc8501b824ba 684 // subProp[0]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 42:cc8501b824ba 685
MasashiNomura 42:cc8501b824ba 686 // subProp[1] = new HbSubProp(F_R);
MasashiNomura 42:cc8501b824ba 687 // subProp[1]->setCoef(IN, 0.00004, 0.0151, 70.947);
MasashiNomura 42:cc8501b824ba 688 // subProp[1]->setCoef(OUT,0.00004, 0.1336, -69.184);
MasashiNomura 42:cc8501b824ba 689
MasashiNomura 42:cc8501b824ba 690 // subProp[2] = new HbSubProp(R_L);
MasashiNomura 42:cc8501b824ba 691 // subProp[2]->setCoef(IN, 0.00004, 0.0244, 53.954);
MasashiNomura 42:cc8501b824ba 692 // subProp[2]->setCoef(OUT,0.00003, 0.1579, -114.22);
MasashiNomura 42:cc8501b824ba 693
MasashiNomura 42:cc8501b824ba 694 // subProp[3] = new HbSubProp(R_R);
MasashiNomura 42:cc8501b824ba 695 // subProp[3]->setCoef(IN, 0.00004, 0.0111, 82.044);
MasashiNomura 42:cc8501b824ba 696 // subProp[3]->setCoef(OUT,0.00003, 0.1617, -123.27);
MasashiNomura 42:cc8501b824ba 697
MasashiNomura 28:fdb3b144e342 698 subProp[0] = new HbSubProp(F_L);
MasashiNomura 42:cc8501b824ba 699 subProp[0]->setCoef(IN, 0.00004, 0.0786, -5.248);
MasashiNomura 42:cc8501b824ba 700 subProp[0]->setCoef(OUT,0.00005, 0.0112, 104.45);
MasashiNomura 22:24c9c2dedca9 701
MasashiNomura 28:fdb3b144e342 702 subProp[1] = new HbSubProp(F_R);
MasashiNomura 42:cc8501b824ba 703 subProp[1]->setCoef(IN, 0.00005, 0.0095, 126.11);
MasashiNomura 42:cc8501b824ba 704 subProp[1]->setCoef(OUT,0.00004, 0.0798, -2.1167);
MasashiNomura 28:fdb3b144e342 705
MasashiNomura 28:fdb3b144e342 706 subProp[2] = new HbSubProp(R_L);
MasashiNomura 42:cc8501b824ba 707 subProp[2]->setCoef(IN, 0.00004, 0.1039, -50.946);
MasashiNomura 42:cc8501b824ba 708 subProp[2]->setCoef(OUT,0.00004, 0.0886, -21.033);
MasashiNomura 28:fdb3b144e342 709
MasashiNomura 28:fdb3b144e342 710 subProp[3] = new HbSubProp(R_R);
MasashiNomura 42:cc8501b824ba 711 subProp[3]->setCoef(IN, 0.00005, 0.0108, 105.15);
MasashiNomura 42:cc8501b824ba 712 subProp[3]->setCoef(OUT,0.00004, 0.0456, 50.611);
MasashiNomura 28:fdb3b144e342 713
takeru0x1103 18:5aa48aec9cae 714 imu = new Imu(p28,p27);
takeru0x1103 18:5aa48aec9cae 715
MasashiNomura 22:24c9c2dedca9 716 ope = new HbUserOpe();
MasashiNomura 22:24c9c2dedca9 717
MasashiNomura 43:156199c2f9b6 718 en_srv_step[0] = 100;
MasashiNomura 43:156199c2f9b6 719 en_srv_step[1] = 100;
MasashiNomura 36:2cc739c7e4cb 720 // //初期化
MasashiNomura 36:2cc739c7e4cb 721 // motorVal[0]=0;
MasashiNomura 36:2cc739c7e4cb 722 // motorVal[1]=0;
MasashiNomura 36:2cc739c7e4cb 723 // motorVal[2]=0;
MasashiNomura 36:2cc739c7e4cb 724 // motorVal[3]=0;
takeru0x1103 17:f9610f3cfa1b 725 }