BLDCmotorDriver.cpp

00001 #include "BLDCmotorDriver.h"
00002 
00003 BLDCmotorDriver::BLDCmotorDriver(PinName pGH_A, PinName pGH_B, PinName pGH_C, PinName pGL_A, PinName pGL_B, PinName pGL_C,
00004                     PinName pH1, PinName pH2, PinName pH3, PinName pFault) : 
00005                     GH_A(pGH_A), GH_B(pGH_B), GH_C(pGH_C), GL_A(pGL_A), GL_B(pGL_B), GL_C(pGL_C),
00006                     H1(pH1), H2(pH2), H3(pH3), Fault(LED1){
00007                      
00008     sampleTime = 1e-3;
00009     switchingPeriod = 1.0 / 20e3;
00010     dutyCycle = tempDutyCycle = 0;
00011     GH_A.period(switchingPeriod); // applies to all PwmOut instances
00012     rl.setLimits(0.1, -0.1, 0, sampleTime); // initial 10 second ramp
00013     //H1.mode(PullNone);
00014     //H2.mode(PullNone);
00015     //H3.mode(PullNone);
00016     H1.rise(this, &BLDCmotorDriver::commutation);
00017     H2.rise(this, &BLDCmotorDriver::commutation);
00018     H3.rise(this, &BLDCmotorDriver::commutation);
00019     H1.fall(this, &BLDCmotorDriver::commutation);
00020     H2.fall(this, &BLDCmotorDriver::commutation);
00021     H3.fall(this, &BLDCmotorDriver::commutation);
00022 }
00023 void BLDCmotorDriver::configure(float sampleTime, float switchingFrequency, float rampUpSlope, float rampDownSlope) {
00024     if (sampleTime < 1e-6)
00025         sampleTime = 1e-3;
00026     if (switchingFrequency < 100)
00027         switchingFrequency = 20e3;
00028     if (rampUpSlope < 0 || rampUpSlope > 1)
00029         rampUpSlope = 0.1;
00030     if (rampDownSlope > 0 || rampDownSlope < -1)
00031         rampDownSlope = -0.1;
00032     this->sampleTime = sampleTime;
00033     switchingPeriod = 1.0 / switchingFrequency;
00034     rl.setLimits(rampUpSlope, rampDownSlope, 0, sampleTime);
00035 }
00036 int BLDCmotorDriver::getSector(){           // hall 120°
00037     
00038     h1 = H1.read();  
00039     h2 = H2.read();
00040     h3 = H3.read();
00041     
00042      
00043     if(h1 == 0 && h2 == 0 && h3 == 1){          
00044             _currentSector = 0;
00045              }    
00046     else if(h1 == 0 && h2 == 1 && h3 == 1){
00047             _currentSector = 1;        
00048             }        
00049     else if(h1 == 0 && h2 == 1 && h3 == 0){  
00050             _currentSector = 2;       
00051             }  
00052     else if(h1 == 1 && h2 == 1 && h3 == 0){
00053             _currentSector = 3;
00054             } 
00055     else if(h1 == 1 && h2 == 0 && h3 == 0){
00056             _currentSector = 4;
00057             }
00058     else if(h1 == 1 && h2 == 0 && h3 == 1){
00059             _currentSector = 5;
00060             }           
00061      previousSector = _currentSector - 1;
00062      difference = _currentSector - previousSector;
00063      if (difference == 1){
00064        currentSector = _currentSector;
00065        Fault = 0;
00066        }
00067      else Fault = 1; 
00068 return currentSector;                
00069 }  
00070 
00071 void BLDCmotorDriver::commutation()  {
00072  dutyCycle = rl.out(tempDutyCycle);
00073  currentSector = getSector();
00074     if (dutyCycle > 0) {
00075         currentSector++;
00076         if(currentSector > 5)currentSector = 0;
00077         switch(currentSector) {  
00078         
00079           case 0:               //001     
00080                 GH_A = dutyCycle;
00081                 GL_A = 0;
00082                 GH_B = 0;
00083                 GL_B = 0;
00084                 GH_C = 0;
00085                 GL_C = 1;
00086                 break;
00087            case 1:            
00088                 GH_A = 0;
00089                 GL_A = 0;
00090                 GH_B = dutyCycle;
00091                 GL_B = 0;
00092                 GH_C = 0;
00093                 GL_C = 1; 
00094                 break;
00095            case 2:             
00096                 GH_A = 0;
00097                 GL_A = 1;
00098                 GH_B = dutyCycle;
00099                 GL_B = 0;
00100                 GH_C = 0;
00101                 GL_C = 0;
00102                 break;
00103             case 3:             
00104                 GH_A = 0;
00105                 GL_A = 1;
00106                 GH_B = 0;
00107                 GL_B = 0;
00108                 GH_C = dutyCycle;
00109                 GL_C = 0;
00110                 break;
00111             case 4:              
00112                 GH_A = 0;
00113                 GL_A = 0;
00114                 GH_B = 0;
00115                 GL_B = 1;
00116                 GH_C = dutyCycle;
00117                 GL_C = 0; 
00118                 break;    
00119             case 5:              
00120                 GH_A = dutyCycle;
00121                 GL_A = 0;
00122                 GH_B = 0;
00123                 GL_B = 1;
00124                 GH_C = 0;
00125                 GL_C = 0; 
00126                 break;
00127         }
00128     } else if (dutyCycle < 0) { 
00129         currentSector--;
00130         if(currentSector < 0)currentSector = 5;
00131         switch(currentSector) {
00132           case 0:              
00133                 GH_A = -dutyCycle;
00134                 GL_A = 0;
00135                 GH_B = 0;
00136                 GL_B = 1;
00137                 GH_C = 0;
00138                 GL_C = 0; 
00139                 break;
00140           case 1:                 
00141                 GH_A = -dutyCycle;
00142                 GL_A = 0;
00143                 GH_B = 0;
00144                 GL_B = 0;
00145                 GH_C = 0;
00146                 GL_C = 1;
00147                 break;
00148            case 2:            
00149                 GH_A = 0;
00150                 GL_A = 0;
00151                 GH_B = -dutyCycle;
00152                 GL_B = 0;
00153                 GH_C = 0;
00154                 GL_C = 1; 
00155                 break;
00156            case 3:             
00157                 GH_A = 0;
00158                 GL_A = 1;
00159                 GH_B = -dutyCycle;
00160                 GL_B = 0;
00161                 GH_C = 0;
00162                 GL_C = 0;
00163                 break;
00164             case 4:             
00165                 GH_A = 0;
00166                 GL_A = 1;
00167                 GH_B = 0;
00168                 GL_B = 0;
00169                 GH_C = -dutyCycle;
00170                 GL_C = 0;
00171                 break;
00172             case 5:              
00173                 GH_A = 0;
00174                 GL_A = 0;
00175                 GH_B = 0;
00176                 GL_B = 1;
00177                 GH_C = -dutyCycle;
00178                 GL_C = 0; 
00179                 break;    
00180                 }                
00181         }else {
00182         coast();
00183     }
00184 }
00185 void BLDCmotorDriver::setDutyCycle(float dc) {
00186     if (dc >= -1 && dc <= 1) {
00187         ticker.attach(this, &BLDCmotorDriver::commutation, sampleTime);
00188         tempDutyCycle = dc;
00189     } else {
00190         coast();
00191     }
00192 }
00193 /*void BLDCmotorDriver::adjustDutyCycle() {
00194     dutyCycle = rl.out(tempDutyCycle);
00195     float diff = tempDutyCycle - dutyCycle;
00196     if (diff < 0.01 || diff > -0.01)
00197         ticker.detach();
00198 }*/
00199 void BLDCmotorDriver::coast() {
00200     GH_A = 0;
00201     GL_A = 0;
00202     GH_B = 0;
00203     GL_B = 0;
00204     GH_C = 0;
00205     GL_C = 0;
00206     dutyCycle = tempDutyCycle = 0;
00207     rl.reset();
00208     ticker.detach();
00209 }
00210 float BLDCmotorDriver::getDutyCycle() {
00211     return dutyCycle;
00212 }