Koichi Kikkawa / BLDCmotorDriver_20200821_motacon_ver4

20200821_motacon_ver4

BLDCmotorDriver.cpp@2:7aae78b85e1d, 2015-06-01 (annotated)

Committer:
mslovic
Date:
Mon Jun 01 13:40:39 2015 +0000
Revision:
2:7aae78b85e1d
Parent:
0:5602fba2a7f7 
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
mslovic 2:7aae78b85e1d 3 BLDCmotorDriver::BLDCmotorDriver(PinName pGH_A, PinName pGH_B, PinName pGH_C, PinName pGL_A, PinName pGL_B, PinName pGL_C,
mslovic 2:7aae78b85e1d 4 PinName pH1, PinName pH2, PinName pH3, PinName pFault) :
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 6 H1(pH1), H2(pH2), H3(pH3), Fault(LED1){
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 13 //H1.mode(PullNone);
mslovic 2:7aae78b85e1d 14 //H2.mode(PullNone);
mslovic 2:7aae78b85e1d 15 //H3.mode(PullNone);
mslovic 2:7aae78b85e1d 16 H1.rise(this, &BLDCmotorDriver::commutation);
mslovic 2:7aae78b85e1d 17 H2.rise(this, &BLDCmotorDriver::commutation);
mslovic 2:7aae78b85e1d 18 H3.rise(this, &BLDCmotorDriver::commutation);
mslovic 2:7aae78b85e1d 19 H1.fall(this, &BLDCmotorDriver::commutation);
mslovic 2:7aae78b85e1d 20 H2.fall(this, &BLDCmotorDriver::commutation);
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 36 int BLDCmotorDriver::getSector(){ // hall 120°
mslovic 2:7aae78b85e1d 37
mslovic 2:7aae78b85e1d 38 h1 = H1.read();
mslovic 2:7aae78b85e1d 39 h2 = H2.read();
mslovic 2:7aae78b85e1d 40 h3 = H3.read();
mslovic 2:7aae78b85e1d 41
mslovic 2:7aae78b85e1d 42
mslovic 2:7aae78b85e1d 43 if(h1 == 0 && h2 == 0 && h3 == 1){
mslovic 2:7aae78b85e1d 44 _currentSector = 0;
mslovic 2:7aae78b85e1d 45 }
mslovic 2:7aae78b85e1d 46 else if(h1 == 0 && h2 == 1 && h3 == 1){
mslovic 2:7aae78b85e1d 47 _currentSector = 1;
mslovic 2:7aae78b85e1d 48 }
mslovic 2:7aae78b85e1d 49 else if(h1 == 0 && h2 == 1 && h3 == 0){
mslovic 2:7aae78b85e1d 50 _currentSector = 2;
mslovic 2:7aae78b85e1d 51 }
mslovic 2:7aae78b85e1d 52 else if(h1 == 1 && h2 == 1 && h3 == 0){
mslovic 2:7aae78b85e1d 53 _currentSector = 3;
mslovic 2:7aae78b85e1d 54 }
mslovic 2:7aae78b85e1d 55 else if(h1 == 1 && h2 == 0 && h3 == 0){
mslovic 2:7aae78b85e1d 56 _currentSector = 4;
mslovic 2:7aae78b85e1d 57 }
mslovic 2:7aae78b85e1d 58 else if(h1 == 1 && h2 == 0 && h3 == 1){
mslovic 2:7aae78b85e1d 59 _currentSector = 5;
mslovic 2:7aae78b85e1d 60 }
mslovic 2:7aae78b85e1d 61 previousSector = _currentSector - 1;
mslovic 2:7aae78b85e1d 62 difference = _currentSector - previousSector;
mslovic 2:7aae78b85e1d 63 if (difference == 1){
mslovic 2:7aae78b85e1d 64 currentSector = _currentSector;
mslovic 2:7aae78b85e1d 65 Fault = 0;
mslovic 2:7aae78b85e1d 66 }
mslovic 2:7aae78b85e1d 67 else Fault = 1;
mslovic 2:7aae78b85e1d 68 return currentSector;
mslovic 2:7aae78b85e1d 69 }
mslovic 2:7aae78b85e1d 70
mslovic 2:7aae78b85e1d 71 void BLDCmotorDriver::commutation() {
mslovic 2:7aae78b85e1d 72 dutyCycle = rl.out(tempDutyCycle);
mslovic 2:7aae78b85e1d 73 currentSector = getSector();
mslovic 0:5602fba2a7f7 74 if (dutyCycle > 0) {
mslovic 2:7aae78b85e1d 75 currentSector++;
mslovic 2:7aae78b85e1d 76 if(currentSector > 5)currentSector = 0;
mslovic 2:7aae78b85e1d 77 switch(currentSector) {
mslovic 2:7aae78b85e1d 78
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 85 GL_C = 1;
mslovic 2:7aae78b85e1d 86 break;
mslovic 2:7aae78b85e1d 87 case 1:
mslovic 2:7aae78b85e1d 88 GH_A = 0;
mslovic 2:7aae78b85e1d 89 GL_A = 0;
mslovic 2:7aae78b85e1d 90 GH_B = dutyCycle;
mslovic 2:7aae78b85e1d 91 GL_B = 0;
mslovic 2:7aae78b85e1d 92 GH_C = 0;
mslovic 0:5602fba2a7f7 93 GL_C = 1;
mslovic 0:5602fba2a7f7 94 break;
mslovic 2:7aae78b85e1d 95 case 2:
mslovic 2:7aae78b85e1d 96 GH_A = 0;
mslovic 2:7aae78b85e1d 97 GL_A = 1;
mslovic 2:7aae78b85e1d 98 GH_B = dutyCycle;
mslovic 2:7aae78b85e1d 99 GL_B = 0;
mslovic 0:5602fba2a7f7 100 GH_C = 0;
mslovic 0:5602fba2a7f7 101 GL_C = 0;
mslovic 0:5602fba2a7f7 102 break;
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 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 2:7aae78b85e1d 118 break;
mslovic 2:7aae78b85e1d 119 case 5:
mslovic 2:7aae78b85e1d 120 GH_A = dutyCycle;
mslovic 2:7aae78b85e1d 121 GL_A = 0;
mslovic 2:7aae78b85e1d 122 GH_B = 0;
mslovic 2:7aae78b85e1d 123 GL_B = 1;
mslovic 2:7aae78b85e1d 124 GH_C = 0;
mslovic 2:7aae78b85e1d 125 GL_C = 0;
mslovic 0:5602fba2a7f7 126 break;
mslovic 2:7aae78b85e1d 127 }
mslovic 2:7aae78b85e1d 128 } else if (dutyCycle < 0) {
mslovic 2:7aae78b85e1d 129 currentSector--;
mslovic 2:7aae78b85e1d 130 if(currentSector < 0)currentSector = 5;
mslovic 2:7aae78b85e1d 131 switch(currentSector) {
mslovic 2:7aae78b85e1d 132 case 0:
mslovic 2:7aae78b85e1d 133 GH_A = -dutyCycle;
mslovic 2:7aae78b85e1d 134 GL_A = 0;
mslovic 2:7aae78b85e1d 135 GH_B = 0;
mslovic 2:7aae78b85e1d 136 GL_B = 1;
mslovic 2:7aae78b85e1d 137 GH_C = 0;
mslovic 2:7aae78b85e1d 138 GL_C = 0;
mslovic 2:7aae78b85e1d 139 break;
mslovic 2:7aae78b85e1d 140 case 1:
mslovic 2:7aae78b85e1d 141 GH_A = -dutyCycle;
mslovic 2:7aae78b85e1d 142 GL_A = 0;
mslovic 2:7aae78b85e1d 143 GH_B = 0;
mslovic 2:7aae78b85e1d 144 GL_B = 0;
mslovic 2:7aae78b85e1d 145 GH_C = 0;
mslovic 2:7aae78b85e1d 146 GL_C = 1;
mslovic 2:7aae78b85e1d 147 break;
mslovic 2:7aae78b85e1d 148 case 2:
mslovic 2:7aae78b85e1d 149 GH_A = 0;
mslovic 2:7aae78b85e1d 150 GL_A = 0;
mslovic 2:7aae78b85e1d 151 GH_B = -dutyCycle;
mslovic 2:7aae78b85e1d 152 GL_B = 0;
mslovic 2:7aae78b85e1d 153 GH_C = 0;
mslovic 2:7aae78b85e1d 154 GL_C = 1;
mslovic 2:7aae78b85e1d 155 break;
mslovic 2:7aae78b85e1d 156 case 3:
mslovic 2:7aae78b85e1d 157 GH_A = 0;
mslovic 2:7aae78b85e1d 158 GL_A = 1;
mslovic 2:7aae78b85e1d 159 GH_B = -dutyCycle;
mslovic 2:7aae78b85e1d 160 GL_B = 0;
mslovic 2:7aae78b85e1d 161 GH_C = 0;
mslovic 2:7aae78b85e1d 162 GL_C = 0;
mslovic 2:7aae78b85e1d 163 break;
mslovic 2:7aae78b85e1d 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 2:7aae78b85e1d 169 GH_C = -dutyCycle;
mslovic 0:5602fba2a7f7 170 GL_C = 0;
mslovic 0:5602fba2a7f7 171 break;
mslovic 2:7aae78b85e1d 172 case 5:
mslovic 0:5602fba2a7f7 173 GH_A = 0;
mslovic 0:5602fba2a7f7 174 GL_A = 0;
mslovic 2:7aae78b85e1d 175 GH_B = 0;
mslovic 2:7aae78b85e1d 176 GL_B = 1;
mslovic 2:7aae78b85e1d 177 GH_C = -dutyCycle;
mslovic 2:7aae78b85e1d 178 GL_C = 0;
mslovic 2:7aae78b85e1d 179 break;
mslovic 0:5602fba2a7f7 180 }
mslovic 0:5602fba2a7f7 181 }else {
mslovic 0:5602fba2a7f7 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 2:7aae78b85e1d 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 2:7aae78b85e1d 193 /*void BLDCmotorDriver::adjustDutyCycle() {
mslovic 2:7aae78b85e1d 194 dutyCycle = rl.out(tempDutyCycle);
mslovic 2:7aae78b85e1d 195 float diff = tempDutyCycle - dutyCycle;
mslovic 2:7aae78b85e1d 196 if (diff < 0.01 || diff > -0.01)
mslovic 2:7aae78b85e1d 197 ticker.detach();
mslovic 2:7aae78b85e1d 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 }