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?
User | Revision | Line number | New 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 | } |