teamALI / Mbed 2 deprecated HB2018

Dependencies:   mbed FreeRTOS

Committer:
MasashiNomura
Date:
Sun Feb 24 10:33:34 2019 +0000
Revision:
48:71aec693a7dc
Parent:
46:5074781a28dd
Child:
60:8ed7b7f0c371
20190224 add cmd pid mode 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;
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 48:71aec693a7dc 164 // iVal = ofs + attval + val;
MasashiNomura 48:71aec693a7dc 165 // UCHAR uPos = pos * 2 + typ;
MasashiNomura 48:71aec693a7dc 166
MasashiNomura 48:71aec693a7dc 167 // if(gf_Print.d2.bf.mrpm){
MasashiNomura 48:71aec693a7dc 168 // sp.printf("POS[%d] Val[%d] ", uPos, iVal);
MasashiNomura 48:71aec693a7dc 169 // }
MasashiNomura 48:71aec693a7dc 170
MasashiNomura 48:71aec693a7dc 171 // fpgaMotor(uPos, calcPwm(iVal));
MasashiNomura 48:71aec693a7dc 172 }
MasashiNomura 48:71aec693a7dc 173
MasashiNomura 48:71aec693a7dc 174 void HbMotCtrl::setRpmValue(){
MasashiNomura 48:71aec693a7dc 175 INT16 iVal = 0;
MasashiNomura 32:7f4145cc3551 176 iVal = ofs + attval + val;
MasashiNomura 28:fdb3b144e342 177 UCHAR uPos = pos * 2 + typ;
MasashiNomura 48:71aec693a7dc 178
MasashiNomura 48:71aec693a7dc 179 if(gf_Print.d2.bf.mrpm){
MasashiNomura 48:71aec693a7dc 180 sp.printf("P%d V:%d ", uPos, iVal);
MasashiNomura 48:71aec693a7dc 181 }
MasashiNomura 48:71aec693a7dc 182
MasashiNomura 32:7f4145cc3551 183 fpgaMotor(uPos, calcPwm(iVal));
MasashiNomura 28:fdb3b144e342 184 }
MasashiNomura 32:7f4145cc3551 185 INT16 HbMotCtrl::getRpmValue(eMotInType type){
MasashiNomura 32:7f4145cc3551 186 if(OFS == type){
MasashiNomura 32:7f4145cc3551 187 return ofs;
MasashiNomura 32:7f4145cc3551 188 } else if(ATT == type){
MasashiNomura 32:7f4145cc3551 189 return attval;
MasashiNomura 32:7f4145cc3551 190 } else /*if(eMotInType::USER == type)*/{
MasashiNomura 32:7f4145cc3551 191 return val;
MasashiNomura 28:fdb3b144e342 192 }
MasashiNomura 28:fdb3b144e342 193 }
MasashiNomura 40:debe99e228d3 194 void HbMotCtrl::setValFpga(INT16 value)
MasashiNomura 40:debe99e228d3 195 {
MasashiNomura 40:debe99e228d3 196 UCHAR uPos = pos * 2 + typ;
MasashiNomura 40:debe99e228d3 197 fpgaval = value;
MasashiNomura 40:debe99e228d3 198 fpgaMotor(uPos, value);
MasashiNomura 40:debe99e228d3 199 }
MasashiNomura 40:debe99e228d3 200 INT16 HbMotCtrl::getValFpga()
MasashiNomura 40:debe99e228d3 201 {
MasashiNomura 40:debe99e228d3 202 return fpgaval;
MasashiNomura 40:debe99e228d3 203 }
MasashiNomura 28:fdb3b144e342 204 //=========================================
MasashiNomura 28:fdb3b144e342 205 //コンストラクタ
MasashiNomura 28:fdb3b144e342 206 //=========================================
MasashiNomura 28:fdb3b144e342 207 HbSubProp::HbSubProp(eMotPos Pos){
MasashiNomura 28:fdb3b144e342 208 pos = Pos;
MasashiNomura 28:fdb3b144e342 209 motCtrl[0] = new HbMotCtrl(pos,0);
MasashiNomura 28:fdb3b144e342 210 motCtrl[0]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 211 motCtrl[0]->setRpmLimt(RPM_MIN, RPM_IN_MAX);
MasashiNomura 28:fdb3b144e342 212 motCtrl[1] = new HbMotCtrl(pos,1);
MasashiNomura 28:fdb3b144e342 213 motCtrl[1]->setPwmLimt(200, PWN_MAX);
MasashiNomura 28:fdb3b144e342 214 motCtrl[1]->setRpmLimt(RPM_MIN, RPM_OUT_MAX);
MasashiNomura 28:fdb3b144e342 215 }
MasashiNomura 28:fdb3b144e342 216 //=========================================
MasashiNomura 28:fdb3b144e342 217 //デストラクタ
MasashiNomura 28:fdb3b144e342 218 //=========================================
MasashiNomura 28:fdb3b144e342 219 HbSubProp::~HbSubProp(){
MasashiNomura 28:fdb3b144e342 220 delete motCtrl[0];
MasashiNomura 28:fdb3b144e342 221 delete motCtrl[1];
MasashiNomura 28:fdb3b144e342 222 }
MasashiNomura 28:fdb3b144e342 223
MasashiNomura 28:fdb3b144e342 224 void HbSubProp::setRpmLimIn(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 225 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 226 motCtrl[0]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 227 }
MasashiNomura 28:fdb3b144e342 228 void HbSubProp::setRpmLimOut(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 229 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 230 motCtrl[1]->setRpmLimt(min,max);
MasashiNomura 28:fdb3b144e342 231 }
MasashiNomura 28:fdb3b144e342 232 void HbSubProp::setPwmLimit(INT16 min, INT16 max){
MasashiNomura 28:fdb3b144e342 233 if(motCtrl[0] == NULL) return;
MasashiNomura 28:fdb3b144e342 234 motCtrl[0]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 235 if(motCtrl[1] == NULL) return;
MasashiNomura 28:fdb3b144e342 236 motCtrl[1]->setPwmLimt(min, PWN_MAX);
MasashiNomura 28:fdb3b144e342 237 }
MasashiNomura 29:eb3d72dd94aa 238 void HbSubProp::setRpmGain(eMotType type, float gain){
MasashiNomura 29:eb3d72dd94aa 239 if(type == IN){
MasashiNomura 29:eb3d72dd94aa 240 motCtrl[0]->setRpmGain(gain);
MasashiNomura 29:eb3d72dd94aa 241 }
MasashiNomura 29:eb3d72dd94aa 242 else{
MasashiNomura 29:eb3d72dd94aa 243 motCtrl[1]->setRpmGain(gain);
MasashiNomura 29:eb3d72dd94aa 244 }
MasashiNomura 29:eb3d72dd94aa 245 }
MasashiNomura 29:eb3d72dd94aa 246 void HbSubProp::setRpmOffset(eMotType type, INT16 ofs){
MasashiNomura 29:eb3d72dd94aa 247 if(type == IN){
MasashiNomura 29:eb3d72dd94aa 248 motCtrl[0]->setRpmOffset(ofs);
MasashiNomura 29:eb3d72dd94aa 249 }
MasashiNomura 29:eb3d72dd94aa 250 else{
MasashiNomura 29:eb3d72dd94aa 251 motCtrl[1]->setRpmOffset(ofs);
MasashiNomura 29:eb3d72dd94aa 252 }
MasashiNomura 29:eb3d72dd94aa 253 }
MasashiNomura 28:fdb3b144e342 254 void HbSubProp::setCoef(eMotType type, float a, float b, float c){
MasashiNomura 28:fdb3b144e342 255 if(type == IN){
MasashiNomura 28:fdb3b144e342 256 if(motCtrl[0] == NULL)return;
MasashiNomura 28:fdb3b144e342 257 motCtrl[0]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 258 }
MasashiNomura 28:fdb3b144e342 259 else {
MasashiNomura 28:fdb3b144e342 260 if(motCtrl[1] == NULL)return;
MasashiNomura 28:fdb3b144e342 261 motCtrl[1]->setMotCoef(a,b,c);
MasashiNomura 28:fdb3b144e342 262 }
MasashiNomura 28:fdb3b144e342 263 }
MasashiNomura 40:debe99e228d3 264
MasashiNomura 32:7f4145cc3551 265 void HbSubProp::setValue(eMotInType type, INT16 val){
MasashiNomura 28:fdb3b144e342 266 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 32:7f4145cc3551 267 motCtrl[0]->setRpmValue(type, val);
MasashiNomura 32:7f4145cc3551 268 motCtrl[1]->setRpmValue(type, val);
MasashiNomura 28:fdb3b144e342 269 }
MasashiNomura 40:debe99e228d3 270
MasashiNomura 48:71aec693a7dc 271 void HbSubProp::setValue(){
MasashiNomura 48:71aec693a7dc 272 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 48:71aec693a7dc 273 motCtrl[0]->setRpmValue();
MasashiNomura 48:71aec693a7dc 274 motCtrl[1]->setRpmValue();
MasashiNomura 48:71aec693a7dc 275 }
MasashiNomura 48:71aec693a7dc 276
MasashiNomura 32:7f4145cc3551 277 INT16 HbSubProp::getValue(eMotInType type){
MasashiNomura 28:fdb3b144e342 278 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 28:fdb3b144e342 279 // 同じはずなのでINのみ
MasashiNomura 32:7f4145cc3551 280 return motCtrl[0]->getRpmValue(type);
MasashiNomura 28:fdb3b144e342 281 }
MasashiNomura 40:debe99e228d3 282
MasashiNomura 40:debe99e228d3 283 void HbSubProp::setValueFpgaMot(eMotType type, INT16 val)
MasashiNomura 40:debe99e228d3 284 {
MasashiNomura 40:debe99e228d3 285 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
MasashiNomura 40:debe99e228d3 286 if(type == IN){
MasashiNomura 40:debe99e228d3 287 motCtrl[0]->setValFpga(val);
MasashiNomura 40:debe99e228d3 288 }
MasashiNomura 40:debe99e228d3 289 else
MasashiNomura 40:debe99e228d3 290 {
MasashiNomura 40:debe99e228d3 291 motCtrl[1]->setValFpga(val);
MasashiNomura 40:debe99e228d3 292 }
MasashiNomura 40:debe99e228d3 293 }
MasashiNomura 28:fdb3b144e342 294
MasashiNomura 40:debe99e228d3 295 INT16 HbSubProp::getValueFpgaMot(eMotType type)
MasashiNomura 40:debe99e228d3 296 {
MasashiNomura 46:5074781a28dd 297 if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
MasashiNomura 40:debe99e228d3 298 if(type == IN){
MasashiNomura 40:debe99e228d3 299 return motCtrl[0]->getValFpga();
MasashiNomura 40:debe99e228d3 300 }
MasashiNomura 40:debe99e228d3 301 else
MasashiNomura 40:debe99e228d3 302 {
MasashiNomura 40:debe99e228d3 303 return motCtrl[1]->getValFpga();
MasashiNomura 40:debe99e228d3 304 }
MasashiNomura 40:debe99e228d3 305 }
MasashiNomura 40:debe99e228d3 306