teamALI / Mbed 2 deprecated HB2018

ジャイロ追加前

Dependencies:   mbed FreeRTOS

HbManager.cpp@64:40943c843da7, 2019-03-11 (annotated)

Committer:
MasashiNomura
Date:
Mon Mar 11 04:24:00 2019 +0000
Revision:
64:40943c843da7
Parent:
62:4b873328d3a8
Parent:
63:aee44afe6363
Child:
66:0b3b290137b6
2019/03/11 Merge

Who changed what in which revision?

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