Koichi Kikkawa / BLDCmotorDriver

Motacon_20200317

Dependents:   HelloWorld_MotorKaisei

BLDCmotorDriver.cpp@6:959e725b6be9, 2020-03-14 (annotated)

Committer:
MPPT51
Date:
Sat Mar 14 00:56:54 2020 +0000
Revision:
6:959e725b6be9
Parent:
5:cf8183ae80ef
Child:
7:fb9e3b508237
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;
MPPT51 6:959e725b6be9 11 GL_A.period(switchingPeriod); // applies to all PwmOut instances
MPPT51 6:959e725b6be9 12 rl.setLimits(0.1, -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 h1 = H1.read();
mslovic 3:a4b4a8e3f2a0 38 h2 = H2.read();
mslovic 3:a4b4a8e3f2a0 39 h3 = H3.read();
mslovic 3:a4b4a8e3f2a0 40 if(h1 == 0 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 41 _currentSector = 0;
mslovic 3:a4b4a8e3f2a0 42 }
mslovic 3:a4b4a8e3f2a0 43 else if(h1 == 0 && h2 == 1 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 44 _currentSector = 1;
mslovic 3:a4b4a8e3f2a0 45 }
mslovic 3:a4b4a8e3f2a0 46 else if(h1 == 0 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 47 _currentSector = 2;
mslovic 3:a4b4a8e3f2a0 48 }
mslovic 3:a4b4a8e3f2a0 49 else if(h1 == 1 && h2 == 1 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 50 _currentSector = 3;
mslovic 3:a4b4a8e3f2a0 51 }
mslovic 3:a4b4a8e3f2a0 52 else if(h1 == 1 && h2 == 0 && h3 == 0){
mslovic 3:a4b4a8e3f2a0 53 _currentSector = 4;
mslovic 3:a4b4a8e3f2a0 54 }
mslovic 3:a4b4a8e3f2a0 55 else if(h1 == 1 && h2 == 0 && h3 == 1){
mslovic 3:a4b4a8e3f2a0 56 _currentSector = 5;
mslovic 3:a4b4a8e3f2a0 57 }
MPPT51 6:959e725b6be9 58 previousSector = _currentSector - 1;
MPPT51 6:959e725b6be9 59 difference = _currentSector - previousSector;
MPPT51 6:959e725b6be9 60 if (difference == 1){
MPPT51 6:959e725b6be9 61 currentSector = _currentSector;
MPPT51 6:959e725b6be9 62 Fault = 0;
MPPT51 6:959e725b6be9 63 }
MPPT51 6:959e725b6be9 64 else{
MPPT51 6:959e725b6be9 65 Fault = 1;
MPPT51 6:959e725b6be9 66 }
MPPT51 6:959e725b6be9 67 return currentSector;
mslovic 3:a4b4a8e3f2a0 68 }
tbjazic 1:786897114846 69
mslovic 3:a4b4a8e3f2a0 70 void BLDCmotorDriver::commutation() {
MPPT51 6:959e725b6be9 71 dutyCycle = rl.out(tempDutyCycle);
MPPT51 6:959e725b6be9 72 currentSector = getSector();
mslovic 0:5602fba2a7f7 73 if (dutyCycle > 0) {
mslovic 3:a4b4a8e3f2a0 74 currentSector++;
MPPT51 6:959e725b6be9 75 if(currentSector > 5){
MPPT51 6:959e725b6be9 76 currentSector = 0;
MPPT51 6:959e725b6be9 77 }
MPPT51 6:959e725b6be9 78 switch(currentSector) {
MPPT51 6:959e725b6be9 79 case 0: //001
MPPT51 5:cf8183ae80ef 80 GL_C = 0;
mslovic 0:5602fba2a7f7 81 GL_B = 0;
MPPT51 6:959e725b6be9 82 GL_A = dutyCycle;
mslovic 0:5602fba2a7f7 83 GH_C = 0;
MPPT51 6:959e725b6be9 84 GH_B = 1;
MPPT51 5:cf8183ae80ef 85 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 86 break;
MPPT51 6:959e725b6be9 87 case 1:
MPPT51 6:959e725b6be9 88 GL_C = dutyCycle;
MPPT51 5:cf8183ae80ef 89 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 90 GL_A = 0;
MPPT51 5:cf8183ae80ef 91 GH_C = 0;
MPPT51 6:959e725b6be9 92 GH_B = 1;
MPPT51 5:cf8183ae80ef 93 GH_A = 0;
mslovic 0:5602fba2a7f7 94 break;
MPPT51 6:959e725b6be9 95 case 2:
MPPT51 6:959e725b6be9 96 GL_C = dutyCycle;
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 6:959e725b6be9 101 GH_A = 1;
mslovic 0:5602fba2a7f7 102 break;
mslovic 3:a4b4a8e3f2a0 103 case 3:
MPPT51 5:cf8183ae80ef 104 GL_C = 0;
MPPT51 6:959e725b6be9 105 GL_B = dutyCycle;
MPPT51 5:cf8183ae80ef 106 GL_A = 0;
MPPT51 5:cf8183ae80ef 107 GH_C = 0;
mslovic 0:5602fba2a7f7 108 GH_B = 0;
MPPT51 6:959e725b6be9 109 GH_A = 1;
mslovic 0:5602fba2a7f7 110 break;
mslovic 3:a4b4a8e3f2a0 111 case 4:
MPPT51 5:cf8183ae80ef 112 GL_C = 0;
MPPT51 6:959e725b6be9 113 GL_B = dutyCycle;
mslovic 0:5602fba2a7f7 114 GL_A = 0;
MPPT51 6:959e725b6be9 115 GH_C = 1;
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 6:959e725b6be9 122 GL_A = dutyCycle;
MPPT51 6:959e725b6be9 123 GH_C = 1;
mslovic 3:a4b4a8e3f2a0 124 GH_B = 0;
MPPT51 5:cf8183ae80ef 125 GH_A = 0;
mslovic 0:5602fba2a7f7 126 break;
mslovic 3:a4b4a8e3f2a0 127 }
MPPT51 6:959e725b6be9 128 }
MPPT51 6:959e725b6be9 129 else if (dutyCycle < 0) {
mslovic 3:a4b4a8e3f2a0 130 currentSector--;
MPPT51 6:959e725b6be9 131 if(currentSector < 0){
MPPT51 6:959e725b6be9 132 currentSector = 5;
MPPT51 6:959e725b6be9 133 }
mslovic 3:a4b4a8e3f2a0 134 switch(currentSector) {
MPPT51 6:959e725b6be9 135 case 0:
MPPT51 6:959e725b6be9 136 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 137 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 138 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 139 GH_C = 0;
MPPT51 6:959e725b6be9 140 GH_B = 0;
MPPT51 6:959e725b6be9 141 GH_A = 0;
MPPT51 6:959e725b6be9 142 break;
MPPT51 6:959e725b6be9 143 case 1:
MPPT51 6:959e725b6be9 144 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 145 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 146 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 147 GH_C = 0;
MPPT51 6:959e725b6be9 148 GH_B = 0;
MPPT51 6:959e725b6be9 149 GH_A = 0;
MPPT51 6:959e725b6be9 150 break;
MPPT51 6:959e725b6be9 151 case 2:
MPPT51 6:959e725b6be9 152 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 153 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 154 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 155 GH_C = 0;
MPPT51 6:959e725b6be9 156 GH_B = 0;
MPPT51 6:959e725b6be9 157 GH_A = 0;
MPPT51 6:959e725b6be9 158 break;
MPPT51 6:959e725b6be9 159 case 3:
MPPT51 6:959e725b6be9 160 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 161 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 162 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 163 GH_C = 0;
MPPT51 6:959e725b6be9 164 GH_B = 0;
MPPT51 6:959e725b6be9 165 GH_A = 0;
MPPT51 6:959e725b6be9 166 break;
MPPT51 6:959e725b6be9 167 case 4:
MPPT51 6:959e725b6be9 168 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 169 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 170 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 171 GH_C = 0;
MPPT51 6:959e725b6be9 172 GH_B = 0;
MPPT51 6:959e725b6be9 173 GH_A = 0;
MPPT51 6:959e725b6be9 174 break;
MPPT51 6:959e725b6be9 175 case 5:
MPPT51 6:959e725b6be9 176 GL_C = -dutyCycle;
MPPT51 6:959e725b6be9 177 GL_B = -dutyCycle;
MPPT51 6:959e725b6be9 178 GL_A = -dutyCycle;
MPPT51 6:959e725b6be9 179 GH_C = 0;
MPPT51 6:959e725b6be9 180 GH_B = 0;
MPPT51 6:959e725b6be9 181 GH_A = 0;
MPPT51 6:959e725b6be9 182 break;
MPPT51 6:959e725b6be9 183 /*
MPPT51 6:959e725b6be9 184 case 0:
MPPT51 5:cf8183ae80ef 185 GL_C = 0;
MPPT51 5:cf8183ae80ef 186 GL_B = 0;
MPPT51 5:cf8183ae80ef 187 GL_A = 1;
MPPT51 5:cf8183ae80ef 188 GH_C = -dutyCycle;
MPPT51 5:cf8183ae80ef 189 GH_B = 0;
MPPT51 5:cf8183ae80ef 190 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 191 break;
mslovic 3:a4b4a8e3f2a0 192 case 1:
MPPT51 5:cf8183ae80ef 193 GL_C = 0;
mslovic 3:a4b4a8e3f2a0 194 GL_B = 0;
MPPT51 5:cf8183ae80ef 195 GL_A = 1;
mslovic 3:a4b4a8e3f2a0 196 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 197 GH_B = -dutyCycle;
MPPT51 5:cf8183ae80ef 198 GH_A = 0;
MPPT51 5:cf8183ae80ef 199 break;
MPPT51 5:cf8183ae80ef 200 case 2:
MPPT51 5:cf8183ae80ef 201 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 202 GL_B = 0;
MPPT51 5:cf8183ae80ef 203 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 204 GH_C = 0;
MPPT51 5:cf8183ae80ef 205 GH_B = -dutyCycle;
MPPT51 5:cf8183ae80ef 206 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 207 break;
MPPT51 5:cf8183ae80ef 208 case 3:
MPPT51 5:cf8183ae80ef 209 GL_C = 1;
MPPT51 5:cf8183ae80ef 210 GL_B = 0;
MPPT51 5:cf8183ae80ef 211 GL_A = 0;
MPPT51 5:cf8183ae80ef 212 GH_C = 0;
MPPT51 5:cf8183ae80ef 213 GH_B = 0;
MPPT51 5:cf8183ae80ef 214 GH_A = -dutyCycle;
MPPT51 5:cf8183ae80ef 215 break;
MPPT51 5:cf8183ae80ef 216 case 4:
MPPT51 5:cf8183ae80ef 217 GL_C = 0;
MPPT51 5:cf8183ae80ef 218 GL_B = 1;
MPPT51 5:cf8183ae80ef 219 GL_A = 0;
MPPT51 5:cf8183ae80ef 220 GH_C = 0;
mslovic 0:5602fba2a7f7 221 GH_B = 0;
MPPT51 5:cf8183ae80ef 222 GH_A = -dutyCycle;
MPPT51 5:cf8183ae80ef 223 break;
MPPT51 5:cf8183ae80ef 224 case 5:
MPPT51 5:cf8183ae80ef 225 GL_C = 0;
MPPT51 5:cf8183ae80ef 226 GL_B = 1;
MPPT51 5:cf8183ae80ef 227 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 228 GH_C = -dutyCycle;
MPPT51 5:cf8183ae80ef 229 GH_B = 0;
mslovic 0:5602fba2a7f7 230 GH_A = 0;
MPPT51 6:959e725b6be9 231 break;
MPPT51 6:959e725b6be9 232 */
MPPT51 6:959e725b6be9 233 }
MPPT51 6:959e725b6be9 234 }
MPPT51 6:959e725b6be9 235 else {
mslovic 3:a4b4a8e3f2a0 236 coast();
mslovic 0:5602fba2a7f7 237 }
mslovic 0:5602fba2a7f7 238 }
mslovic 0:5602fba2a7f7 239 void BLDCmotorDriver::setDutyCycle(float dc) {
mslovic 0:5602fba2a7f7 240 if (dc >= -1 && dc <= 1) {
mslovic 3:a4b4a8e3f2a0 241 ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
mslovic 0:5602fba2a7f7 242 tempDutyCycle = dc;
mslovic 0:5602fba2a7f7 243 } else {
mslovic 0:5602fba2a7f7 244 coast();
mslovic 0:5602fba2a7f7 245 }
mslovic 0:5602fba2a7f7 246 }
mslovic 3:a4b4a8e3f2a0 247 /*void BLDCmotorDriver::adjustDutyCycle() {
tbjazic 1:786897114846 248 dutyCycle = rl.out(tempDutyCycle);
tbjazic 1:786897114846 249 float diff = tempDutyCycle - dutyCycle;
tbjazic 1:786897114846 250 if (diff < 0.01 || diff > -0.01)
tbjazic 1:786897114846 251 ticker.detach();
mslovic 3:a4b4a8e3f2a0 252 }*/
mslovic 0:5602fba2a7f7 253 void BLDCmotorDriver::coast() {
mslovic 0:5602fba2a7f7 254 GH_A = 0;
mslovic 0:5602fba2a7f7 255 GL_A = 0;
mslovic 0:5602fba2a7f7 256 GH_B = 0;
mslovic 0:5602fba2a7f7 257 GL_B = 0;
mslovic 0:5602fba2a7f7 258 GH_C = 0;
mslovic 0:5602fba2a7f7 259 GL_C = 0;
mslovic 0:5602fba2a7f7 260 dutyCycle = tempDutyCycle = 0;
mslovic 0:5602fba2a7f7 261 rl.reset();
mslovic 0:5602fba2a7f7 262 ticker.detach();
mslovic 0:5602fba2a7f7 263 }
mslovic 0:5602fba2a7f7 264 float BLDCmotorDriver::getDutyCycle() {
mslovic 0:5602fba2a7f7 265 return dutyCycle;
mslovic 0:5602fba2a7f7 266 }