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@28:fdb3b144e342, 2018-12-15 (annotated)

Committer:
MasashiNomura
Date:
Sat Dec 15 14:20:04 2018 +0000
Revision:
28:fdb3b144e342
Parent:
27:ff63c23bc689
Child:
29:eb3d72dd94aa
2018/12/15 modify HbMotorClass -> HbSubProp&HbMotCtrl

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 28:fdb3b144e342 72 for(int i = 0; i < 4; ++i){
MasashiNomura 28:fdb3b144e342 73 ready.reset(i);
MasashiNomura 28:fdb3b144e342 74 }
MasashiNomura 28:fdb3b144e342 75 ofs = 0;
MasashiNomura 28:fdb3b144e342 76 pwmMin = 0;
MasashiNomura 28:fdb3b144e342 77 pwmMax = 0;
MasashiNomura 28:fdb3b144e342 78 rpmMin = 0;
MasashiNomura 28:fdb3b144e342 79 rpmMax = 0;
MasashiNomura 28:fdb3b144e342 80 coefA = 0;
MasashiNomura 28:fdb3b144e342 81 coefB = 0;
MasashiNomura 28:fdb3b144e342 82 coefC = 0;
MasashiNomura 28:fdb3b144e342 83 curPwmVal = 0;
MasashiNomura 28:fdb3b144e342 84 curRpmVal = 0;
MasashiNomura 28:fdb3b144e342 85 }
MasashiNomura 28:fdb3b144e342 86 //=========================================
MasashiNomura 28:fdb3b144e342 87 //デストラクタ
MasashiNomura 28:fdb3b144e342 88 //=========================================
MasashiNomura 28:fdb3b144e342 89 HbMotCtrl::~HbMotCtrl(){
MasashiNomura 28:fdb3b144e342 90 }
MasashiNomura 28:fdb3b144e342 91
MasashiNomura 28:fdb3b144e342 92 bool HbMotCtrl::isReady()
MasashiNomura 28:fdb3b144e342 93 {
MasashiNomura 28:fdb3b144e342 94 return true;
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 ready.set(0);
MasashiNomura 28:fdb3b144e342 100 }
MasashiNomura 28:fdb3b144e342 101 void HbMotCtrl::setRpmLimt(INT16 Min, INT16 Max){
MasashiNomura 28:fdb3b144e342 102 rpmMax = Max;
MasashiNomura 28:fdb3b144e342 103 rpmMin = Min;
MasashiNomura 28:fdb3b144e342 104 ready.set(1);
MasashiNomura 28:fdb3b144e342 105 }
MasashiNomura 28:fdb3b144e342 106 void HbMotCtrl::setMotCoef(float a, float b, float c){
MasashiNomura 28:fdb3b144e342 107 coefA = a;
MasashiNomura 28:fdb3b144e342 108 coefB = b;
MasashiNomura 28:fdb3b144e342 109 coefC = c;
MasashiNomura 28:fdb3b144e342 110 ready.set(2);
MasashiNomura 28:fdb3b144e342 111 }
MasashiNomura 28:fdb3b144e342 112 //void HbMotCtrl::makeTable(){
MasashiNomura 28:fdb3b144e342 113 // 0は0rpm,1%は1500rpm(minimum)....100%はMaxrpm
MasashiNomura 28:fdb3b144e342 114 // tblVal[0] = 0;
MasashiNomura 28:fdb3b144e342 115 // tblVal[99] = pwmMax;
MasashiNomura 28:fdb3b144e342 116 // float rpmStep = (rpmMax - rpmMin) / 99;
MasashiNomura 28:fdb3b144e342 117 // int tmpRpm;
MasashiNomura 28:fdb3b144e342 118 // int tmpPwm;
MasashiNomura 28:fdb3b144e342 119 // for(int i = 1; i < 99; ++i){
MasashiNomura 28:fdb3b144e342 120 // tmpRpm = rpmMin + (int)((rpmStep * (i-1)) + 0.5);
MasashiNomura 28:fdb3b144e342 121 // tmpPwm = (int)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
MasashiNomura 28:fdb3b144e342 122 // tblVal[i] = tmpPwm;
MasashiNomura 28:fdb3b144e342 123 // }
MasashiNomura 28:fdb3b144e342 124 // ready.set(3);
MasashiNomura 28:fdb3b144e342 125 //}
MasashiNomura 28:fdb3b144e342 126
MasashiNomura 28:fdb3b144e342 127 INT16 HbMotCtrl::calcPwm(INT16 percent)
MasashiNomura 28:fdb3b144e342 128 {
MasashiNomura 28:fdb3b144e342 129 float rpmStep = (rpmMax - rpmMin) / 99.0;
MasashiNomura 28:fdb3b144e342 130 int tmpRpm;
MasashiNomura 28:fdb3b144e342 131 INT16 tmpPwm;
MasashiNomura 28:fdb3b144e342 132 if(percent == 0){
MasashiNomura 28:fdb3b144e342 133 tmpPwm = 0;
MasashiNomura 28:fdb3b144e342 134 } else {
MasashiNomura 28:fdb3b144e342 135 tmpRpm = rpmMin + (int)((rpmStep * (percent-1)) + 0.5);
MasashiNomura 28:fdb3b144e342 136 tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
MasashiNomura 28:fdb3b144e342 137 }
MasashiNomura 28:fdb3b144e342 138
MasashiNomura 28:fdb3b144e342 139 return tmpPwm;
MasashiNomura 28:fdb3b144e342 140 }
MasashiNomura 28:fdb3b144e342 141
MasashiNomura 28:fdb3b144e342 142 void HbMotCtrl::setValue(float value){
MasashiNomura 28:fdb3b144e342 143 int iVal = 0;
MasashiNomura 28:fdb3b144e342 144 if(value + 0.5 > 49){
MasashiNomura 28:fdb3b144e342 145 iVal = 49;
MasashiNomura 28:fdb3b144e342 146 }
MasashiNomura 28:fdb3b144e342 147 else if (value < 0){
MasashiNomura 28:fdb3b144e342 148 iVal = 0;
MasashiNomura 28:fdb3b144e342 149 }
MasashiNomura 28:fdb3b144e342 150 else{
MasashiNomura 28:fdb3b144e342 151 iVal = value + 0.5;
MasashiNomura 28:fdb3b144e342 152 }
MasashiNomura 28:fdb3b144e342 153 val = iVal;
MasashiNomura 28:fdb3b144e342 154 iVal+=ofs;
MasashiNomura 28:fdb3b144e342 155 UCHAR uPos = pos * 2 + typ;
MasashiNomura 28:fdb3b144e342 156
MasashiNomura 28:fdb3b144e342 157 fpgaMotor(uPos, calcPwm(iVal));
MasashiNomura 28:fdb3b144e342 158 }
MasashiNomura 28:fdb3b144e342 159 void HbMotCtrl::setOfs(float value){
MasashiNomura 28:fdb3b144e342 160 int iVal = 0;
MasashiNomura 28:fdb3b144e342 161 if(value + 0.5 > 49){
MasashiNomura 28:fdb3b144e342 162 iVal = 49;
MasashiNomura 28:fdb3b144e342 163 }
MasashiNomura 28:fdb3b144e342 164 else if (value < 0){
MasashiNomura 28:fdb3b144e342 165 iVal = 0;
MasashiNomura 28:fdb3b144e342 166 }
MasashiNomura 28:fdb3b144e342 167 else{
MasashiNomura 28:fdb3b144e342 168 iVal = value + 0.5;
MasashiNomura 28:fdb3b144e342 169 }
MasashiNomura 28:fdb3b144e342 170 ofs = iVal;
MasashiNomura 28:fdb3b144e342 171 iVal+=val;
MasashiNomura 28:fdb3b144e342 172 UCHAR uPos = pos * 2 + typ;
MasashiNomura 28:fdb3b144e342 173 fpgaMotor(uPos, calcPwm(iVal));
MasashiNomura 28:fdb3b144e342 174 }
MasashiNomura 28:fdb3b144e342 175 float HbMotCtrl::getValue(){
MasashiNomura 28:fdb3b144e342 176 return val;
MasashiNomura 28:fdb3b144e342 177 }
MasashiNomura 28:fdb3b144e342 178 float HbMotCtrl::getOfs(){
MasashiNomura 28:fdb3b144e342 179 return ofs;
MasashiNomura 28:fdb3b144e342 180 }
MasashiNomura 28:fdb3b144e342 181
MasashiNomura 28:fdb3b144e342 182 //=========================================
MasashiNomura 28:fdb3b144e342 183 //コンストラクタ
MasashiNomura 28:fdb3b144e342 184 //=========================================
MasashiNomura 28:fdb3b144e342 185 HbSubProp::HbSubProp(eMotPos Pos){
MasashiNomura 28:fdb3b144e342 186 pos = Pos;
MasashiNomura 28:fdb3b144e342 187 motCtrl[0] = new HbMotCtrl(pos,0);
MasashiNomura 28:fdb3b144e342 188 motCtrl[0]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 189 motCtrl[0]->setRpmLimt(RPM_MIN, RPM_IN_MAX);
MasashiNomura 28:fdb3b144e342 190 motCtrl[1] = new HbMotCtrl(pos,1);
MasashiNomura 28:fdb3b144e342 191 motCtrl[1]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 192 motCtrl[1]->setRpmLimt(RPM_MIN, RPM_OUT_MAX);
MasashiNomura 28:fdb3b144e342 193 }
MasashiNomura 28:fdb3b144e342 194 //=========================================
MasashiNomura 28:fdb3b144e342 195 //デストラクタ
MasashiNomura 28:fdb3b144e342 196 //=========================================
MasashiNomura 28:fdb3b144e342 197 HbSubProp::~HbSubProp(){
MasashiNomura 28:fdb3b144e342 198 delete motCtrl[0];
MasashiNomura 28:fdb3b144e342 199 delete motCtrl[1];
MasashiNomura 28:fdb3b144e342 200 }
MasashiNomura 28:fdb3b144e342 201
MasashiNomura 28:fdb3b144e342 202 void HbSubProp::setRpmLimIn(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 203 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 204 motCtrl[0]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 205 }
MasashiNomura 28:fdb3b144e342 206 void HbSubProp::setRpmLimOut(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 207 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 208 motCtrl[1]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 209 }
MasashiNomura 28:fdb3b144e342 210 void HbSubProp::setPwmLimit(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 211 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 212 motCtrl[0]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 213 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 214 motCtrl[1]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 215 }
MasashiNomura 28:fdb3b144e342 216 void HbSubProp::setCoef(eMotType type, float a, float b, float c){
MasashiNomura 28:fdb3b144e342 217 if(type == IN){
MasashiNomura 28:fdb3b144e342 218 if(motCtrl[0] == NULL)return;
MasashiNomura 28:fdb3b144e342 219 motCtrl[0]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 220 }
MasashiNomura 28:fdb3b144e342 221 else {
MasashiNomura 28:fdb3b144e342 222 if(motCtrl[1] == NULL)return;
MasashiNomura 28:fdb3b144e342 223 motCtrl[1]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 224 }
MasashiNomura 28:fdb3b144e342 225 }
MasashiNomura 28:fdb3b144e342 226 //void HbSubProp::makeTbl(){
MasashiNomura 28:fdb3b144e342 227 // if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 228 // if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 229 // motCtrl[0]->makeTable();
MasashiNomura 28:fdb3b144e342 230 // motCtrl[1]->makeTable();
MasashiNomura 28:fdb3b144e342 231 //}
MasashiNomura 28:fdb3b144e342 232 void HbSubProp::setValue(float val){
MasashiNomura 28:fdb3b144e342 233 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 234 motCtrl[0]->setValue(val);
MasashiNomura 28:fdb3b144e342 235 motCtrl[1]->setValue(val);
MasashiNomura 28:fdb3b144e342 236 }
MasashiNomura 28:fdb3b144e342 237 void HbSubProp::setOfs(float val){
MasashiNomura 28:fdb3b144e342 238 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 239 motCtrl[0]->setOfs(val);
MasashiNomura 28:fdb3b144e342 240 motCtrl[1]->setOfs(val);
MasashiNomura 28:fdb3b144e342 241 }
MasashiNomura 28:fdb3b144e342 242 float HbSubProp::getValue(){
MasashiNomura 28:fdb3b144e342 243 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 28:fdb3b144e342 244 // 同じはずなのでINのみ
MasashiNomura 28:fdb3b144e342 245 return motCtrl[0]->getValue();
MasashiNomura 28:fdb3b144e342 246 }
MasashiNomura 28:fdb3b144e342 247 float HbSubProp::getOfs(){
MasashiNomura 28:fdb3b144e342 248 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 28:fdb3b144e342 249 // 同じはずなのでINのみ
MasashiNomura 28:fdb3b144e342 250 return motCtrl[0]->getOfs();
MasashiNomura 28:fdb3b144e342 251 }
MasashiNomura 28:fdb3b144e342 252