Koichi Kikkawa / BLDCmotorDriver_20200902_1

20200821_motacon_ver4

BLDCmotorDriver.cpp@7:7365c75b2af1, 2020-09-02 (annotated)

Committer:
MPPT51
Date:
Wed Sep 02 05:04:31 2020 +0000
Revision:
7:7365c75b2af1
Parent:
6:51643d078474 
20200902_kikkawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mslovic 0:5602fba2a7f7 1 #include "BLDCmotorDriver.h"
mslovic 0:5602fba2a7f7 2
tbjazic 1:786897114846 3 BLDCmotorDriver::BLDCmotorDriver(PinName pGH_A, PinName pGH_B, PinName pGH_C, PinName pGL_A, PinName pGL_B, PinName pGL_C,
mslovic 3:a4b4a8e3f2a0 4 PinName pH1, PinName pH2, PinName pH3, PinName pFault) :
tbjazic 1:786897114846 5 GH_A(pGH_A), GH_B(pGH_B), GH_C(pGH_C), GL_A(pGL_A), GL_B(pGL_B), GL_C(pGL_C),
mslovic 3:a4b4a8e3f2a0 6 H1(pH1), H2(pH2), H3(pH3), Fault(LED1){
MPPT51 6:51643d078474 7 HS_usec = 1e9; //速度表示を0にするため、最初にHS_usecを非常に大きい値にする。HS_usecが0だとprintfにinfと表示される。(main関数での計算上そうなる)
MPPT51 6:51643d078474 8 sampleTime = 1e-4;
MPPT51 6:51643d078474 9 switchingPeriod = 1.0 / 20e3; //PWMの周波数を20kHzにしている
mslovic 0:5602fba2a7f7 10 dutyCycle = tempDutyCycle = 0;
MPPT51 6:51643d078474 11 GL_A.period(switchingPeriod); // applies to all PwmOut instances
MPPT51 6:51643d078474 12 H1.rise(this, &BLDCmotorDriver::H1rise); //U相ホールセンサの立上りだけ回転数計測処理してからcommutation処理するようにしている
tbjazic 1:786897114846 13 H2.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 14 H3.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 15 H1.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 16 H2.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 17 H3.fall(this, &BLDCmotorDriver::commutation);
MPPT51 7:7365c75b2af1 18 ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
MPPT51 6:51643d078474 19 }
MPPT51 6:51643d078474 20 int BLDCmotorDriver::getSector(){ //ホールセンサ信号読み取り モータ回転角検出
mslovic 3:a4b4a8e3f2a0 21 h1 = H1.read();
mslovic 3:a4b4a8e3f2a0 22 h2 = H2.read();
mslovic 3:a4b4a8e3f2a0 23 h3 = H3.read();
MPPT51 6:51643d078474 24 if(h1 == 0 && h2 == 0 && h3 == 1){ _currentSector = 0; }
MPPT51 6:51643d078474 25 else if(h1 == 0 && h2 == 1 && h3 == 1){ _currentSector = 1; }
MPPT51 6:51643d078474 26 else if(h1 == 0 && h2 == 1 && h3 == 0){ _currentSector = 2; }
MPPT51 6:51643d078474 27 else if(h1 == 1 && h2 == 1 && h3 == 0){ _currentSector = 3; }
MPPT51 6:51643d078474 28 else if(h1 == 1 && h2 == 0 && h3 == 0){ _currentSector = 4; }
MPPT51 6:51643d078474 29 else if(h1 == 1 && h2 == 0 && h3 == 1){ _currentSector = 5; }
MPPT51 6:51643d078474 30 currentSector = _currentSector;
MPPT51 6:51643d078474 31 Fault = 0;
MPPT51 6:51643d078474 32 return currentSector;
MPPT51 6:51643d078474 33 }
MPPT51 6:51643d078474 34 void BLDCmotorDriver::H1rise() { //H1 (U相ホールセンサ)
MPPT51 7:7365c75b2af1 35 //回転数計測
MPPT51 6:51643d078474 36 HS_check = 0; //ホールセンサ割込みが起きていることを表すため0を代入
MPPT51 6:51643d078474 37 HS_cnt++;
MPPT51 6:51643d078474 38 t.stop();
MPPT51 7:7365c75b2af1 39 if( t.read_ms() >= 50 ){ //Xms間に何回ホールセンサ(H1)が立ち上がったか確認し、立上りから次の立上りまでの1回あたりの時間を調べる。時間が長いと(200msとか)dutyを急激に変化させたとき、duty制限が追いつかずモータがガタつく(速度で制限を決めているため)。
MPPT51 7:7365c75b2af1 40 HS_usec = t.read_us() / HS_cnt; //反対に時間が短いと速度計測の精度が落ちる //ちょうどいい時間を探すorプログラム改良が必要
MPPT51 6:51643d078474 41 HS_cnt = 0;
MPPT51 6:51643d078474 42 t.reset();
MPPT51 6:51643d078474 43 }
MPPT51 6:51643d078474 44 t.start();
MPPT51 7:7365c75b2af1 45 //駆動信号出力処理
MPPT51 7:7365c75b2af1 46 commutation();
MPPT51 6:51643d078474 47 }
MPPT51 6:51643d078474 48 void BLDCmotorDriver::commutation() {
MPPT51 7:7365c75b2af1 49 //ホールセンサ間の時間計測の一部
MPPT51 7:7365c75b2af1 50 if( t.read_ms() >= 300 && HS_check ){ //ホールセンサ(U相立上り)割り込みが一定時間発生していない(モータが回転していない場合)の処理
MPPT51 7:7365c75b2af1 51 HS_usec = 1e9; //速度表示を0にするためHS_usecを非常に大きい値にする
MPPT51 6:51643d078474 52 }
MPPT51 7:7365c75b2af1 53 HS_check = 1; //ホールセンサ割込み関数で0に戻される。割込みが起きない場合(1のまま一定時間過ぎると)HS_usecに1e9代入する処理に繋がる。
mslovic 3:a4b4a8e3f2a0 54
MPPT51 7:7365c75b2af1 55 //モータ回転数と車両時速の計算
MPPT51 7:7365c75b2af1 56 rpm = (float)(3750 / (HS_usec * 0.001)); //マイクロ秒をミリ秒に直して逆数を3750に掛ける //BLDCmotorDriver.cpp(.h)で使っている変数をmainで使う場合は「M.」を付ける。「M.」なのはmainの上の方でそう設定してるから。usecはmicro second:マイクロ秒
MPPT51 7:7365c75b2af1 57 speed = rpm * 0.10518; //3750とか0.010518は、excelで計算している。ファイル名「」
MPPT51 7:7365c75b2af1 58 //加速の制限
MPPT51 7:7365c75b2af1 59 dc_limit1 = (0.0063*speed*speed + 0.8107*speed + 20.24) / 100; //加速側制限.elsxの式 //制御確認用モータ用の加速調整(電源電圧13Vにする) //そのまま車載モータにも使えるかも実際に確認する必要あり。70km/hでduty制限なくなる(dc_limit>=100)
MPPT51 7:7365c75b2af1 60 if( tempDutyCycle > dc_limit1 ){ tempDutyCycle = dc_limit1; } //加速制限の式で求めた制限値を超えている場合、その値を代入する
MPPT51 7:7365c75b2af1 61 //モード駆動信号出力
MPPT51 6:51643d078474 62 dutyCycle = tempDutyCycle;
MPPT51 6:51643d078474 63 currentSector = getSector();
mslovic 0:5602fba2a7f7 64 if (dutyCycle > 0) {
MPPT51 7:7365c75b2af1 65 if( accel ){
MPPT51 6:51643d078474 66 currentSector++;
MPPT51 7:7365c75b2af1 67 if(currentSector > 5){ currentSector = 0; }
MPPT51 6:51643d078474 68 switch(currentSector) { /*正転*/
MPPT51 6:51643d078474 69 case 0: //001
MPPT51 6:51643d078474 70 GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 6:51643d078474 71 break;
MPPT51 6:51643d078474 72 case 1:
MPPT51 6:51643d078474 73 GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 6:51643d078474 74 break;
MPPT51 6:51643d078474 75 case 2:
MPPT51 6:51643d078474 76 GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 6:51643d078474 77 break;
MPPT51 6:51643d078474 78 case 3:
MPPT51 6:51643d078474 79 GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 6:51643d078474 80 break;
MPPT51 6:51643d078474 81 case 4:
MPPT51 6:51643d078474 82 GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 6:51643d078474 83 break;
MPPT51 6:51643d078474 84 case 5:
MPPT51 6:51643d078474 85 GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 6:51643d078474 86 break;
MPPT51 6:51643d078474 87 }
MPPT51 6:51643d078474 88 }
MPPT51 7:7365c75b2af1 89 else if ( !accel ) { //アクセルOFFのとき
MPPT51 7:7365c75b2af1 90 GL_C = 0; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 0; //何もしない
mslovic 3:a4b4a8e3f2a0 91 }
MPPT51 6:51643d078474 92 }
MPPT51 7:7365c75b2af1 93 /*
MPPT51 6:51643d078474 94 else if( dutyCycle < 0 ){
MPPT51 7:7365c75b2af1 95 if( direction ){ //逆転
MPPT51 6:51643d078474 96 currentSector--;
MPPT51 6:51643d078474 97 if(currentSector < 0){
MPPT51 6:51643d078474 98 currentSector = 5;
MPPT51 6:51643d078474 99 }
MPPT51 6:51643d078474 100 switch(currentSector) {
MPPT51 6:51643d078474 101 case 0:
MPPT51 6:51643d078474 102 GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 6:51643d078474 103 break;
MPPT51 6:51643d078474 104 case 1:
MPPT51 6:51643d078474 105 GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 6:51643d078474 106 break;
MPPT51 6:51643d078474 107 case 2:
MPPT51 6:51643d078474 108 GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 6:51643d078474 109 break;
MPPT51 6:51643d078474 110 case 3:
MPPT51 6:51643d078474 111 GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 6:51643d078474 112 break;
MPPT51 6:51643d078474 113 case 4:
MPPT51 6:51643d078474 114 GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 6:51643d078474 115 break;
MPPT51 6:51643d078474 116 case 5:
MPPT51 6:51643d078474 117 GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 6:51643d078474 118 break;
MPPT51 6:51643d078474 119 }
MPPT51 6:51643d078474 120 }
MPPT51 6:51643d078474 121 else if ( !direction ) { //dutyがマイナスかつ進行方向が後退になっている場合
MPPT51 6:51643d078474 122 GL_C = -dutyCycle; GL_B = -dutyCycle; GL_A = -dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
MPPT51 6:51643d078474 123 }
MPPT51 6:51643d078474 124 }
MPPT51 7:7365c75b2af1 125 */
MPPT51 7:7365c75b2af1 126 else if( dutyCycle < 0 ){
MPPT51 7:7365c75b2af1 127 GL_C = -dutyCycle; GL_B = -dutyCycle; GL_A = -dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
mslovic 0:5602fba2a7f7 128 }
mslovic 0:5602fba2a7f7 129 }
mslovic 0:5602fba2a7f7 130 void BLDCmotorDriver::setDutyCycle(float dc) {
mslovic 0:5602fba2a7f7 131 if (dc >= -1 && dc <= 1) {
mslovic 3:a4b4a8e3f2a0 132 ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
mslovic 0:5602fba2a7f7 133 tempDutyCycle = dc;
mslovic 0:5602fba2a7f7 134 } else {
mslovic 0:5602fba2a7f7 135 coast();
mslovic 0:5602fba2a7f7 136 }
mslovic 0:5602fba2a7f7 137 }
MPPT51 6:51643d078474 138 void BLDCmotorDriver::setDirection(float DS) {
MPPT51 6:51643d078474 139 if (!DS) {
MPPT51 6:51643d078474 140 direction = 0;
MPPT51 6:51643d078474 141 } else {
MPPT51 6:51643d078474 142 direction = 1;
MPPT51 6:51643d078474 143 }
MPPT51 6:51643d078474 144 }
mslovic 0:5602fba2a7f7 145 void BLDCmotorDriver::coast() {
MPPT51 6:51643d078474 146 GH_A = 0; GL_A = 0; GH_B = 0; GL_B = 0; GH_C = 0; GL_C = 0;
mslovic 0:5602fba2a7f7 147 dutyCycle = tempDutyCycle = 0;
mslovic 0:5602fba2a7f7 148 ticker.detach();
mslovic 0:5602fba2a7f7 149 }
mslovic 0:5602fba2a7f7 150 float BLDCmotorDriver::getDutyCycle() {
mslovic 0:5602fba2a7f7 151 return dutyCycle;
mslovic 0:5602fba2a7f7 152 }