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@46:5074781a28dd, 2019-02-19 (annotated)

Committer:
MasashiNomura
Date:
Tue Feb 19 11:52:49 2019 +0000
Revision:
46:5074781a28dd
Parent:
40:debe99e228d3
Child:
48:71aec693a7dc
2019/02/19 change Engine Throttle UP/DOWN Btn Quantity and modify comment

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