Koichi Kikkawa / BLDCmotorDriver_20200821_motacon_ver4

20200821_motacon_ver4

BLDCmotorDriver.cpp@5:cf8183ae80ef, 2020-03-13 (annotated)

Committer:
MPPT51
Date:
Fri Mar 13 01:07:30 2020 +0000
Revision:
5:cf8183ae80ef
Parent:
3:a4b4a8e3f2a0
Child:
6:51643d078474
motacon

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;
mslovic 0:5602fba2a7f7 9 switchingPeriod = 1.0 / 20e3;
mslovic 0:5602fba2a7f7 10 dutyCycle = tempDutyCycle = 0;
mslovic 0:5602fba2a7f7 11 GH_A.period(switchingPeriod); // applies to all PwmOut instances
MPPT51 5:cf8183ae80ef 12 // rl.setLimits(0.1, -0.5, 0, sampleTime); // initial 10 second ramp
mslovic 3:a4b4a8e3f2a0 13 //H1.mode(PullNone);
mslovic 3:a4b4a8e3f2a0 14 //H2.mode(PullNone);
mslovic 3:a4b4a8e3f2a0 15 //H3.mode(PullNone);
tbjazic 1:786897114846 16 H1.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 17 H2.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 18 H3.rise(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 19 H1.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 20 H2.fall(this, &BLDCmotorDriver::commutation);
tbjazic 1:786897114846 21 H3.fall(this, &BLDCmotorDriver::commutation);
mslovic 0:5602fba2a7f7 22 }
mslovic 0:5602fba2a7f7 23 void BLDCmotorDriver::configure(float sampleTime, float switchingFrequency, float rampUpSlope, float rampDownSlope) {
mslovic 0:5602fba2a7f7 24 if (sampleTime < 1e-6)
mslovic 0:5602fba2a7f7 25 sampleTime = 1e-3;
mslovic 0:5602fba2a7f7 26 if (switchingFrequency < 100)
mslovic 0:5602fba2a7f7 27 switchingFrequency = 20e3;
mslovic 0:5602fba2a7f7 28 if (rampUpSlope < 0 || rampUpSlope > 1)
mslovic 0:5602fba2a7f7 29 rampUpSlope = 0.1;
mslovic 0:5602fba2a7f7 30 if (rampDownSlope > 0 || rampDownSlope < -1)
mslovic 0:5602fba2a7f7 31 rampDownSlope = -0.1;
mslovic 0:5602fba2a7f7 32 this->sampleTime = sampleTime;
mslovic 0:5602fba2a7f7 33 switchingPeriod = 1.0 / switchingFrequency;
mslovic 0:5602fba2a7f7 34 rl.setLimits(rampUpSlope, rampDownSlope, 0, sampleTime);
mslovic 0:5602fba2a7f7 35 }
mslovic 3:a4b4a8e3f2a0 36 int BLDCmotorDriver::getSector(){ // hall 120°
mslovic 3:a4b4a8e3f2a0 37
mslovic 3:a4b4a8e3f2a0 38 h1 = H1.read();
mslovic 3:a4b4a8e3f2a0 39 h2 = H2.read();
mslovic 3:a4b4a8e3f2a0 40 h3 = H3.read();
mslovic 3:a4b4a8e3f2a0 41
mslovic 3:a4b4a8e3f2a0 42
mslovic 3:a4b4a8e3f2a0 43 if(h1 == 0 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 44 _currentSector = 0;
mslovic 3:a4b4a8e3f2a0 45 }
mslovic 3:a4b4a8e3f2a0 46 else if(h1 == 0 && h2 == 1 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 47 _currentSector = 1;
mslovic 3:a4b4a8e3f2a0 48 }
mslovic 3:a4b4a8e3f2a0 49 else if(h1 == 0 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 50 _currentSector = 2;
mslovic 3:a4b4a8e3f2a0 51 }
mslovic 3:a4b4a8e3f2a0 52 else if(h1 == 1 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 53 _currentSector = 3;
mslovic 3:a4b4a8e3f2a0 54 }
mslovic 3:a4b4a8e3f2a0 55 else if(h1 == 1 && h2 == 0 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 56 _currentSector = 4;
mslovic 3:a4b4a8e3f2a0 57 }
mslovic 3:a4b4a8e3f2a0 58 else if(h1 == 1 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 59 _currentSector = 5;
mslovic 3:a4b4a8e3f2a0 60 }
mslovic 3:a4b4a8e3f2a0 61 previousSector = _currentSector - 1;
mslovic 3:a4b4a8e3f2a0 62 difference = _currentSector - previousSector;
mslovic 3:a4b4a8e3f2a0 63 if (difference == 1){
mslovic 3:a4b4a8e3f2a0 64 currentSector = _currentSector;
mslovic 3:a4b4a8e3f2a0 65 Fault = 0;
mslovic 3:a4b4a8e3f2a0 66 }
mslovic 3:a4b4a8e3f2a0 67 else Fault = 1;
mslovic 3:a4b4a8e3f2a0 68 return currentSector;
mslovic 3:a4b4a8e3f2a0 69 }
tbjazic 1:786897114846 70
mslovic 3:a4b4a8e3f2a0 71 void BLDCmotorDriver::commutation() {
mslovic 3:a4b4a8e3f2a0 72 dutyCycle = rl.out(tempDutyCycle);
mslovic 3:a4b4a8e3f2a0 73 currentSector = getSector();
mslovic 0:5602fba2a7f7 74 if (dutyCycle > 0) {
mslovic 3:a4b4a8e3f2a0 75 currentSector++;
mslovic 3:a4b4a8e3f2a0 76 if(currentSector > 5)currentSector = 0;
mslovic 3:a4b4a8e3f2a0 77 switch(currentSector) {
mslovic 3:a4b4a8e3f2a0 78
mslovic 3:a4b4a8e3f2a0 79 case 0: //001
MPPT51 5:cf8183ae80ef 80 GL_C = 0;
mslovic 0:5602fba2a7f7 81 GL_B = 0;
MPPT51 5:cf8183ae80ef 82 GL_A = 1;
mslovic 0:5602fba2a7f7 83 GH_C = 0;
MPPT51 5:cf8183ae80ef 84 GH_B = dutyCycle;
MPPT51 5:cf8183ae80ef 85 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 86 break;
mslovic 3:a4b4a8e3f2a0 87 case 1:
MPPT51 5:cf8183ae80ef 88 GL_C = 1;
MPPT51 5:cf8183ae80ef 89 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 90 GL_A = 0;
MPPT51 5:cf8183ae80ef 91 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 92 GH_B = dutyCycle;
MPPT51 5:cf8183ae80ef 93 GH_A = 0;
mslovic 0:5602fba2a7f7 94 break;
mslovic 3:a4b4a8e3f2a0 95 case 2:
MPPT51 5:cf8183ae80ef 96 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 97 GL_B = 0;
MPPT51 5:cf8183ae80ef 98 GL_A = 0;
mslovic 0:5602fba2a7f7 99 GH_C = 0;
MPPT51 5:cf8183ae80ef 100 GH_B = 0;
MPPT51 5:cf8183ae80ef 101 GH_A = dutyCycle;
mslovic 0:5602fba2a7f7 102 break;
mslovic 3:a4b4a8e3f2a0 103 case 3:
MPPT51 5:cf8183ae80ef 104 GL_C = 0;
MPPT51 5:cf8183ae80ef 105 GL_B = 1;
MPPT51 5:cf8183ae80ef 106 GL_A = 0;
MPPT51 5:cf8183ae80ef 107 GH_C = 0;
mslovic 0:5602fba2a7f7 108 GH_B = 0;
MPPT51 5:cf8183ae80ef 109 GH_A = dutyCycle;
mslovic 0:5602fba2a7f7 110 break;
mslovic 3:a4b4a8e3f2a0 111 case 4:
MPPT51 5:cf8183ae80ef 112 GL_C = 0;
MPPT51 5:cf8183ae80ef 113 GL_B = 1;
mslovic 0:5602fba2a7f7 114 GL_A = 0;
MPPT51 5:cf8183ae80ef 115 GH_C = dutyCycle;
mslovic 0:5602fba2a7f7 116 GH_B = 0;
MPPT51 5:cf8183ae80ef 117 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 118 break;
mslovic 3:a4b4a8e3f2a0 119 case 5:
MPPT51 5:cf8183ae80ef 120 GL_C = 0;
MPPT51 5:cf8183ae80ef 121 GL_B = 0;
MPPT51 5:cf8183ae80ef 122 GL_A = 1;
MPPT51 5:cf8183ae80ef 123 GH_C = dutyCycle;
mslovic 3:a4b4a8e3f2a0 124 GH_B = 0;
MPPT51 5:cf8183ae80ef 125 GH_A = 0;
mslovic 0:5602fba2a7f7 126 break;
mslovic 3:a4b4a8e3f2a0 127 }
mslovic 3:a4b4a8e3f2a0 128 } else if (dutyCycle < 0) {
mslovic 3:a4b4a8e3f2a0 129 currentSector--;
mslovic 3:a4b4a8e3f2a0 130 if(currentSector < 0)currentSector = 5;
mslovic 3:a4b4a8e3f2a0 131 switch(currentSector) {
MPPT51 5:cf8183ae80ef 132 case 0:
MPPT51 5:cf8183ae80ef 133 GL_C = 0;
MPPT51 5:cf8183ae80ef 134 GL_B = 0;
MPPT51 5:cf8183ae80ef 135 GL_A = 1;
MPPT51 5:cf8183ae80ef 136 GH_C = -dutyCycle;
MPPT51 5:cf8183ae80ef 137 GH_B = 0;
MPPT51 5:cf8183ae80ef 138 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 139 break;
mslovic 3:a4b4a8e3f2a0 140 case 1:
MPPT51 5:cf8183ae80ef 141 GL_C = 0;
mslovic 3:a4b4a8e3f2a0 142 GL_B = 0;
MPPT51 5:cf8183ae80ef 143 GL_A = 1;
mslovic 3:a4b4a8e3f2a0 144 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 145 GH_B = -dutyCycle;
MPPT51 5:cf8183ae80ef 146 GH_A = 0;
MPPT51 5:cf8183ae80ef 147 break;
MPPT51 5:cf8183ae80ef 148 case 2:
MPPT51 5:cf8183ae80ef 149 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 150 GL_B = 0;
MPPT51 5:cf8183ae80ef 151 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 152 GH_C = 0;
MPPT51 5:cf8183ae80ef 153 GH_B = -dutyCycle;
MPPT51 5:cf8183ae80ef 154 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 155 break;
MPPT51 5:cf8183ae80ef 156 case 3:
MPPT51 5:cf8183ae80ef 157 GL_C = 1;
MPPT51 5:cf8183ae80ef 158 GL_B = 0;
MPPT51 5:cf8183ae80ef 159 GL_A = 0;
MPPT51 5:cf8183ae80ef 160 GH_C = 0;
MPPT51 5:cf8183ae80ef 161 GH_B = 0;
MPPT51 5:cf8183ae80ef 162 GH_A = -dutyCycle;
MPPT51 5:cf8183ae80ef 163 break;
MPPT51 5:cf8183ae80ef 164 case 4:
MPPT51 5:cf8183ae80ef 165 GL_C = 0;
MPPT51 5:cf8183ae80ef 166 GL_B = 1;
MPPT51 5:cf8183ae80ef 167 GL_A = 0;
MPPT51 5:cf8183ae80ef 168 GH_C = 0;
mslovic 0:5602fba2a7f7 169 GH_B = 0;
MPPT51 5:cf8183ae80ef 170 GH_A = -dutyCycle;
MPPT51 5:cf8183ae80ef 171 break;
MPPT51 5:cf8183ae80ef 172 case 5:
MPPT51 5:cf8183ae80ef 173 GL_C = 0;
MPPT51 5:cf8183ae80ef 174 GL_B = 1;
MPPT51 5:cf8183ae80ef 175 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 176 GH_C = -dutyCycle;
MPPT51 5:cf8183ae80ef 177 GH_B = 0;
mslovic 0:5602fba2a7f7 178 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 179 break;
mslovic 3:a4b4a8e3f2a0 180 }
mslovic 3:a4b4a8e3f2a0 181 }else {
mslovic 3:a4b4a8e3f2a0 182 coast();
mslovic 0:5602fba2a7f7 183 }
mslovic 0:5602fba2a7f7 184 }
mslovic 0:5602fba2a7f7 185 void BLDCmotorDriver::setDutyCycle(float dc) {
mslovic 0:5602fba2a7f7 186 if (dc >= -1 && dc <= 1) {
mslovic 3:a4b4a8e3f2a0 187 ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
mslovic 0:5602fba2a7f7 188 tempDutyCycle = dc;
mslovic 0:5602fba2a7f7 189 } else {
mslovic 0:5602fba2a7f7 190 coast();
mslovic 0:5602fba2a7f7 191 }
mslovic 0:5602fba2a7f7 192 }
mslovic 3:a4b4a8e3f2a0 193 /*void BLDCmotorDriver::adjustDutyCycle() {
tbjazic 1:786897114846 194 dutyCycle = rl.out(tempDutyCycle);
tbjazic 1:786897114846 195 float diff = tempDutyCycle - dutyCycle;
tbjazic 1:786897114846 196 if (diff < 0.01 || diff > -0.01)
tbjazic 1:786897114846 197 ticker.detach();
mslovic 3:a4b4a8e3f2a0 198 }*/
mslovic 0:5602fba2a7f7 199 void BLDCmotorDriver::coast() {
mslovic 0:5602fba2a7f7 200 GH_A = 0;
mslovic 0:5602fba2a7f7 201 GL_A = 0;
mslovic 0:5602fba2a7f7 202 GH_B = 0;
mslovic 0:5602fba2a7f7 203 GL_B = 0;
mslovic 0:5602fba2a7f7 204 GH_C = 0;
mslovic 0:5602fba2a7f7 205 GL_C = 0;
mslovic 0:5602fba2a7f7 206 dutyCycle = tempDutyCycle = 0;
mslovic 0:5602fba2a7f7 207 rl.reset();
mslovic 0:5602fba2a7f7 208 ticker.detach();
mslovic 0:5602fba2a7f7 209 }
mslovic 0:5602fba2a7f7 210 float BLDCmotorDriver::getDutyCycle() {
mslovic 0:5602fba2a7f7 211 return dutyCycle;
mslovic 0:5602fba2a7f7 212 }