KENYA YANAGIHARA / BLDCmotorDriver

CQ Inverter Kit 2 test

BLDCmotorDriver.cpp@5:35a85d9524f1, 2020-09-07 (annotated)

Committer:
kenya8787
Date:
Mon Sep 07 08:29:40 2020 +0000
Revision:
5:35a85d9524f1
Parent:
3:a4b4a8e3f2a0 
for test CQ KIT Rect 120deg

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