Koichi Kikkawa / BLDCmotorDriver_re1

Motacon_20200317

BLDCmotorDriver.cpp@7:fb9e3b508237, 2020-03-17 (annotated)

Committer:
MPPT51
Date:
Tue Mar 17 05:11:00 2020 +0000
Revision:
7:fb9e3b508237
Parent:
6:959e725b6be9 
motorcon

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){
mslovic 3:a4b4a8e3f2a0 7
mslovic 0:5602fba2a7f7 8 sampleTime = 1e-3;
MPPT51 7:fb9e3b508237 9 switchingPeriod = 1.0 / 20e3; //PWMの周波数を20kHzにしている
mslovic 0:5602fba2a7f7 10 dutyCycle = tempDutyCycle = 0;
MPPT51 6:959e725b6be9 11 GL_A.period(switchingPeriod); // applies to all PwmOut instances
MPPT51 7:fb9e3b508237 12 rl.setLimits(0.5, -0.5, 0, sampleTime); // initial 10 second ramp 大きい値ほど素早く切り替わる
tbjazic 1:786897114846 13 H1.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 14 H2.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 15 H3.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 16 H1.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 17 H2.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 18 H3.fall(this, &BLDCmotorDriver::commutation);
MPPT51 7:fb9e3b508237 19 } /*
mslovic 0:5602fba2a7f7 20 void BLDCmotorDriver::configure(float sampleTime, float switchingFrequency, float rampUpSlope, float rampDownSlope) {
mslovic 0:5602fba2a7f7 21 if (sampleTime < 1e-6)
mslovic 0:5602fba2a7f7 22 sampleTime = 1e-3;
mslovic 0:5602fba2a7f7 23 if (switchingFrequency < 100)
mslovic 0:5602fba2a7f7 24 switchingFrequency = 20e3;
mslovic 0:5602fba2a7f7 25 if (rampUpSlope < 0 || rampUpSlope > 1)
mslovic 0:5602fba2a7f7 26 rampUpSlope = 0.1;
mslovic 0:5602fba2a7f7 27 if (rampDownSlope > 0 || rampDownSlope < -1)
mslovic 0:5602fba2a7f7 28 rampDownSlope = -0.1;
mslovic 0:5602fba2a7f7 29 this->sampleTime = sampleTime;
mslovic 0:5602fba2a7f7 30 switchingPeriod = 1.0 / switchingFrequency;
mslovic 0:5602fba2a7f7 31 rl.setLimits(rampUpSlope, rampDownSlope, 0, sampleTime);
MPPT51 7:fb9e3b508237 32 } */
mslovic 3:a4b4a8e3f2a0 33 int BLDCmotorDriver::getSector(){ // hall 120°
mslovic 3:a4b4a8e3f2a0 34 h1 = H1.read();
mslovic 3:a4b4a8e3f2a0 35 h2 = H2.read();
mslovic 3:a4b4a8e3f2a0 36 h3 = H3.read();
mslovic 3:a4b4a8e3f2a0 37 if(h1 == 0 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 38 _currentSector = 0;
mslovic 3:a4b4a8e3f2a0 39 }
mslovic 3:a4b4a8e3f2a0 40 else if(h1 == 0 && h2 == 1 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 41 _currentSector = 1;
mslovic 3:a4b4a8e3f2a0 42 }
mslovic 3:a4b4a8e3f2a0 43 else if(h1 == 0 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 44 _currentSector = 2;
mslovic 3:a4b4a8e3f2a0 45 }
mslovic 3:a4b4a8e3f2a0 46 else if(h1 == 1 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 47 _currentSector = 3;
mslovic 3:a4b4a8e3f2a0 48 }
mslovic 3:a4b4a8e3f2a0 49 else if(h1 == 1 && h2 == 0 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 50 _currentSector = 4;
mslovic 3:a4b4a8e3f2a0 51 }
mslovic 3:a4b4a8e3f2a0 52 else if(h1 == 1 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 53 _currentSector = 5;
mslovic 3:a4b4a8e3f2a0 54 }
MPPT51 6:959e725b6be9 55 previousSector = _currentSector - 1;
MPPT51 6:959e725b6be9 56 difference = _currentSector - previousSector;
MPPT51 6:959e725b6be9 57 if (difference == 1){
MPPT51 6:959e725b6be9 58 currentSector = _currentSector;
MPPT51 6:959e725b6be9 59 Fault = 0;
MPPT51 6:959e725b6be9 60 }
MPPT51 6:959e725b6be9 61 else{
MPPT51 6:959e725b6be9 62 Fault = 1;
MPPT51 6:959e725b6be9 63 }
MPPT51 6:959e725b6be9 64 return currentSector;
mslovic 3:a4b4a8e3f2a0 65 }
tbjazic 1:786897114846 66
mslovic 3:a4b4a8e3f2a0 67 void BLDCmotorDriver::commutation() {
MPPT51 6:959e725b6be9 68 dutyCycle = rl.out(tempDutyCycle);
MPPT51 6:959e725b6be9 69 currentSector = getSector();
mslovic 0:5602fba2a7f7 70 if (dutyCycle > 0) {
MPPT51 7:fb9e3b508237 71 if( !direction ){
MPPT51 7:fb9e3b508237 72 currentSector++;
MPPT51 7:fb9e3b508237 73 if(currentSector > 5){
MPPT51 7:fb9e3b508237 74 currentSector = 0;
MPPT51 7:fb9e3b508237 75 }
MPPT51 7:fb9e3b508237 76 switch(currentSector) { /*正転*/
MPPT51 7:fb9e3b508237 77 case 0: //001
MPPT51 7:fb9e3b508237 78 GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 7:fb9e3b508237 79 break;
MPPT51 7:fb9e3b508237 80 case 1:
MPPT51 7:fb9e3b508237 81 GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 7:fb9e3b508237 82 break;
MPPT51 7:fb9e3b508237 83 case 2:
MPPT51 7:fb9e3b508237 84 GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 7:fb9e3b508237 85 break;
MPPT51 7:fb9e3b508237 86 case 3:
MPPT51 7:fb9e3b508237 87 GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 7:fb9e3b508237 88 break;
MPPT51 7:fb9e3b508237 89 case 4:
MPPT51 7:fb9e3b508237 90 GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 7:fb9e3b508237 91 break;
MPPT51 7:fb9e3b508237 92 case 5:
MPPT51 7:fb9e3b508237 93 GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 7:fb9e3b508237 94 break;
MPPT51 7:fb9e3b508237 95 }
MPPT51 6:959e725b6be9 96 }
MPPT51 7:fb9e3b508237 97 else if ( direction ) { //dutyがマイナスかつ進行方向が後退になっている場合
MPPT51 7:fb9e3b508237 98 GL_C = dutyCycle; GL_B = dutyCycle; GL_A = dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
mslovic 3:a4b4a8e3f2a0 99 }
MPPT51 6:959e725b6be9 100 }
MPPT51 7:fb9e3b508237 101 else if( dutyCycle < 0 ){
MPPT51 7:fb9e3b508237 102 if( direction ){ /*逆転*/
MPPT51 7:fb9e3b508237 103 currentSector--;
MPPT51 7:fb9e3b508237 104 if(currentSector < 0){
MPPT51 7:fb9e3b508237 105 currentSector = 5;
MPPT51 7:fb9e3b508237 106 }
MPPT51 7:fb9e3b508237 107 switch(currentSector) {
MPPT51 7:fb9e3b508237 108 case 0:
MPPT51 7:fb9e3b508237 109 GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 7:fb9e3b508237 110 break;
MPPT51 7:fb9e3b508237 111 case 1:
MPPT51 7:fb9e3b508237 112 GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 7:fb9e3b508237 113 break;
MPPT51 7:fb9e3b508237 114 case 2:
MPPT51 7:fb9e3b508237 115 GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
MPPT51 7:fb9e3b508237 116 break;
MPPT51 7:fb9e3b508237 117 case 3:
MPPT51 7:fb9e3b508237 118 GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 7:fb9e3b508237 119 break;
MPPT51 7:fb9e3b508237 120 case 4:
MPPT51 7:fb9e3b508237 121 GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
MPPT51 7:fb9e3b508237 122 break;
MPPT51 7:fb9e3b508237 123 case 5:
MPPT51 7:fb9e3b508237 124 GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
MPPT51 7:fb9e3b508237 125 break;
MPPT51 7:fb9e3b508237 126 }
MPPT51 6:959e725b6be9 127 }
MPPT51 7:fb9e3b508237 128 else if ( !direction ) { //dutyがマイナスかつ進行方向が後退になっている場合
MPPT51 7:fb9e3b508237 129 GL_C = -dutyCycle; GL_B = -dutyCycle; GL_A = -dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
MPPT51 7:fb9e3b508237 130 }
MPPT51 7:fb9e3b508237 131 }
MPPT51 6:959e725b6be9 132 else {
mslovic 3:a4b4a8e3f2a0 133 coast();
mslovic 0:5602fba2a7f7 134 }
mslovic 0:5602fba2a7f7 135 }
mslovic 0:5602fba2a7f7 136 void BLDCmotorDriver::setDutyCycle(float dc) {
mslovic 0:5602fba2a7f7 137 if (dc >= -1 && dc <= 1) {
mslovic 3:a4b4a8e3f2a0 138 ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
mslovic 0:5602fba2a7f7 139 tempDutyCycle = dc;
mslovic 0:5602fba2a7f7 140 } else {
mslovic 0:5602fba2a7f7 141 coast();
mslovic 0:5602fba2a7f7 142 }
mslovic 0:5602fba2a7f7 143 }
MPPT51 7:fb9e3b508237 144 void BLDCmotorDriver::setDirection(float DS) {
MPPT51 7:fb9e3b508237 145 if (!DS) {
MPPT51 7:fb9e3b508237 146 direction = 0;
MPPT51 7:fb9e3b508237 147 } else {
MPPT51 7:fb9e3b508237 148 direction = 1;
MPPT51 7:fb9e3b508237 149 }
MPPT51 7:fb9e3b508237 150 if( !direction ){
MPPT51 7:fb9e3b508237 151 // GH_A=0;
MPPT51 7:fb9e3b508237 152 }else {
MPPT51 7:fb9e3b508237 153 // GH_A=1;
MPPT51 7:fb9e3b508237 154 }
MPPT51 7:fb9e3b508237 155 }
mslovic 3:a4b4a8e3f2a0 156 /*void BLDCmotorDriver::adjustDutyCycle() {
tbjazic 1:786897114846 157 dutyCycle = rl.out(tempDutyCycle);
tbjazic 1:786897114846 158 float diff = tempDutyCycle - dutyCycle;
tbjazic 1:786897114846 159 if (diff < 0.01 || diff > -0.01)
tbjazic 1:786897114846 160 ticker.detach();
mslovic 3:a4b4a8e3f2a0 161 }*/
mslovic 0:5602fba2a7f7 162 void BLDCmotorDriver::coast() {
mslovic 0:5602fba2a7f7 163 GH_A = 0;
mslovic 0:5602fba2a7f7 164 GL_A = 0;
mslovic 0:5602fba2a7f7 165 GH_B = 0;
mslovic 0:5602fba2a7f7 166 GL_B = 0;
mslovic 0:5602fba2a7f7 167 GH_C = 0;
mslovic 0:5602fba2a7f7 168 GL_C = 0;
mslovic 0:5602fba2a7f7 169 dutyCycle = tempDutyCycle = 0;
mslovic 0:5602fba2a7f7 170 rl.reset();
mslovic 0:5602fba2a7f7 171 ticker.detach();
mslovic 0:5602fba2a7f7 172 }
mslovic 0:5602fba2a7f7 173 float BLDCmotorDriver::getDutyCycle() {
mslovic 0:5602fba2a7f7 174 return dutyCycle;
mslovic 0:5602fba2a7f7 175 }