Koichi Kikkawa / BLDCmotorDriver_20200821_motacon_ver4

20200821_motacon_ver4

BLDCmotorDriver.cpp@3:a4b4a8e3f2a0, 2015-06-01 (annotated)

Committer:
mslovic
Date:
Mon Jun 01 13:47:23 2015 +0000
Revision:
3:a4b4a8e3f2a0
Parent:
1:786897114846
Child:
5:cf8183ae80ef
First working version

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
mslovic 0:5602fba2a7f7 12 rl.setLimits(0.1, -0.1, 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
mslovic 0:5602fba2a7f7 80 GH_A = dutyCycle;
mslovic 0:5602fba2a7f7 81 GL_A = 0;
mslovic 0:5602fba2a7f7 82 GH_B = 0;
mslovic 0:5602fba2a7f7 83 GL_B = 0;
mslovic 0:5602fba2a7f7 84 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 85 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 86 break;
mslovic 3:a4b4a8e3f2a0 87 case 1:
mslovic 3:a4b4a8e3f2a0 88 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 89 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 90 GH_B = dutyCycle;
mslovic 3:a4b4a8e3f2a0 91 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 92 GH_C = 0;
mslovic 0:5602fba2a7f7 93 GL_C = 1;
mslovic 0:5602fba2a7f7 94 break;
mslovic 3:a4b4a8e3f2a0 95 case 2:
mslovic 3:a4b4a8e3f2a0 96 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 97 GL_A = 1;
mslovic 3:a4b4a8e3f2a0 98 GH_B = dutyCycle;
mslovic 3:a4b4a8e3f2a0 99 GL_B = 0;
mslovic 0:5602fba2a7f7 100 GH_C = 0;
mslovic 0:5602fba2a7f7 101 GL_C = 0;
mslovic 0:5602fba2a7f7 102 break;
mslovic 3:a4b4a8e3f2a0 103 case 3:
mslovic 0:5602fba2a7f7 104 GH_A = 0;
mslovic 0:5602fba2a7f7 105 GL_A = 1;
mslovic 0:5602fba2a7f7 106 GH_B = 0;
mslovic 0:5602fba2a7f7 107 GL_B = 0;
mslovic 0:5602fba2a7f7 108 GH_C = dutyCycle;
mslovic 0:5602fba2a7f7 109 GL_C = 0;
mslovic 0:5602fba2a7f7 110 break;
mslovic 3:a4b4a8e3f2a0 111 case 4:
mslovic 0:5602fba2a7f7 112 GH_A = 0;
mslovic 0:5602fba2a7f7 113 GL_A = 0;
mslovic 0:5602fba2a7f7 114 GH_B = 0;
mslovic 0:5602fba2a7f7 115 GL_B = 1;
mslovic 0:5602fba2a7f7 116 GH_C = dutyCycle;
mslovic 0:5602fba2a7f7 117 GL_C = 0;
mslovic 3:a4b4a8e3f2a0 118 break;
mslovic 3:a4b4a8e3f2a0 119 case 5:
mslovic 3:a4b4a8e3f2a0 120 GH_A = dutyCycle;
mslovic 3:a4b4a8e3f2a0 121 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 122 GH_B = 0;
mslovic 3:a4b4a8e3f2a0 123 GL_B = 1;
mslovic 3:a4b4a8e3f2a0 124 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 125 GL_C = 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) {
mslovic 3:a4b4a8e3f2a0 132 case 0:
mslovic 3:a4b4a8e3f2a0 133 GH_A = -dutyCycle;
mslovic 3:a4b4a8e3f2a0 134 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 135 GH_B = 0;
mslovic 3:a4b4a8e3f2a0 136 GL_B = 1;
mslovic 3:a4b4a8e3f2a0 137 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 138 GL_C = 0;
mslovic 3:a4b4a8e3f2a0 139 break;
mslovic 3:a4b4a8e3f2a0 140 case 1:
mslovic 3:a4b4a8e3f2a0 141 GH_A = -dutyCycle;
mslovic 3:a4b4a8e3f2a0 142 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 143 GH_B = 0;
mslovic 3:a4b4a8e3f2a0 144 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 145 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 146 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 147 break;
mslovic 3:a4b4a8e3f2a0 148 case 2:
mslovic 3:a4b4a8e3f2a0 149 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 150 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 151 GH_B = -dutyCycle;
mslovic 3:a4b4a8e3f2a0 152 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 153 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 154 GL_C = 1;
mslovic 3:a4b4a8e3f2a0 155 break;
mslovic 3:a4b4a8e3f2a0 156 case 3:
mslovic 3:a4b4a8e3f2a0 157 GH_A = 0;
mslovic 3:a4b4a8e3f2a0 158 GL_A = 1;
mslovic 3:a4b4a8e3f2a0 159 GH_B = -dutyCycle;
mslovic 3:a4b4a8e3f2a0 160 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 161 GH_C = 0;
mslovic 3:a4b4a8e3f2a0 162 GL_C = 0;
mslovic 3:a4b4a8e3f2a0 163 break;
mslovic 3:a4b4a8e3f2a0 164 case 4:
mslovic 0:5602fba2a7f7 165 GH_A = 0;
mslovic 0:5602fba2a7f7 166 GL_A = 1;
mslovic 0:5602fba2a7f7 167 GH_B = 0;
mslovic 0:5602fba2a7f7 168 GL_B = 0;
mslovic 3:a4b4a8e3f2a0 169 GH_C = -dutyCycle;
mslovic 0:5602fba2a7f7 170 GL_C = 0;
mslovic 0:5602fba2a7f7 171 break;
mslovic 3:a4b4a8e3f2a0 172 case 5:
mslovic 0:5602fba2a7f7 173 GH_A = 0;
mslovic 0:5602fba2a7f7 174 GL_A = 0;
mslovic 3:a4b4a8e3f2a0 175 GH_B = 0;
mslovic 3:a4b4a8e3f2a0 176 GL_B = 1;
mslovic 3:a4b4a8e3f2a0 177 GH_C = -dutyCycle;
mslovic 3:a4b4a8e3f2a0 178 GL_C = 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 }