![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ジャイロ追加前
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?
User | Revision | Line number | New 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 | } |