teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

HbMotor.cpp@32:7f4145cc3551, 2018-12-20 (annotated)

Committer:
MasashiNomura
Date:
Thu Dec 20 11:38:09 2018 +0000
Revision:
32:7f4145cc3551
Parent:
29:eb3d72dd94aa
Child:
40:debe99e228d3
12/20 Add new Feedback for PID and Modify MotCtrl,SubProp,Attitude,HbManager,etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 18:5aa48aec9cae 1 #include "HbMotor.h"
takeru0x1103 18:5aa48aec9cae 2 #include "fpga.h"
takeru0x1103 18:5aa48aec9cae 3
takeru0x1103 18:5aa48aec9cae 4 //=========================================
takeru0x1103 18:5aa48aec9cae 5 //コンストラクタ
takeru0x1103 18:5aa48aec9cae 6 //=========================================
takeru0x1103 18:5aa48aec9cae 7 HbMotor::HbMotor(UCHAR iID ){
takeru0x1103 18:5aa48aec9cae 8 id =iID;
takeru0x1103 19:4b0fe9a5ec38 9 ofs =0;//
MasashiNomura 25:f3a6e7eec9c3 10 limitH =2000;
MasashiNomura 25:f3a6e7eec9c3 11 limitL =-2000;
MasashiNomura 25:f3a6e7eec9c3 12 curVal = 0;
takeru0x1103 18:5aa48aec9cae 13 }
takeru0x1103 18:5aa48aec9cae 14
takeru0x1103 18:5aa48aec9cae 15 //-----------------------------------------
takeru0x1103 18:5aa48aec9cae 16 //モーター設定
takeru0x1103 18:5aa48aec9cae 17 //-----------------------------------------
takeru0x1103 18:5aa48aec9cae 18 void HbMotor::setValue(INT16 iVal){
takeru0x1103 18:5aa48aec9cae 19 UINT16 val;
takeru0x1103 18:5aa48aec9cae 20
takeru0x1103 18:5aa48aec9cae 21 //入力リミット
takeru0x1103 18:5aa48aec9cae 22 if( iVal > limitH ){
takeru0x1103 18:5aa48aec9cae 23 val = limitH;
takeru0x1103 18:5aa48aec9cae 24 }else if( iVal < limitL ){
takeru0x1103 18:5aa48aec9cae 25 val = limitL;
takeru0x1103 18:5aa48aec9cae 26 }else{
takeru0x1103 18:5aa48aec9cae 27 val = iVal;
takeru0x1103 18:5aa48aec9cae 28 }
takeru0x1103 18:5aa48aec9cae 29
takeru0x1103 18:5aa48aec9cae 30 //オフセット重畳
MasashiNomura 25:f3a6e7eec9c3 31 curVal = val = val + ofs;
MasashiNomura 25:f3a6e7eec9c3 32 //sp.printf("MOT [%d]",curVal);//Test Code
takeru0x1103 19:4b0fe9a5ec38 33 //PWM幅設定
takeru0x1103 18:5aa48aec9cae 34 fpgaSubProp(id,val);
takeru0x1103 18:5aa48aec9cae 35 }
MasashiNomura 25:f3a6e7eec9c3 36
MasashiNomura 25:f3a6e7eec9c3 37 void HbMotor::setOfs(INT16 iVal){
MasashiNomura 24:c5945aaae777 38 ofs = iVal;
MasashiNomura 24:c5945aaae777 39 }
MasashiNomura 24:c5945aaae777 40
MasashiNomura 25:f3a6e7eec9c3 41 INT16 HbMotor::getOfs(){
MasashiNomura 25:f3a6e7eec9c3 42 return ofs;
MasashiNomura 25:f3a6e7eec9c3 43 }
MasashiNomura 25:f3a6e7eec9c3 44
MasashiNomura 24:c5945aaae777 45 void HbMotor::setLimit(INT16 low, INT16 hi){
MasashiNomura 24:c5945aaae777 46 limitL = low;
MasashiNomura 24:c5945aaae777 47 limitH = hi;
MasashiNomura 25:f3a6e7eec9c3 48 }
MasashiNomura 25:f3a6e7eec9c3 49
MasashiNomura 25:f3a6e7eec9c3 50 INT16 HbMotor::getCurrentValue(){
MasashiNomura 25:f3a6e7eec9c3 51 return curVal;
MasashiNomura 25:f3a6e7eec9c3 52 }
MasashiNomura 25:f3a6e7eec9c3 53
MasashiNomura 25:f3a6e7eec9c3 54 void HbMotor::setValueDirect(INT16 iVal){
MasashiNomura 25:f3a6e7eec9c3 55 //PWM幅設定
MasashiNomura 25:f3a6e7eec9c3 56 //sp.printf("MOT [%d]",iVal);//Test Code
MasashiNomura 25:f3a6e7eec9c3 57 fpgaSubProp(id,iVal);
MasashiNomura 25:f3a6e7eec9c3 58 }
MasashiNomura 27:ff63c23bc689 59
MasashiNomura 27:ff63c23bc689 60 void HbMotor::setValueFPGA(UCHAR iID, INT16 iVal){
MasashiNomura 27:ff63c23bc689 61 UCHAR tmp = id * 2 + iID;
MasashiNomura 27:ff63c23bc689 62 fpgaMotor(tmp,iVal);
MasashiNomura 27:ff63c23bc689 63 }
MasashiNomura 28:fdb3b144e342 64
MasashiNomura 28:fdb3b144e342 65
MasashiNomura 28:fdb3b144e342 66 //=========================================
MasashiNomura 28:fdb3b144e342 67 //コンストラクタ
MasashiNomura 28:fdb3b144e342 68 //=========================================
MasashiNomura 28:fdb3b144e342 69 HbMotCtrl::HbMotCtrl(eMotPos Pos, INT16 Type){
MasashiNomura 28:fdb3b144e342 70 pos = Pos;
MasashiNomura 28:fdb3b144e342 71 typ = Type;
MasashiNomura 29:eb3d72dd94aa 72 //for(int i = 0; i < 4; ++i){
MasashiNomura 29:eb3d72dd94aa 73 // ready.reset(i);
MasashiNomura 29:eb3d72dd94aa 74 //}
MasashiNomura 29:eb3d72dd94aa 75 val = 0;
MasashiNomura 32:7f4145cc3551 76 ofs = 0;
MasashiNomura 32:7f4145cc3551 77 attval = 0;
MasashiNomura 28:fdb3b144e342 78 pwmMin = 0;
MasashiNomura 28:fdb3b144e342 79 pwmMax = 0;
MasashiNomura 28:fdb3b144e342 80 rpmMin = 0;
MasashiNomura 28:fdb3b144e342 81 rpmMax = 0;
MasashiNomura 29:eb3d72dd94aa 82 rpmGain = 1;
MasashiNomura 29:eb3d72dd94aa 83 rpmOfs = 0;
MasashiNomura 28:fdb3b144e342 84 coefA = 0;
MasashiNomura 28:fdb3b144e342 85 coefB = 0;
MasashiNomura 28:fdb3b144e342 86 coefC = 0;
MasashiNomura 29:eb3d72dd94aa 87 //curPwmVal = 0;
MasashiNomura 29:eb3d72dd94aa 88 //curRpmVal = 0;
MasashiNomura 28:fdb3b144e342 89 }
MasashiNomura 28:fdb3b144e342 90 //=========================================
MasashiNomura 28:fdb3b144e342 91 //デストラクタ
MasashiNomura 28:fdb3b144e342 92 //=========================================
MasashiNomura 28:fdb3b144e342 93 HbMotCtrl::~HbMotCtrl(){
MasashiNomura 28:fdb3b144e342 94 }
MasashiNomura 28:fdb3b144e342 95
MasashiNomura 28:fdb3b144e342 96 void HbMotCtrl::setPwmLimt(INT16 Min, INT16 Max){
MasashiNomura 28:fdb3b144e342 97 pwmMax = Max;
MasashiNomura 28:fdb3b144e342 98 pwmMin = Min;
MasashiNomura 28:fdb3b144e342 99 }
MasashiNomura 28:fdb3b144e342 100 void HbMotCtrl::setRpmLimt(INT16 Min, INT16 Max){
MasashiNomura 28:fdb3b144e342 101 rpmMax = Max;
MasashiNomura 28:fdb3b144e342 102 rpmMin = Min;
MasashiNomura 29:eb3d72dd94aa 103 }
MasashiNomura 29:eb3d72dd94aa 104 void HbMotCtrl::setRpmOffset(INT16 ofs){
MasashiNomura 29:eb3d72dd94aa 105 rpmOfs = ofs;
MasashiNomura 29:eb3d72dd94aa 106 }
MasashiNomura 29:eb3d72dd94aa 107 void HbMotCtrl::setRpmGain(float gain){
MasashiNomura 29:eb3d72dd94aa 108 rpmGain = gain;
MasashiNomura 28:fdb3b144e342 109 }
MasashiNomura 28:fdb3b144e342 110 void HbMotCtrl::setMotCoef(float a, float b, float c){
MasashiNomura 28:fdb3b144e342 111 coefA = a;
MasashiNomura 28:fdb3b144e342 112 coefB = b;
MasashiNomura 28:fdb3b144e342 113 coefC = c;
MasashiNomura 28:fdb3b144e342 114 }
MasashiNomura 28:fdb3b144e342 115
MasashiNomura 32:7f4145cc3551 116 INT16 HbMotCtrl::calcPwm(INT16 rpm)
MasashiNomura 28:fdb3b144e342 117 {
MasashiNomura 28:fdb3b144e342 118 int tmpRpm;
MasashiNomura 28:fdb3b144e342 119 INT16 tmpPwm;
MasashiNomura 32:7f4145cc3551 120 if(rpm <= 0){
MasashiNomura 29:eb3d72dd94aa 121 tmpPwm = 0;
MasashiNomura 29:eb3d72dd94aa 122 } else {
MasashiNomura 29:eb3d72dd94aa 123 tmpRpm = rpmGain * rpm + rpmOfs;
MasashiNomura 29:eb3d72dd94aa 124 if(tmpRpm > rpmMax) {
MasashiNomura 29:eb3d72dd94aa 125 tmpRpm = rpmMax;
MasashiNomura 29:eb3d72dd94aa 126 }
MasashiNomura 29:eb3d72dd94aa 127 else if( tmpRpm < 0){
MasashiNomura 29:eb3d72dd94aa 128 tmpRpm = 0;
MasashiNomura 29:eb3d72dd94aa 129 }
MasashiNomura 29:eb3d72dd94aa 130 tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
MasashiNomura 29:eb3d72dd94aa 131 if(tmpPwm < 0) tmpPwm = 0;
MasashiNomura 29:eb3d72dd94aa 132 }
MasashiNomura 29:eb3d72dd94aa 133
MasashiNomura 29:eb3d72dd94aa 134 return tmpPwm;
MasashiNomura 29:eb3d72dd94aa 135 }
MasashiNomura 29:eb3d72dd94aa 136
MasashiNomura 32:7f4145cc3551 137 // void HbMotCtrl::setRpmValue(INT16 value){
MasashiNomura 32:7f4145cc3551 138 // INT16 iVal = 0;
MasashiNomura 32:7f4145cc3551 139 // if(value + ofs > rpmMax){
MasashiNomura 32:7f4145cc3551 140 // iVal = rpmMax - ofs;
MasashiNomura 32:7f4145cc3551 141 // }
MasashiNomura 32:7f4145cc3551 142 // else if (value < 0){
MasashiNomura 32:7f4145cc3551 143 // iVal = 0;
MasashiNomura 32:7f4145cc3551 144 // }
MasashiNomura 32:7f4145cc3551 145 // else{
MasashiNomura 32:7f4145cc3551 146 // iVal = value;
MasashiNomura 32:7f4145cc3551 147 // }
MasashiNomura 32:7f4145cc3551 148 // val = iVal;
MasashiNomura 32:7f4145cc3551 149 // iVal+=ofs;
MasashiNomura 32:7f4145cc3551 150 // UCHAR uPos = pos * 2 + typ;
MasashiNomura 29:eb3d72dd94aa 151
MasashiNomura 32:7f4145cc3551 152 // fpgaMotor(uPos, calcPwm((UINT16)iVal));
MasashiNomura 32:7f4145cc3551 153 // }
MasashiNomura 29:eb3d72dd94aa 154
MasashiNomura 32:7f4145cc3551 155 void HbMotCtrl::setRpmValue(eMotInType type, INT16 value){
MasashiNomura 29:eb3d72dd94aa 156 INT16 iVal = 0;
MasashiNomura 32:7f4145cc3551 157 INT16 tmpVal;
MasashiNomura 32:7f4145cc3551 158 if(type == OFS){
MasashiNomura 32:7f4145cc3551 159 iVal = attval + val;
MasashiNomura 32:7f4145cc3551 160 } else if(type == ATT){
MasashiNomura 32:7f4145cc3551 161 iVal = val + ofs;
MasashiNomura 32:7f4145cc3551 162 } else /*if(type == USER)*/{
MasashiNomura 32:7f4145cc3551 163 iVal = attval + ofs;
MasashiNomura 28:fdb3b144e342 164 }
MasashiNomura 32:7f4145cc3551 165 if(value + iVal > rpmMax){
MasashiNomura 32:7f4145cc3551 166 tmpVal = rpmMax - iVal;
MasashiNomura 32:7f4145cc3551 167 }
MasashiNomura 32:7f4145cc3551 168 else if (value < 0 && type != ATT){
MasashiNomura 32:7f4145cc3551 169 tmpVal = 0;
MasashiNomura 28:fdb3b144e342 170 }
MasashiNomura 28:fdb3b144e342 171 else{
MasashiNomura 32:7f4145cc3551 172 tmpVal = value;
MasashiNomura 28:fdb3b144e342 173 }
MasashiNomura 32:7f4145cc3551 174 if(OFS == type){
MasashiNomura 32:7f4145cc3551 175 ofs = tmpVal;
MasashiNomura 32:7f4145cc3551 176 } else if(ATT == type){
MasashiNomura 32:7f4145cc3551 177 attval = tmpVal;
MasashiNomura 32:7f4145cc3551 178 } else /*if(eMotInType::USER == type)*/
MasashiNomura 32:7f4145cc3551 179 {
MasashiNomura 32:7f4145cc3551 180 val = tmpVal;
MasashiNomura 32:7f4145cc3551 181 }
MasashiNomura 32:7f4145cc3551 182
MasashiNomura 32:7f4145cc3551 183 iVal = ofs + attval + val;
MasashiNomura 28:fdb3b144e342 184 UCHAR uPos = pos * 2 + typ;
MasashiNomura 28:fdb3b144e342 185
MasashiNomura 32:7f4145cc3551 186 fpgaMotor(uPos, calcPwm(iVal));
MasashiNomura 28:fdb3b144e342 187 }
MasashiNomura 32:7f4145cc3551 188
MasashiNomura 32:7f4145cc3551 189 // void HbMotCtrl::setRpmOfs(INT16 value){
MasashiNomura 32:7f4145cc3551 190 // int iVal = 0;
MasashiNomura 32:7f4145cc3551 191 // if(value + val > rpmMax){
MasashiNomura 32:7f4145cc3551 192 // iVal = rpmMax - val;
MasashiNomura 32:7f4145cc3551 193 // }
MasashiNomura 32:7f4145cc3551 194 // else if (value < 0){
MasashiNomura 32:7f4145cc3551 195 // iVal = 0;
MasashiNomura 32:7f4145cc3551 196 // }
MasashiNomura 32:7f4145cc3551 197 // else{
MasashiNomura 32:7f4145cc3551 198 // iVal = value;
MasashiNomura 32:7f4145cc3551 199 // }
MasashiNomura 32:7f4145cc3551 200 // ofs = iVal;
MasashiNomura 32:7f4145cc3551 201 // iVal+=val;
MasashiNomura 32:7f4145cc3551 202 // UCHAR uPos = pos * 2 + typ;
MasashiNomura 32:7f4145cc3551 203 // fpgaMotor(uPos, calcPwm((UINT16)iVal));
MasashiNomura 32:7f4145cc3551 204 // }
MasashiNomura 32:7f4145cc3551 205
MasashiNomura 32:7f4145cc3551 206 INT16 HbMotCtrl::getRpmValue(eMotInType type){
MasashiNomura 32:7f4145cc3551 207 if(OFS == type){
MasashiNomura 32:7f4145cc3551 208 return ofs;
MasashiNomura 32:7f4145cc3551 209 } else if(ATT == type){
MasashiNomura 32:7f4145cc3551 210 return attval;
MasashiNomura 32:7f4145cc3551 211 } else /*if(eMotInType::USER == type)*/{
MasashiNomura 32:7f4145cc3551 212 return val;
MasashiNomura 28:fdb3b144e342 213 }
MasashiNomura 28:fdb3b144e342 214 }
MasashiNomura 32:7f4145cc3551 215 // INT16 HbMotCtrl::getRpmOfs(){
MasashiNomura 32:7f4145cc3551 216 // return ofs;
MasashiNomura 32:7f4145cc3551 217 // }
MasashiNomura 28:fdb3b144e342 218
MasashiNomura 28:fdb3b144e342 219 //=========================================
MasashiNomura 28:fdb3b144e342 220 //コンストラクタ
MasashiNomura 28:fdb3b144e342 221 //=========================================
MasashiNomura 28:fdb3b144e342 222 HbSubProp::HbSubProp(eMotPos Pos){
MasashiNomura 28:fdb3b144e342 223 pos = Pos;
MasashiNomura 28:fdb3b144e342 224 motCtrl[0] = new HbMotCtrl(pos,0);
MasashiNomura 28:fdb3b144e342 225 motCtrl[0]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 226 motCtrl[0]->setRpmLimt(RPM_MIN, RPM_IN_MAX);
MasashiNomura 28:fdb3b144e342 227 motCtrl[1] = new HbMotCtrl(pos,1);
MasashiNomura 28:fdb3b144e342 228 motCtrl[1]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 229 motCtrl[1]->setRpmLimt(RPM_MIN, RPM_OUT_MAX);
MasashiNomura 28:fdb3b144e342 230 }
MasashiNomura 28:fdb3b144e342 231 //=========================================
MasashiNomura 28:fdb3b144e342 232 //デストラクタ
MasashiNomura 28:fdb3b144e342 233 //=========================================
MasashiNomura 28:fdb3b144e342 234 HbSubProp::~HbSubProp(){
MasashiNomura 28:fdb3b144e342 235 delete motCtrl[0];
MasashiNomura 28:fdb3b144e342 236 delete motCtrl[1];
MasashiNomura 28:fdb3b144e342 237 }
MasashiNomura 28:fdb3b144e342 238
MasashiNomura 28:fdb3b144e342 239 void HbSubProp::setRpmLimIn(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 240 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 241 motCtrl[0]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 242 }
MasashiNomura 28:fdb3b144e342 243 void HbSubProp::setRpmLimOut(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 244 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 245 motCtrl[1]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 246 }
MasashiNomura 28:fdb3b144e342 247 void HbSubProp::setPwmLimit(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 248 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 249 motCtrl[0]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 250 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 251 motCtrl[1]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 252 }
MasashiNomura 29:eb3d72dd94aa 253 void HbSubProp::setRpmGain(eMotType type, float gain){
MasashiNomura 29:eb3d72dd94aa 254 if(type == IN){
MasashiNomura 29:eb3d72dd94aa 255 motCtrl[0]->setRpmGain(gain);
MasashiNomura 29:eb3d72dd94aa 256 }
MasashiNomura 29:eb3d72dd94aa 257 else{
MasashiNomura 29:eb3d72dd94aa 258 motCtrl[1]->setRpmGain(gain);
MasashiNomura 29:eb3d72dd94aa 259 }
MasashiNomura 29:eb3d72dd94aa 260 }
MasashiNomura 29:eb3d72dd94aa 261 void HbSubProp::setRpmOffset(eMotType type, INT16 ofs){
MasashiNomura 29:eb3d72dd94aa 262 if(type == IN){
MasashiNomura 29:eb3d72dd94aa 263 motCtrl[0]->setRpmOffset(ofs);
MasashiNomura 29:eb3d72dd94aa 264 }
MasashiNomura 29:eb3d72dd94aa 265 else{
MasashiNomura 29:eb3d72dd94aa 266 motCtrl[1]->setRpmOffset(ofs);
MasashiNomura 29:eb3d72dd94aa 267 }
MasashiNomura 29:eb3d72dd94aa 268 }
MasashiNomura 28:fdb3b144e342 269 void HbSubProp::setCoef(eMotType type, float a, float b, float c){
MasashiNomura 28:fdb3b144e342 270 if(type == IN){
MasashiNomura 28:fdb3b144e342 271 if(motCtrl[0] == NULL)return;
MasashiNomura 28:fdb3b144e342 272 motCtrl[0]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 273 }
MasashiNomura 28:fdb3b144e342 274 else {
MasashiNomura 28:fdb3b144e342 275 if(motCtrl[1] == NULL)return;
MasashiNomura 28:fdb3b144e342 276 motCtrl[1]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 277 }
MasashiNomura 28:fdb3b144e342 278 }
MasashiNomura 28:fdb3b144e342 279 //void HbSubProp::makeTbl(){
MasashiNomura 28:fdb3b144e342 280 // if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 281 // if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 282 // motCtrl[0]->makeTable();
MasashiNomura 28:fdb3b144e342 283 // motCtrl[1]->makeTable();
MasashiNomura 28:fdb3b144e342 284 //}
MasashiNomura 32:7f4145cc3551 285 void HbSubProp::setValue(eMotInType type, INT16 val){
MasashiNomura 28:fdb3b144e342 286 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 32:7f4145cc3551 287 motCtrl[0]->setRpmValue(type, val);
MasashiNomura 32:7f4145cc3551 288 motCtrl[1]->setRpmValue(type, val);
MasashiNomura 28:fdb3b144e342 289 }
MasashiNomura 32:7f4145cc3551 290 // void HbSubProp::setOfs(INT16 val){
MasashiNomura 32:7f4145cc3551 291 // if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 32:7f4145cc3551 292 // motCtrl[0]->setRpmOfs(val);
MasashiNomura 32:7f4145cc3551 293 // motCtrl[1]->setRpmOfs(val);
MasashiNomura 32:7f4145cc3551 294 // }
MasashiNomura 32:7f4145cc3551 295 INT16 HbSubProp::getValue(eMotInType type){
MasashiNomura 28:fdb3b144e342 296 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 28:fdb3b144e342 297 // 同じはずなのでINのみ
MasashiNomura 32:7f4145cc3551 298 return motCtrl[0]->getRpmValue(type);
MasashiNomura 28:fdb3b144e342 299 }
MasashiNomura 32:7f4145cc3551 300 // INT16 HbSubProp::getOfs(){
MasashiNomura 32:7f4145cc3551 301 // if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 32:7f4145cc3551 302 // // 同じはずなのでINのみ
MasashiNomura 32:7f4145cc3551 303 // return motCtrl[0]->getRpmOfs();
MasashiNomura 32:7f4145cc3551 304 // }
MasashiNomura 28:fdb3b144e342 305