├── Arduino Uno更新程序方法.pdf ├── DSCN2308.png ├── DSCN2309.png ├── README.md ├── SM-BLDC318H-V1.Sch ├── SM-BLDC318H-V1.pdf ├── SM-BLDC318H-V1_BOM.xlsx ├── SM-BLDC318H-V1成品物料清单(160908).xls ├── SM-BLDC318H说明书.pdf ├── lib ├── libraries.zip └── readme ├── seamabldcallinoneV1.1.ino └── seamabldcallinoneV1.ino /Arduino Uno更新程序方法.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/Arduino Uno更新程序方法.pdf -------------------------------------------------------------------------------- /DSCN2308.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/DSCN2308.png -------------------------------------------------------------------------------- /DSCN2309.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/DSCN2309.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # arduino-bldc 2 | Brushless motor has many advantages, the development of a brushless motor controller requires a lot of knowledge and experience 3 | the project uses Arduino UNO( atmega328) as the basis for development, the controller is characterized by: 4 | 5 | 1, support for a sensored or sensorless bldc motor 6 | 7 | 2, support for open loop and closed loop 8 | 9 | 3, software and hardware current protection 10 | 11 | 4, serial communication 12 | 13 | 5，Voltage：12-48V 14 | 15 | 6，Current：rated 10A / max 20A 16 | 17 | more： 18 | http://www.seama.com.cn 19 | 20 | 本无刷电机驱动器采用开源硬件，经过自己的测试和验证，是学习无刷电机控制器的入门好教材 21 | 特点： 22 | 23 | 1，主控MCU采用ATMEGA328; 24 | 25 | 2，电路采用原装ST场效应管,80V110A（STP110N8F6）；额定10A/最大20A 26 | 27 | 3，原装IR作预驱； 28 | 29 | 4，采用原装DC-DC降压芯片，可在12-48V内工作； 30 | 31 | 5，具有高压大电流、低导通内阻等特点； 32 | 33 | 6，具有上电自检、欠压/过压、过流、堵转、霍尔相序检测等多重保护功能； 34 | 35 | 7，支持有感无感无刷电机驱动。 36 | 37 | contact： 38 | qq群: 218193115 39 | www.seama.com.cn 40 | -------------------------------------------------------------------------------- /SM-BLDC318H-V1.Sch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/SM-BLDC318H-V1.Sch -------------------------------------------------------------------------------- /SM-BLDC318H-V1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/SM-BLDC318H-V1.pdf -------------------------------------------------------------------------------- /SM-BLDC318H-V1_BOM.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/SM-BLDC318H-V1_BOM.xlsx -------------------------------------------------------------------------------- /SM-BLDC318H-V1成品物料清单(160908).xls: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/SM-BLDC318H-V1成品物料清单(160908).xls -------------------------------------------------------------------------------- /SM-BLDC318H说明书.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/SM-BLDC318H说明书.pdf -------------------------------------------------------------------------------- /lib/libraries.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seamatech/arduino-bldc/f9de8a2ed79ca4a50f2e85306a3489dd3d55696c/lib/libraries.zip -------------------------------------------------------------------------------- /lib/readme: -------------------------------------------------------------------------------- 1 | Download the zip file and extend the files to arduino library.(C:\Users\Administrator\Documents\Arduino\libraries) 2 | the program need the pwm and flextimer2 lib. 3 | 4 | 下载压缩包并解压缩复制lib文件到arduino的库文件夹里（C:\Users\Administrator\Documents\Arduino\libraries） 5 | -------------------------------------------------------------------------------- /seamabldcallinoneV1.1.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //sema bldc v1.0 6 | //we use pwm lib for high speed pwm 7 | //we use flexitimer2 for detect the hall sensor 8 | //The MosFET status: 9 | //1-UH(Pin7) 3-VH(Pin8) 5-WH;(Pin9) 10 | //2-UL(Pin3) 4-VL(Pin4) 6-WL;(Pin6) 11 | #define MosPin_RUN1 digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0) //101 CW Hall=5; (1,4) 10-01-00 U-V-W AB 12 | #define MosPin_RUN2 digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1) //100 CW Hall=4; (1,6) 10-00-01 U-V-W AC 13 | #define MosPin_RUN3 digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1) //110 CW Hall=6; (3,6) 00-10-01 U-V-W BC 14 | #define MosPin_RUN4 digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0) //010 CW Hall=2; (3,2) 01-10-00 U-V-W BA 15 | #define MosPin_RUN5 digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0) //011 CW Hall=3; (5,2) 01-00-10 U-V-W CA 16 | #define MosPin_RUN6 digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0) //001 CW Hall=1; (5,4) 00-10-01 U-V-W CB 17 | #define MosPin_STOP digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0) //All MosFet Close 00-00-00 U-V-W 18 | #define MosPin_BRK digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,1);digitalWrite(6,1) //The Low MosFet Open 01-01-01 U-V-W 19 | 20 | #define _MotorPhasePairs 5 21 | #define _UpdatePWMTime 1 //The Value more big, the motor will start up more slow. 22 | #define _PWMUpStep 2 23 | #define _PWMDownStep 1 24 | #define _DelayTimes 1 25 | #define _UpOffSetDelayTime 10 26 | #define _MaxBRKTimes 1000 27 | #define _MaxPinChangeTime 500 28 | #define _KeyMaxCount 20 29 | 30 | #define _MaxIshunt 500 //Limit Normal run current about 12A ( the resistance value 10momega) 31 | #define _MaxUpIshunt 580 //Limit Startup Current about 3A 32 | #define _MaxMaxIshunt 780 //Limit Peeak Current about 16A 33 | #define _MaxIshuntCheck 220 //Check the mosfet status. Limit the current. 34 | #define _MaxIshuntTime 2 35 | 36 | #define _MaxUpPWM 900 //Limit Startup the PWM 37 | #define _MaxLocationPWM 150 //Location the rotor position. 38 | #define _MaxLocationTime 100 39 | #define _MaxMotorUpStep 5 40 | 41 | #define _BEMFLow 0 42 | #define _BEMFHigh 1 43 | 44 | #define _MaxUpdateStallTime 20000 //The motor stall time 45 | #define _MaxSensorLessUpdateStallTime 5000 //The sensorless time 46 | 47 | #define M_LoopRun 0 48 | #define M_KP 8 49 | #define M_KI 2 50 | #define M_KD 0 51 | #define M_Scale 30 52 | #define M_MaxMaxPWM 1016 53 | #define M_MinMinPWM 200 54 | #define M_MaxSpeed 3000 55 | #define M_MinSpeed 200 56 | 57 | //int UpDelayTime[]= {2000,1900,1800,1600,1400,1200,1000,800,600,550,500,480,460,440,420,400,380,360,355,350,345,340,335,330,325,320,315,310,305,300,298,292}; 58 | int UpDelayTime[]= {50,50,50,49,49,49,43,42,40,40,39,39,38,38,37,37,36,35,35,34,34,34,33,33,32,31,31,30}; 59 | //int UpDelayTime[]= {1650,1600,1550,1500,1500,1450,1450,1400,1400,1400,1380,1370,1360,1360,1355,1355,1350,1350,1340,1340,1330,1330,1320,1320,1320,1320,1320,1310}; 60 | int frequency = 20000; //pwm frequency in Hz 16khz 61 | int HallValue,LastHallValue,RunPin,DirPin,BEMFPinA,BEMFPinB,BEMFPinC; 62 | int RunEnable=1,DirStatus=1; 63 | int ReadPoint,NowPwm=0; 64 | unsigned int UpdatePwm=0; 65 | unsigned int UpdateStallTime=0; 66 | int OutStatus, Ishunt,AvgIshunt=0,SubIshunt,ErrIshunt,CountErr=0,Error=0,HallErr=0,CountRun=0,RunStatus=0,CurrentDirect; 67 | int RunPinChangeCount=0,DirPinChangeCount=0; 68 | int FlashBegan,FlashErr; 69 | unsigned long AvgMotorStepTime,MotorStepTime,LastStepTime,ThisFlashTime,lastrtime; 70 | unsigned long BEMFTimeStamp,LastRunCycleTime,RunTime; 71 | int MotorPhase,LastPhase,SensorLess=0; 72 | int Hall_1=0,Hall_2=0,Hall_3=0,Hall_4=0,Hall_5=0,Hall_6=0,RightRunStatus=0,SensorLessSound=1; 73 | int MotorActualSpeed,TargetSpeed; 74 | int EK, EK1,EK2,CEK,CEK1,CEK2,YK,DealLoop; 75 | float EnvTemperature; 76 | 77 | void setup(){ 78 | Serial.begin(9600); 79 | pinMode(2, INPUT); //Over Current for Hardware 硬件保护 80 | pinMode(3, OUTPUT); //U-L 81 | pinMode(4, OUTPUT); //V-L 82 | pinMode(6, OUTPUT); //W-L 83 | pinMode(7, OUTPUT); //U-H 84 | pinMode(8, OUTPUT); //V-H 85 | pinMode(9, OUTPUT); //W-H 86 | //the hall sensor abc 87 | pinMode(11, INPUT); //Hall A 88 | pinMode(12, INPUT); //Hall B 89 | pinMode(13, INPUT); //Hall C 90 | pinMode(18, INPUT); //Dir 91 | pinMode(19, INPUT); // Enable 92 | // 93 | pinMode(14, INPUT_PULLUP); //BEMFPinA 94 | pinMode(15, INPUT_PULLUP); // BEMFPinB 95 | pinMode(16, INPUT_PULLUP); // BEMFPinC 96 | 97 | //setP16( ); 98 | MotorStop(); 99 | InitTimersSafe(); 100 | bool success = SetPinFrequencySafe(10, frequency); //pin 10 for high speed pwm, 101 | digitalWrite(5,1); 102 | NowPwm=0; 103 | pwmWrite(10,0); 104 | CheckMos(); 105 | MotorSound(); 106 | FlexiTimer2::set(1, 1.0/15000, MotorRUN); 107 | FlexiTimer2::start(); 108 | digitalWrite(5,0); 109 | wdt_enable(WDTO_120MS); 110 | } 111 | 112 | void setP32( ) { 113 | Serial.println("ADC Prescaler = 32"); // 101 114 | ADCSRA |= (1 << ADPS2); // 1 115 | ADCSRA &= ~(1 << ADPS1); // 0 116 | ADCSRA |= (1 << ADPS0); // 1 117 | } 118 | 119 | 120 | void setP128( ) { // 121 | Serial.println("ADC Prescaler = 128"); // 111 122 | ADCSRA |= (1 << ADPS2); // 1 123 | ADCSRA |= (1 << ADPS1); // 1 124 | ADCSRA |= (1 << ADPS0); // 1 125 | } // setP128 126 | 127 | 128 | void setP16( ) { 129 | Serial.println("ADC Prescaler = 16"); // 100 130 | ADCSRA |= (1 << ADPS2); // 1 131 | ADCSRA &= ~(1 << ADPS1); // 0 132 | ADCSRA &= ~(1 << ADPS0); // 0 133 | } 134 | 135 | void MotorRUN(){ 136 | wdt_reset(); 137 | UpdatePwm++; 138 | 139 | if(RunEnable==0&&Error==0){ 140 | if(SensorLess==1&&RunStatus==2){ 141 | if(CurrentDirect==1){ 142 | SenseBEMFCW(); 143 | UpdateStallTime++; 144 | if(MotorPhase!=LastPhase){ 145 | if(RunStatus<3)CountRun++; 146 | MosPin_STOP; 147 | delayMicroseconds(2); 148 | SensorLessCW(); 149 | LastPhase=MotorPhase; 150 | UpdateStallTime=0; 151 | digitalWrite(5,OutStatus); 152 | if(MotorPhase==1){ 153 | MotorStepTime=micros()-LastStepTime; 154 | LastStepTime=micros(); 155 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 156 | DealLoop=1; 157 | OutStatus=!OutStatus; 158 | } 159 | } 160 | } 161 | 162 | else{ 163 | SenseBEMFCCW(); 164 | UpdateStallTime++; 165 | if(MotorPhase!=LastPhase){ 166 | if(RunStatus<3)CountRun++; 167 | MosPin_STOP; 168 | delayMicroseconds(2); 169 | SensorLessCCW(); 170 | LastPhase=MotorPhase; 171 | UpdateStallTime=0; 172 | digitalWrite(5,OutStatus); 173 | 174 | if(MotorPhase==1){ 175 | MotorStepTime=micros()-LastStepTime; 176 | LastStepTime=micros(); 177 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 178 | DealLoop=1; 179 | OutStatus=!OutStatus; 180 | } 181 | } 182 | } 183 | 184 | }//SelesorLess Status 185 | if(SensorLess==0){ 186 | UpdateStallTime++; 187 | HallValue= digitalRead(11)*4+(int)digitalRead(12)*2+digitalRead(13); 188 | if(RunStatus>0)if(HallValue==0||HallValue==7)Error=3; 189 | if(HallValue!=LastHallValue){ 190 | MosPin_STOP; 191 | //delayMicroseconds(2); 192 | HallValue= digitalRead(11)*4+(int)digitalRead(12)*2+digitalRead(13); 193 | if(CurrentDirect==1){ 194 | MotorCCW(); 195 | } 196 | else{ 197 | MotorCW(); 198 | } 199 | LastHallValue=HallValue; 200 | UpdateStallTime=0; 201 | if(RunStatus<3)CountRun++; 202 | digitalWrite(5,OutStatus); 203 | 204 | if(HallValue==2){ 205 | MotorStepTime=micros()-LastStepTime; 206 | LastStepTime=micros(); 207 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 208 | DealLoop=1; 209 | OutStatus=!OutStatus; 210 | } 211 | } 212 | }//Sensored Status 213 | } 214 | else{ 215 | MosPin_STOP; 216 | } 217 | } 218 | 219 | void SensorLessCW(){ 220 | switch(MotorPhase){ 221 | case 1: 222 | MosPin_RUN1; 223 | break; 224 | case 2: 225 | MosPin_RUN2; 226 | break; 227 | case 3: 228 | MosPin_RUN3; 229 | break; 230 | case 4: 231 | MosPin_RUN4; 232 | break; 233 | case 5: 234 | MosPin_RUN5; 235 | break; 236 | case 6: 237 | MosPin_RUN6; 238 | break; 239 | default: MosPin_STOP; 240 | } 241 | } 242 | 243 | void SensorLessCCW(){ 244 | switch(MotorPhase){ 245 | case 1: 246 | MosPin_RUN1; 247 | break; 248 | case 2: //100 249 | MosPin_RUN6; 250 | break; 251 | case 3: //110 252 | MosPin_RUN5; 253 | break; 254 | case 4: //010 255 | MosPin_RUN4; 256 | break; 257 | case 5: //011 258 | MosPin_RUN3; 259 | break; 260 | case 6: //001 261 | MosPin_RUN2; 262 | break; 263 | default: MosPin_STOP; 264 | } 265 | } 266 | 267 | void SenseBEMFCW(){ 268 | BEMFPinA=digitalRead(14); 269 | BEMFPinB=digitalRead(15); 270 | BEMFPinC=digitalRead(16); 271 | switch(MotorPhase){ 272 | case 1: 273 | if(BEMFPinC==_BEMFLow){ 274 | RunTime=micros()-BEMFTimeStamp; 275 | delayMicroseconds(2); 276 | BEMFPinC=digitalRead(16); 277 | if((BEMFPinC==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 278 | MotorPhase++; 279 | LastRunCycleTime=micros()-BEMFTimeStamp; 280 | BEMFTimeStamp=micros(); 281 | } 282 | } 283 | break; 284 | case 2: 285 | if(BEMFPinB==_BEMFHigh){ 286 | RunTime=micros()-BEMFTimeStamp; 287 | delayMicroseconds(2); 288 | BEMFPinB=digitalRead(15); 289 | if((BEMFPinB==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 290 | MotorPhase++; 291 | LastRunCycleTime=micros()-BEMFTimeStamp; 292 | BEMFTimeStamp=micros(); 293 | } 294 | } 295 | break; 296 | case 3: 297 | if(BEMFPinA==_BEMFLow){ 298 | RunTime=micros()-BEMFTimeStamp; 299 | delayMicroseconds(2); 300 | BEMFPinA=digitalRead(14); 301 | if((BEMFPinA==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 302 | MotorPhase++; 303 | LastRunCycleTime=micros()-BEMFTimeStamp; 304 | BEMFTimeStamp=micros(); 305 | } 306 | 307 | } 308 | break; 309 | case 4: 310 | if(BEMFPinC==_BEMFHigh){ 311 | RunTime=micros()-BEMFTimeStamp; 312 | delayMicroseconds(2); 313 | BEMFPinC=digitalRead(16); 314 | if((BEMFPinC==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 315 | MotorPhase++; 316 | LastRunCycleTime=micros()-BEMFTimeStamp; 317 | BEMFTimeStamp=micros(); 318 | } 319 | } 320 | break; 321 | case 5: 322 | if(BEMFPinB==_BEMFLow){ 323 | RunTime=micros()-BEMFTimeStamp; 324 | delayMicroseconds(2); 325 | BEMFPinB=digitalRead(15); 326 | if((BEMFPinB==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 327 | MotorPhase++; 328 | LastRunCycleTime=micros()-BEMFTimeStamp; 329 | BEMFTimeStamp=micros(); 330 | } 331 | } 332 | break; 333 | case 6: 334 | if(BEMFPinA==_BEMFHigh){ 335 | RunTime=micros()-BEMFTimeStamp; 336 | delayMicroseconds(2); 337 | BEMFPinA=digitalRead(14); 338 | if((BEMFPinA==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 339 | MotorPhase=1; 340 | LastRunCycleTime=micros()-BEMFTimeStamp; 341 | BEMFTimeStamp=micros(); 342 | } 343 | } 344 | break; 345 | default: MosPin_STOP; 346 | } 347 | 348 | } 349 | 350 | 351 | void SenseBEMFCCW(){ 352 | BEMFPinA=digitalRead(14); 353 | BEMFPinB=digitalRead(15); 354 | BEMFPinC=digitalRead(16); 355 | 356 | 357 | switch(MotorPhase){ 358 | case 1: 359 | if(BEMFPinC==_BEMFHigh){ 360 | RunTime=micros()-BEMFTimeStamp; 361 | delayMicroseconds(2); 362 | BEMFPinC=digitalRead(16); 363 | if((BEMFPinC==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 364 | MotorPhase++; 365 | LastRunCycleTime=micros()-BEMFTimeStamp; 366 | BEMFTimeStamp=micros(); 367 | } 368 | } 369 | break; 370 | case 2: 371 | if(BEMFPinA==_BEMFLow){ 372 | RunTime=micros()-BEMFTimeStamp; 373 | delayMicroseconds(2); 374 | BEMFPinA=digitalRead(14); 375 | if((BEMFPinA==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 376 | MotorPhase++; 377 | LastRunCycleTime=micros()-BEMFTimeStamp; 378 | BEMFTimeStamp=micros(); 379 | } 380 | } 381 | break; 382 | case 3: 383 | if(BEMFPinB==_BEMFHigh){ 384 | RunTime=micros()-BEMFTimeStamp; 385 | delayMicroseconds(2); 386 | BEMFPinB=digitalRead(15); 387 | if((BEMFPinB==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 388 | MotorPhase++; 389 | LastRunCycleTime=micros()-BEMFTimeStamp; 390 | BEMFTimeStamp=micros(); 391 | } 392 | } 393 | break; 394 | case 4: 395 | if(BEMFPinC==_BEMFLow){ 396 | RunTime=micros()-BEMFTimeStamp; 397 | delayMicroseconds(2); 398 | BEMFPinC=digitalRead(16); 399 | if((BEMFPinC==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 400 | MotorPhase++; 401 | LastRunCycleTime=micros()-BEMFTimeStamp; 402 | BEMFTimeStamp=micros(); 403 | } 404 | } 405 | break; 406 | case 5: 407 | if(BEMFPinA==_BEMFHigh){ 408 | RunTime=micros()-BEMFTimeStamp; 409 | delayMicroseconds(2); 410 | BEMFPinA=digitalRead(14); 411 | if((BEMFPinA==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 412 | MotorPhase++; 413 | LastRunCycleTime=micros()-BEMFTimeStamp; 414 | BEMFTimeStamp=micros(); 415 | } 416 | } 417 | break; 418 | case 6: 419 | if(BEMFPinB==_BEMFLow){ 420 | RunTime=micros()-BEMFTimeStamp; 421 | delayMicroseconds(2); 422 | BEMFPinB=digitalRead(15); 423 | if((BEMFPinB==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 424 | MotorPhase=1; 425 | LastRunCycleTime=micros()-BEMFTimeStamp; 426 | BEMFTimeStamp=micros(); 427 | } 428 | } 429 | break; 430 | default: MosPin_STOP; 431 | } 432 | 433 | } 434 | /* 435 | 436 | void SenseBEMF(){ 437 | BEMFPinA=digitalRead(14); 438 | BEMFPinB=digitalRead(15); 439 | BEMFPinC=digitalRead(16); 440 | HallValue= digitalRead(14)*4+(int)digitalRead(15)*2+digitalRead(16); 441 | 442 | switch(MotorPhase){ 443 | case 1: //101 444 | 445 | if(sense==1){ 446 | if(BEMFPinC==_BEMFLow){ 447 | delayMicroseconds(1); 448 | BEMFPinC=digitalRead(16); 449 | if(BEMFPinC==_BEMFLow){ LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 450 | else{ 451 | //nothing 452 | } 453 | }else{ 454 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 455 | MotorPhase++; sense=1; 456 | } 457 | } 458 | 459 | break; 460 | case 2: //100 461 | 462 | if(sense==1){ 463 | if(BEMFPinB==_BEMFHigh){ 464 | delayMicroseconds(1); 465 | BEMFPinB=digitalRead(15); 466 | if(BEMFPinB==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 467 | else{ 468 | } 469 | 470 | }else{ 471 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 472 | MotorPhase++; sense=1; 473 | } 474 | } 475 | break; 476 | case 3: //110 477 | 478 | if(sense==1){ 479 | 480 | if(BEMFPinA==_BEMFLow){ 481 | delayMicroseconds(1); 482 | BEMFPinA=digitalRead(14); 483 | if(BEMFPinA==_BEMFLow){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;RunTest++;}} 484 | else{ 485 | 486 | } 487 | }else{ 488 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 489 | MotorPhase++; sense=1; 490 | } 491 | } 492 | break; 493 | case 4: //010 494 | 495 | if(sense==1){ 496 | if(BEMFPinC==_BEMFHigh){ 497 | delayMicroseconds(1); 498 | BEMFPinC=digitalRead(16); 499 | if(BEMFPinC==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 500 | else{ 501 | } 502 | }else{ 503 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 504 | MotorPhase++; sense=1; 505 | } 506 | } 507 | break; 508 | case 5: //011 509 | 510 | if(sense==1){ 511 | 512 | if(BEMFPinB==_BEMFLow){ 513 | delayMicroseconds(1); 514 | BEMFPinB=digitalRead(15); 515 | if(BEMFPinB==_BEMFLow){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 516 | else{ 517 | } 518 | }else{ 519 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 520 | MotorPhase++; sense=1; 521 | } 522 | } 523 | break; 524 | case 6: //001 525 | 526 | if(sense==1){ 527 | if(BEMFPinA==_BEMFHigh){ 528 | delayMicroseconds(1); 529 | BEMFPinA=digitalRead(14); 530 | if(BEMFPinA==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 531 | else{ 532 | 533 | } 534 | }else{ 535 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 536 | MotorPhase=1; sense=1; 537 | } 538 | } 539 | break; 540 | default: MosPin_STOP; 541 | } 542 | 543 | } 544 | */ 545 | void MotorCW(){ 546 | switch(HallValue){ 547 | case 5: //101 548 | MosPin_RUN1; 549 | break; 550 | case 4: //100 551 | MosPin_RUN2; 552 | break; 553 | case 6: //110 554 | MosPin_RUN3; 555 | break; 556 | case 2: //010 557 | MosPin_RUN4; 558 | break; 559 | case 3: //011 560 | MosPin_RUN5; 561 | break; 562 | case 1: //001 563 | MosPin_RUN6; 564 | break; 565 | default: MosPin_STOP; 566 | } 567 | } 568 | /* 569 | //Same special motor need this sequence, 有些电机比较特殊，需要用这个反向表 570 | void MotorCCW(){ 571 | switch(HallValue){ 572 | case 5: //101 573 | MosPin_RUN3; 574 | break; 575 | case 4: //100 576 | MosPin_RUN4; 577 | break; 578 | case 6: //110 579 | MosPin_RUN5; 580 | break; 581 | case 2: //010 582 | MosPin_RUN6; 583 | break; 584 | case 3: //011 585 | MosPin_RUN1; 586 | break; 587 | case 1: //001 588 | MosPin_RUN2; 589 | break; 590 | default: MosPin_STOP; 591 | } 592 | } 593 | 594 | */ 595 | 596 | void MotorCCW(){ 597 | switch(HallValue){ 598 | case 5: //101 599 | MosPin_RUN4; 600 | break; 601 | case 4: //100 602 | MosPin_RUN5; 603 | break; 604 | case 6: //110 605 | MosPin_RUN6; 606 | break; 607 | case 2: //010 608 | MosPin_RUN1; 609 | break; 610 | case 3: //011 611 | MosPin_RUN2; 612 | break; 613 | case 1: //001 614 | MosPin_RUN3; 615 | break; 616 | default: MosPin_STOP; 617 | } 618 | } 619 | 620 | 621 | void CheckInputPin(){ 622 | RunPin=digitalRead(19); 623 | DirPin=digitalRead(18); 624 | //RunPin Check; 625 | if(RunEnable!=RunPin){ 626 | RunPinChangeCount++; 627 | if(RunPinChangeCount>_KeyMaxCount){ 628 | RunEnable=RunPin; 629 | if(RunEnable==1)Error=0; //复位错误 630 | RunPinChangeCount=0; 631 | 632 | } 633 | } 634 | else{ 635 | RunPinChangeCount=0; 636 | } 637 | 638 | //DirPin Check 639 | if(DirStatus!=DirPin){ 640 | DirPinChangeCount++; 641 | if(DirPinChangeCount>_KeyMaxCount){ DirStatus=DirPin;DirPinChangeCount=0;} 642 | } 643 | else{ 644 | DirPinChangeCount=0; 645 | } 646 | 647 | } 648 | 649 | void FlashError(int Err){ 650 | int TempDiff; 651 | if(FlashErr<(Err+1)){ 652 | if(FlashBegan==1){ThisFlashTime=millis();FlashBegan=0;} 653 | TempDiff=millis()-ThisFlashTime; 654 | if(TempDiff<100)digitalWrite(5,1); 655 | if(TempDiff>100&&TempDiff<500)digitalWrite(5,0); 656 | if(TempDiff>500){FlashBegan=1;FlashErr++;} 657 | } 658 | else{ 659 | if(FlashBegan==1){ThisFlashTime=millis();FlashBegan=0;} 660 | TempDiff=millis()-ThisFlashTime; 661 | if(TempDiff<1000){digitalWrite(5,0);}else{FlashErr=0;Serial.println(Error);Serial.println(ErrIshunt);} 662 | } 663 | 664 | } 665 | 666 | 667 | void CurrentCheck(){ 668 | 669 | //read the Current 670 | Ishunt=analogRead(A6); 671 | SubIshunt=SubIshunt-AvgIshunt+Ishunt; 672 | AvgIshunt=SubIshunt/5; 673 | //Startup Current 674 | if(AvgIshunt>_MaxUpIshunt&&RunStatus==1){Error=5;MosPin_STOP;ErrIshunt=AvgIshunt;} 675 | //Normal Current 676 | if(AvgIshunt>_MaxIshunt&&RunStatus>1){ 677 | CountErr++; 678 | if(CountErr>_MaxIshuntTime){Error=6;MosPin_STOP;ErrIshunt=AvgIshunt;} 679 | } 680 | else{ 681 | CountErr--; 682 | if(CountErr<0)CountErr=0; 683 | } 684 | //The peak current. 685 | if(Ishunt>_MaxMaxIshunt){Error=7;MosPin_STOP;ErrIshunt=Ishunt;} 686 | 687 | if(digitalRead(2)==0){ 688 | Error=1; //硬件过流保护 689 | } 690 | } 691 | 692 | void readEnvTemp(){ 693 | 694 | double Digital_Value=analogRead(A7); //读取串联电阻上的电压值（数字量） 695 | double Voltage_Value=(Digital_Value/1023)*5.00;//换算成模拟量的电压值 696 | double Rt_Value= Voltage_Value*82/(48-Voltage_Value); //计算出热敏电阻的阻值 697 | EnvTemperature=(double)1/(log(Rt_Value/10)/3980 + 1/( 25 + 273.15)) - 273.15; //计算所感知的温度并发送 698 | 699 | } 700 | 701 | 702 | void CheckMos(){ 703 | pwmWrite(10,100); 704 | //check UH 705 | digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 706 | delayMicroseconds(5); 707 | InitIshuntCheck(6); 708 | 709 | //check VH 710 | MosPin_STOP; 711 | delayMicroseconds(5); 712 | digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 713 | delayMicroseconds(5); 714 | InitIshuntCheck(6); 715 | //Check WH 716 | MosPin_STOP; 717 | delayMicroseconds(5); 718 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 719 | delayMicroseconds(5); 720 | InitIshuntCheck(6); 721 | //Check UL 722 | MosPin_STOP; 723 | delayMicroseconds(5); 724 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0); 725 | delayMicroseconds(5); 726 | InitIshuntCheck(6); 727 | //Check VL 728 | MosPin_STOP; 729 | delayMicroseconds(5); 730 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0); 731 | delayMicroseconds(5); 732 | InitIshuntCheck(6); 733 | //Check WL 734 | MosPin_STOP; 735 | delayMicroseconds(5); 736 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1); 737 | delayMicroseconds(5); 738 | InitIshuntCheck(6); 739 | 740 | //Finish Test Close MosFET. 741 | MosPin_STOP; 742 | pwmWrite(10,0); 743 | } 744 | void InitIshuntCheck(int times){ 745 | for(int i=0;i_MaxIshuntCheck){while(1){MosPin_STOP;FlashError(1);};} 750 | } 751 | 752 | 753 | void SoundFreq(int ontime, int offtime) { 754 | int i=0; 755 | for(i=0; i1000)ReadPoint=1016; 800 | if(ReadPoint<50){Error=2;MotorStop();}else{if(Error==2)Error=0;} //when the pointmeter value <20, The motor will stop. 801 | TargetSpeed=map(ReadPoint,50,1016,M_MinSpeed,M_MaxSpeed); 802 | if(TargetSpeedM_MaxSpeed)TargetSpeed=M_MaxSpeed; 804 | } 805 | 806 | 807 | void MotorRunUp(){ 808 | int i; 809 | int j; 810 | if(SensorLess==1){ 811 | //Location the Roller; 812 | for(i=0;i<_MaxLocationPWM;i++){ 813 | if(Error==0){ 814 | NowPwm++; 815 | pwmWrite(10,NowPwm/4); 816 | MosPin_RUN1; 817 | delayMicroseconds(_MaxLocationTime); 818 | Ishunt=analogRead(A6); 819 | CurrentCheck(); 820 | 821 | }else{ 822 | break; 823 | } 824 | } 825 | MotorPhase=2; //The Next Phase; 826 | j=0; 827 | for(;;){ 828 | if(Error==0){ 829 | if(CurrentDirect==1){ 830 | SensorLessCW();} 831 | else{ 832 | SensorLessCCW(); 833 | } 834 | LastPhase=MotorPhase; 835 | if(j>(_MaxMotorUpStep)){ 836 | LastRunCycleTime=UpDelayTime[j]*_DelayTimes+_UpOffSetDelayTime; 837 | RunStatus=2; 838 | 839 | break; 840 | } 841 | for(i=0;i<_DelayTimes;i++){ 842 | delayMicroseconds(UpDelayTime[j]); 843 | delayMicroseconds(_UpOffSetDelayTime); 844 | } 845 | 846 | MotorPhase++; 847 | if(MotorPhase>6){ 848 | j++;MotorPhase=1; 849 | NowPwm++; 850 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 851 | pwmWrite(10,NowPwm/4); 852 | } 853 | CurrentCheck(); 854 | BEMFTimeStamp=micros(); 855 | }else 856 | { 857 | break; 858 | } 859 | } 860 | } 861 | else{ //Motor Up by Sensored method. 862 | if (UpdatePwm>_UpdatePWMTime){ 863 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; 864 | NowPwm=NowPwm+_PWMUpStep; 865 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 866 | pwmWrite(10,NowPwm/4); 867 | UpdatePwm=0; 868 | } 869 | ForceRun(); 870 | RightRun(); 871 | if(CountRun>18){ 872 | if(RightRunStatus==1){ 873 | RunStatus=2; 874 | } 875 | else{ 876 | Error=8; //Hall sequence wrong, 877 | } 878 | } 879 | 880 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 881 | if(CurrentDirect!=DirStatus){RunStatus=3; } 882 | } 883 | 884 | } 885 | 886 | void ForceRun(){ 887 | HallValue= digitalRead(11)*4+digitalRead(12)*2+digitalRead(13); 888 | if(HallValue==0||HallValue==7)Error=3; 889 | if(CurrentDirect==1){ 890 | MotorCCW(); 891 | } 892 | else{ 893 | MotorCW(); 894 | } 895 | CurrentCheck(); 896 | LastHallValue=HallValue; 897 | } 898 | 899 | 900 | void RightRun(){ 901 | switch(HallValue){ 902 | case 1: 903 | Hall_1=1; 904 | break; 905 | case 2: 906 | Hall_2=1; 907 | break; 908 | case 3: 909 | Hall_3=1; 910 | break; 911 | case 4: 912 | Hall_4=1; 913 | break; 914 | case 5: 915 | Hall_5=1; 916 | break; 917 | case 6: 918 | Hall_6=1; 919 | break; 920 | default: 921 | RightRunStatus=0; 922 | break; 923 | } 924 | RightRunStatus=Hall_1&&Hall_2&&Hall_3&&Hall_4&&Hall_5&&Hall_6; 925 | } 926 | 927 | void OpenRun(){ 928 | if (UpdatePwm>_UpdatePWMTime){ 929 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; 930 | if(ReadPoint>NowPwm){ 931 | NowPwm=NowPwm+_PWMUpStep; 932 | } 933 | else{ 934 | NowPwm=NowPwm-_PWMDownStep; 935 | } 936 | pwmWrite(10,NowPwm/4); 937 | UpdatePwm=0; 938 | if(CountRun>80&&M_LoopRun&&SensorLess==0)RunStatus=5; 939 | 940 | } 941 | } 942 | 943 | 944 | void LoopRun(){ 945 | 946 | if(DealLoop==1){ 947 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; // 获得以RPM 为单位的速度，//MotorParameters[2]是极对数 948 | 949 | CEK = M_KP + M_KI + M_KD ; 950 | CEK1 = 0 - M_KP - 2*M_KD ; 951 | CEK2 = M_KD ; 952 | EK2 = EK1 ; 953 | EK1 = EK ; 954 | EK = TargetSpeed - MotorActualSpeed; 955 | 956 | YK = EK * CEK + EK1 * CEK1 + EK2 *CEK2 ; 957 | YK=YK/M_Scale; 958 | if(YK>50)YK=50; 959 | if(YK<-50)YK=-50; 960 | NowPwm=NowPwm+YK; 961 | 962 | if(NowPwm <= M_MinMinPWM) 963 | NowPwm = M_MinMinPWM; 964 | else if(NowPwm > M_MaxMaxPWM) 965 | NowPwm = M_MaxMaxPWM; 966 | pwmWrite(10,NowPwm/4); 967 | DealLoop=0; 968 | } 969 | } 970 | 971 | void MotorStop(){ 972 | NowPwm=0; 973 | pwmWrite(10,0); 974 | MosPin_STOP; 975 | RunStatus=0; 976 | CountRun=0; 977 | UpdateStallTime=0; 978 | if(Error==0)digitalWrite(5,0); 979 | } 980 | 981 | void loop(){ 982 | 983 | CheckInputPin(); 984 | if(RunEnable==0){ 985 | 986 | ReadPointValue(); 987 | if(Error==0){ 988 | switch(RunStatus){ 989 | case 0: 990 | // setP32( ); 991 | HallValue=0; 992 | HallValue= digitalRead(11)*4+digitalRead(12)*2+digitalRead(13); 993 | if(HallValue==0||HallValue==7){ 994 | SensorLess=1; 995 | if(SensorLessSound==1){ 996 | pwmWrite(10,60); 997 | Sound(3, 20, 80, 1200); 998 | pwmWrite(10,0); 999 | SensorLessSound=0; 1000 | } 1001 | } 1002 | else{ 1003 | SensorLess=0; 1004 | SensorLessSound=1; 1005 | } 1006 | // FlexiTimer2::start(); 1007 | CurrentDirect=DirStatus; 1008 | UpdateStallTime=0; 1009 | CountRun=0; 1010 | NowPwm=0; 1011 | Ishunt=3; //假设电流采样值 1012 | SubIshunt=15; //假设电流采样值 1013 | AvgIshunt=3; //假设电流采样值 1014 | MotorStepTime=0; 1015 | AvgMotorStepTime=0; 1016 | Hall_1=0;Hall_2=0;Hall_3=0;Hall_4=0;Hall_5=0;Hall_6=0;RightRunStatus=0; 1017 | RunStatus=1; 1018 | break; 1019 | case 1: 1020 | MotorRunUp(); 1021 | 1022 | break; 1023 | case 2: 1024 | OpenRun(); 1025 | if(CurrentDirect!=DirStatus)RunStatus=3; 1026 | break; 1027 | case 3: 1028 | // FlexiTimer2::stop(); 1029 | NowPwm=0; 1030 | pwmWrite(10,0); 1031 | MosPin_STOP; 1032 | delay(800); 1033 | MosPin_BRK; 1034 | delay(400); 1035 | MosPin_STOP; 1036 | delay(100); 1037 | CurrentDirect=DirStatus; 1038 | RunStatus=0; 1039 | break; 1040 | case 5: 1041 | LoopRun(); 1042 | if(CurrentDirect!=DirStatus)RunStatus=3; 1043 | break; 1044 | default: 1045 | Error=9; 1046 | MosPin_STOP; 1047 | break; 1048 | } 1049 | } 1050 | else{ 1051 | pwmWrite(10,0); 1052 | MosPin_STOP; 1053 | FlashError(Error); //flash the Error statues 1054 | } 1055 | if(UpdateStallTime>_MaxUpdateStallTime)Error=4; 1056 | if(UpdateStallTime>_MaxSensorLessUpdateStallTime&&SensorLess==1)Error=4; 1057 | } 1058 | else{ 1059 | MotorStop(); 1060 | } 1061 | CurrentCheck(); 1062 | readEnvTemp(); 1063 | //Debug for output data; 1064 | if((millis()-lastrtime)>1000){ 1065 | 1066 | Serial.print("ReadPoint"); 1067 | Serial.println(ReadPoint); 1068 | // Serial.print("Ishunt"); 1069 | // Serial.println(AvgIshunt); 1070 | // Serial.print("Cunrrent"); 1071 | // float tempi=AvgIshunt*125/256; 1072 | // Serial.println(tempi); 1073 | lastrtime=millis(); 1074 | } 1075 | 1076 | } 1077 | 1078 | 1079 | 1080 | -------------------------------------------------------------------------------- /seamabldcallinoneV1.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //sema bldc v1.0 6 | //we use pwm lib for high speed pwm 7 | //we use flexitimer2 for detect the hall sensor 8 | //The MosFET status: 9 | //1-UH(Pin7) 3-VH(Pin8) 5-WH;(Pin9) 10 | //2-UL(Pin2) 4-VL(Pin3) 6-WL;(Pin4) //旧版 11 | //2-UL(Pin3) 4-VL(Pin4) 6-WL;(Pin6) 12 | #define MosPin_RUN1 digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0) //101 CW Hall=5; (1,4) 10-01-00 U-V-W AB 13 | #define MosPin_RUN2 digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1) //100 CW Hall=4; (1,6) 10-00-01 U-V-W AC 14 | #define MosPin_RUN3 digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1) //110 CW Hall=6; (3,6) 00-10-01 U-V-W BC 15 | #define MosPin_RUN4 digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0) //010 CW Hall=2; (3,2) 01-10-00 U-V-W BA 16 | #define MosPin_RUN5 digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0) //011 CW Hall=3; (5,2) 01-00-10 U-V-W CA 17 | #define MosPin_RUN6 digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0) //001 CW Hall=1; (5,4) 00-10-01 U-V-W CB 18 | #define MosPin_STOP digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0) //All MosFet Close 00-00-00 U-V-W 19 | #define MosPin_BRK digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,1);digitalWrite(6,1) //The Low MosFet Open 01-01-01 U-V-W 20 | 21 | #define _MotorPhasePairs 5 22 | #define _UpdatePWMTime 1 //The Value more big, the motor will start up more slow. 23 | #define _PWMUpStep 2 24 | #define _PWMDownStep 1 25 | #define _DelayTimes 1 26 | #define _UpOffSetDelayTime 10 27 | #define _MaxBRKTimes 1000 28 | #define _MaxPinChangeTime 500 29 | #define _KeyMaxCount 20 30 | 31 | #define _MaxIshunt 600 //Limit Normal run current about 12A ( the resistance value 10momega) 32 | #define _MaxUpIshunt 580 //Limit Startup Current about 3A 33 | #define _MaxMaxIshunt 680 //Limit Peeak Current about 16A 34 | #define _MaxIshuntCheck 220 //Check the mosfet status. Limit the current. 35 | #define _MaxIshuntTime 2 36 | 37 | #define _MaxUpPWM 900 //Limit Startup the PWM 38 | #define _MaxLocationPWM 150 //Location the rotor position. 39 | #define _MaxLocationTime 100 40 | #define _MaxMotorUpStep 5 41 | 42 | #define _BEMFLow 0 43 | #define _BEMFHigh 1 44 | 45 | #define _MaxUpdateStallTime 20000 //The motor stall time 46 | #define _MaxSensorLessUpdateStallTime 5000 //The sensorless time 47 | 48 | #define M_LoopRun 0 49 | #define M_KP 8 50 | #define M_KI 2 51 | #define M_KD 0 52 | #define M_Scale 30 53 | #define M_MaxMaxPWM 1016 54 | #define M_MinMinPWM 200 55 | #define M_MaxSpeed 3000 56 | #define M_MinSpeed 200 57 | 58 | //int UpDelayTime[]= {2000,1900,1800,1600,1400,1200,1000,800,600,550,500,480,460,440,420,400,380,360,355,350,345,340,335,330,325,320,315,310,305,300,298,292}; 59 | int UpDelayTime[]= {50,50,50,49,49,49,43,42,40,40,39,39,38,38,37,37,36,35,35,34,34,34,33,33,32,31,31,30}; 60 | //int UpDelayTime[]= {1650,1600,1550,1500,1500,1450,1450,1400,1400,1400,1380,1370,1360,1360,1355,1355,1350,1350,1340,1340,1330,1330,1320,1320,1320,1320,1320,1310}; 61 | int frequency = 20000; //pwm frequency in Hz 16khz 62 | int HallValue,LastHallValue,RunPin,DirPin,BEMFPinA,BEMFPinB,BEMFPinC; 63 | int RunEnable=1,DirStatus=1; 64 | int ReadPoint,NowPwm=0; 65 | unsigned int UpdatePwm=0; 66 | unsigned int UpdateStallTime=0; 67 | int OutStatus, Ishunt,AvgIshunt=0,SubIshunt,ErrIshunt,CountErr=0,Error=0,HallErr=0,CountRun=0,RunStatus=0,CurrentDirect; 68 | int RunPinChangeCount=0,DirPinChangeCount=0; 69 | int FlashBegan,FlashErr; 70 | unsigned long AvgMotorStepTime,MotorStepTime,LastStepTime,ThisFlashTime,lastrtime; 71 | unsigned long BEMFTimeStamp,LastRunCycleTime,RunTime; 72 | int MotorPhase,LastPhase,SensorLess=0; 73 | int Hall_1=0,Hall_2=0,Hall_3=0,Hall_4=0,Hall_5=0,Hall_6=0,RightRunStatus=0,SensorLessSound=1; 74 | int MotorActualSpeed,TargetSpeed; 75 | int EK, EK1,EK2,CEK,CEK1,CEK2,YK,DealLoop; 76 | float EnvTemperature; 77 | 78 | void setup(){ 79 | Serial.begin(9600); 80 | pinMode(2, OUTPUT); //U-L 81 | pinMode(3, OUTPUT); //V-L 82 | pinMode(4, OUTPUT); //W-L 83 | pinMode(7, OUTPUT); //U-H 84 | pinMode(8, OUTPUT); //V-H 85 | pinMode(9, OUTPUT); //W-H 86 | //the hall sensor abc 87 | pinMode(11, INPUT); //Hall A 88 | pinMode(12, INPUT); //Hall B 89 | pinMode(13, INPUT); //Hall C 90 | pinMode(18, INPUT); //Dir 91 | pinMode(19, INPUT); // Enable 92 | // 93 | pinMode(14, INPUT_PULLUP); //BEMFPinA 94 | pinMode(15, INPUT_PULLUP); // BEMFPinB 95 | pinMode(16, INPUT_PULLUP); // BEMFPinC 96 | 97 | //setP16( ); 98 | MotorStop(); 99 | InitTimersSafe(); 100 | bool success = SetPinFrequencySafe(10, frequency); //pin 10 for high speed pwm, 101 | digitalWrite(5,1); 102 | NowPwm=0; 103 | pwmWrite(10,0); 104 | CheckMos(); 105 | MotorSound(); 106 | FlexiTimer2::set(1, 1.0/15000, MotorRUN); 107 | FlexiTimer2::start(); 108 | digitalWrite(5,0); 109 | wdt_enable(WDTO_120MS); 110 | } 111 | 112 | void setP32( ) { 113 | Serial.println("ADC Prescaler = 32"); // 101 114 | ADCSRA |= (1 << ADPS2); // 1 115 | ADCSRA &= ~(1 << ADPS1); // 0 116 | ADCSRA |= (1 << ADPS0); // 1 117 | } 118 | 119 | 120 | void setP128( ) { // 121 | Serial.println("ADC Prescaler = 128"); // 111 122 | ADCSRA |= (1 << ADPS2); // 1 123 | ADCSRA |= (1 << ADPS1); // 1 124 | ADCSRA |= (1 << ADPS0); // 1 125 | } // setP128 126 | 127 | 128 | void setP16( ) { 129 | Serial.println("ADC Prescaler = 16"); // 100 130 | ADCSRA |= (1 << ADPS2); // 1 131 | ADCSRA &= ~(1 << ADPS1); // 0 132 | ADCSRA &= ~(1 << ADPS0); // 0 133 | } 134 | 135 | void MotorRUN(){ 136 | wdt_reset(); 137 | UpdatePwm++; 138 | 139 | if(RunEnable==0&&Error==0){ 140 | if(SensorLess==1&&RunStatus==2){ 141 | if(CurrentDirect==1){ 142 | SenseBEMFCW(); 143 | UpdateStallTime++; 144 | if(MotorPhase!=LastPhase){ 145 | if(RunStatus<3)CountRun++; 146 | MosPin_STOP; 147 | delayMicroseconds(2); 148 | SensorLessCW(); 149 | LastPhase=MotorPhase; 150 | UpdateStallTime=0; 151 | digitalWrite(5,OutStatus); 152 | if(MotorPhase==1){ 153 | MotorStepTime=micros()-LastStepTime; 154 | LastStepTime=micros(); 155 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 156 | DealLoop=1; 157 | OutStatus=!OutStatus; 158 | } 159 | } 160 | } 161 | 162 | else{ 163 | SenseBEMFCCW(); 164 | UpdateStallTime++; 165 | if(MotorPhase!=LastPhase){ 166 | if(RunStatus<3)CountRun++; 167 | MosPin_STOP; 168 | delayMicroseconds(2); 169 | SensorLessCCW(); 170 | LastPhase=MotorPhase; 171 | UpdateStallTime=0; 172 | digitalWrite(5,OutStatus); 173 | 174 | if(MotorPhase==1){ 175 | MotorStepTime=micros()-LastStepTime; 176 | LastStepTime=micros(); 177 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 178 | DealLoop=1; 179 | OutStatus=!OutStatus; 180 | } 181 | } 182 | } 183 | 184 | }//SelesorLess Status 185 | if(SensorLess==0){ 186 | UpdateStallTime++; 187 | HallValue= digitalRead(11)*4+(int)digitalRead(12)*2+digitalRead(13); 188 | if(RunStatus>0)if(HallValue==0||HallValue==7)Error=3; 189 | if(HallValue!=LastHallValue){ 190 | MosPin_STOP; 191 | //delayMicroseconds(2); 192 | HallValue= digitalRead(11)*4+(int)digitalRead(12)*2+digitalRead(13); 193 | if(CurrentDirect==1){ 194 | MotorCCW(); 195 | } 196 | else{ 197 | MotorCW(); 198 | } 199 | LastHallValue=HallValue; 200 | UpdateStallTime=0; 201 | if(RunStatus<3)CountRun++; 202 | digitalWrite(5,OutStatus); 203 | 204 | if(HallValue==2){ 205 | MotorStepTime=micros()-LastStepTime; 206 | LastStepTime=micros(); 207 | AvgMotorStepTime=(AvgMotorStepTime+MotorStepTime)>>1; 208 | DealLoop=1; 209 | OutStatus=!OutStatus; 210 | } 211 | } 212 | }//Sensored Status 213 | } 214 | else{ 215 | MosPin_STOP; 216 | } 217 | } 218 | 219 | void SensorLessCW(){ 220 | switch(MotorPhase){ 221 | case 1: 222 | MosPin_RUN1; 223 | break; 224 | case 2: 225 | MosPin_RUN2; 226 | break; 227 | case 3: 228 | MosPin_RUN3; 229 | break; 230 | case 4: 231 | MosPin_RUN4; 232 | break; 233 | case 5: 234 | MosPin_RUN5; 235 | break; 236 | case 6: 237 | MosPin_RUN6; 238 | break; 239 | default: MosPin_STOP; 240 | } 241 | } 242 | 243 | void SensorLessCCW(){ 244 | switch(MotorPhase){ 245 | case 1: 246 | MosPin_RUN1; 247 | break; 248 | case 2: //100 249 | MosPin_RUN6; 250 | break; 251 | case 3: //110 252 | MosPin_RUN5; 253 | break; 254 | case 4: //010 255 | MosPin_RUN4; 256 | break; 257 | case 5: //011 258 | MosPin_RUN3; 259 | break; 260 | case 6: //001 261 | MosPin_RUN2; 262 | break; 263 | default: MosPin_STOP; 264 | } 265 | } 266 | 267 | void SenseBEMFCW(){ 268 | BEMFPinA=digitalRead(14); 269 | BEMFPinB=digitalRead(15); 270 | BEMFPinC=digitalRead(16); 271 | switch(MotorPhase){ 272 | case 1: 273 | if(BEMFPinC==_BEMFLow){ 274 | RunTime=micros()-BEMFTimeStamp; 275 | delayMicroseconds(2); 276 | BEMFPinC=digitalRead(16); 277 | if((BEMFPinC==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 278 | MotorPhase++; 279 | LastRunCycleTime=micros()-BEMFTimeStamp; 280 | BEMFTimeStamp=micros(); 281 | } 282 | } 283 | break; 284 | case 2: 285 | if(BEMFPinB==_BEMFHigh){ 286 | RunTime=micros()-BEMFTimeStamp; 287 | delayMicroseconds(2); 288 | BEMFPinB=digitalRead(15); 289 | if((BEMFPinB==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 290 | MotorPhase++; 291 | LastRunCycleTime=micros()-BEMFTimeStamp; 292 | BEMFTimeStamp=micros(); 293 | } 294 | } 295 | break; 296 | case 3: 297 | if(BEMFPinA==_BEMFLow){ 298 | RunTime=micros()-BEMFTimeStamp; 299 | delayMicroseconds(2); 300 | BEMFPinA=digitalRead(14); 301 | if((BEMFPinA==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 302 | MotorPhase++; 303 | LastRunCycleTime=micros()-BEMFTimeStamp; 304 | BEMFTimeStamp=micros(); 305 | } 306 | 307 | } 308 | break; 309 | case 4: 310 | if(BEMFPinC==_BEMFHigh){ 311 | RunTime=micros()-BEMFTimeStamp; 312 | delayMicroseconds(2); 313 | BEMFPinC=digitalRead(16); 314 | if((BEMFPinC==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 315 | MotorPhase++; 316 | LastRunCycleTime=micros()-BEMFTimeStamp; 317 | BEMFTimeStamp=micros(); 318 | } 319 | } 320 | break; 321 | case 5: 322 | if(BEMFPinB==_BEMFLow){ 323 | RunTime=micros()-BEMFTimeStamp; 324 | delayMicroseconds(2); 325 | BEMFPinB=digitalRead(15); 326 | if((BEMFPinB==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 327 | MotorPhase++; 328 | LastRunCycleTime=micros()-BEMFTimeStamp; 329 | BEMFTimeStamp=micros(); 330 | } 331 | } 332 | break; 333 | case 6: 334 | if(BEMFPinA==_BEMFHigh){ 335 | RunTime=micros()-BEMFTimeStamp; 336 | delayMicroseconds(2); 337 | BEMFPinA=digitalRead(14); 338 | if((BEMFPinA==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 339 | MotorPhase=1; 340 | LastRunCycleTime=micros()-BEMFTimeStamp; 341 | BEMFTimeStamp=micros(); 342 | } 343 | } 344 | break; 345 | default: MosPin_STOP; 346 | } 347 | 348 | } 349 | 350 | 351 | void SenseBEMFCCW(){ 352 | BEMFPinA=digitalRead(14); 353 | BEMFPinB=digitalRead(15); 354 | BEMFPinC=digitalRead(16); 355 | 356 | 357 | switch(MotorPhase){ 358 | case 1: 359 | if(BEMFPinC==_BEMFHigh){ 360 | RunTime=micros()-BEMFTimeStamp; 361 | delayMicroseconds(2); 362 | BEMFPinC=digitalRead(16); 363 | if((BEMFPinC==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 364 | MotorPhase++; 365 | LastRunCycleTime=micros()-BEMFTimeStamp; 366 | BEMFTimeStamp=micros(); 367 | } 368 | } 369 | break; 370 | case 2: 371 | if(BEMFPinA==_BEMFLow){ 372 | RunTime=micros()-BEMFTimeStamp; 373 | delayMicroseconds(2); 374 | BEMFPinA=digitalRead(14); 375 | if((BEMFPinA==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 376 | MotorPhase++; 377 | LastRunCycleTime=micros()-BEMFTimeStamp; 378 | BEMFTimeStamp=micros(); 379 | } 380 | } 381 | break; 382 | case 3: 383 | if(BEMFPinB==_BEMFHigh){ 384 | RunTime=micros()-BEMFTimeStamp; 385 | delayMicroseconds(2); 386 | BEMFPinB=digitalRead(15); 387 | if((BEMFPinB==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 388 | MotorPhase++; 389 | LastRunCycleTime=micros()-BEMFTimeStamp; 390 | BEMFTimeStamp=micros(); 391 | } 392 | } 393 | break; 394 | case 4: 395 | if(BEMFPinC==_BEMFLow){ 396 | RunTime=micros()-BEMFTimeStamp; 397 | delayMicroseconds(2); 398 | BEMFPinC=digitalRead(16); 399 | if((BEMFPinC==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 400 | MotorPhase++; 401 | LastRunCycleTime=micros()-BEMFTimeStamp; 402 | BEMFTimeStamp=micros(); 403 | } 404 | } 405 | break; 406 | case 5: 407 | if(BEMFPinA==_BEMFHigh){ 408 | RunTime=micros()-BEMFTimeStamp; 409 | delayMicroseconds(2); 410 | BEMFPinA=digitalRead(14); 411 | if((BEMFPinA==_BEMFHigh)&&(RunTime>(LastRunCycleTime/2))){ 412 | MotorPhase++; 413 | LastRunCycleTime=micros()-BEMFTimeStamp; 414 | BEMFTimeStamp=micros(); 415 | } 416 | } 417 | break; 418 | case 6: 419 | if(BEMFPinB==_BEMFLow){ 420 | RunTime=micros()-BEMFTimeStamp; 421 | delayMicroseconds(2); 422 | BEMFPinB=digitalRead(15); 423 | if((BEMFPinB==_BEMFLow)&&(RunTime>(LastRunCycleTime/2))){ 424 | MotorPhase=1; 425 | LastRunCycleTime=micros()-BEMFTimeStamp; 426 | BEMFTimeStamp=micros(); 427 | } 428 | } 429 | break; 430 | default: MosPin_STOP; 431 | } 432 | 433 | } 434 | /* 435 | 436 | void SenseBEMF(){ 437 | BEMFPinA=digitalRead(14); 438 | BEMFPinB=digitalRead(15); 439 | BEMFPinC=digitalRead(16); 440 | HallValue= digitalRead(14)*4+(int)digitalRead(15)*2+digitalRead(16); 441 | 442 | switch(MotorPhase){ 443 | case 1: //101 444 | 445 | if(sense==1){ 446 | if(BEMFPinC==_BEMFLow){ 447 | delayMicroseconds(1); 448 | BEMFPinC=digitalRead(16); 449 | if(BEMFPinC==_BEMFLow){ LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 450 | else{ 451 | //nothing 452 | } 453 | }else{ 454 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 455 | MotorPhase++; sense=1; 456 | } 457 | } 458 | 459 | break; 460 | case 2: //100 461 | 462 | if(sense==1){ 463 | if(BEMFPinB==_BEMFHigh){ 464 | delayMicroseconds(1); 465 | BEMFPinB=digitalRead(15); 466 | if(BEMFPinB==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 467 | else{ 468 | } 469 | 470 | }else{ 471 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 472 | MotorPhase++; sense=1; 473 | } 474 | } 475 | break; 476 | case 3: //110 477 | 478 | if(sense==1){ 479 | 480 | if(BEMFPinA==_BEMFLow){ 481 | delayMicroseconds(1); 482 | BEMFPinA=digitalRead(14); 483 | if(BEMFPinA==_BEMFLow){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;RunTest++;}} 484 | else{ 485 | 486 | } 487 | }else{ 488 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 489 | MotorPhase++; sense=1; 490 | } 491 | } 492 | break; 493 | case 4: //010 494 | 495 | if(sense==1){ 496 | if(BEMFPinC==_BEMFHigh){ 497 | delayMicroseconds(1); 498 | BEMFPinC=digitalRead(16); 499 | if(BEMFPinC==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 500 | else{ 501 | } 502 | }else{ 503 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 504 | MotorPhase++; sense=1; 505 | } 506 | } 507 | break; 508 | case 5: //011 509 | 510 | if(sense==1){ 511 | 512 | if(BEMFPinB==_BEMFLow){ 513 | delayMicroseconds(1); 514 | BEMFPinB=digitalRead(15); 515 | if(BEMFPinB==_BEMFLow){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 516 | else{ 517 | } 518 | }else{ 519 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 520 | MotorPhase++; sense=1; 521 | } 522 | } 523 | break; 524 | case 6: //001 525 | 526 | if(sense==1){ 527 | if(BEMFPinA==_BEMFHigh){ 528 | delayMicroseconds(1); 529 | BEMFPinA=digitalRead(14); 530 | if(BEMFPinA==_BEMFHigh){LastRunCycleTime=(micros()-BEMFTimeStamp)/3;BEMFTimeStamp=micros();sense=0;}} 531 | else{ 532 | 533 | } 534 | }else{ 535 | if(micros()-BEMFTimeStamp>LastRunCycleTime){ 536 | MotorPhase=1; sense=1; 537 | } 538 | } 539 | break; 540 | default: MosPin_STOP; 541 | } 542 | 543 | } 544 | */ 545 | void MotorCW(){ 546 | switch(HallValue){ 547 | case 5: //101 548 | MosPin_RUN1; 549 | break; 550 | case 4: //100 551 | MosPin_RUN2; 552 | break; 553 | case 6: //110 554 | MosPin_RUN3; 555 | break; 556 | case 2: //010 557 | MosPin_RUN4; 558 | break; 559 | case 3: //011 560 | MosPin_RUN5; 561 | break; 562 | case 1: //001 563 | MosPin_RUN6; 564 | break; 565 | default: MosPin_STOP; 566 | } 567 | } 568 | /* 569 | void MotorCCW(){ 570 | switch(HallValue){ 571 | case 5: //101 572 | MosPin_RUN3; 573 | break; 574 | case 4: //100 575 | MosPin_RUN4; 576 | break; 577 | case 6: //110 578 | MosPin_RUN5; 579 | break; 580 | case 2: //010 581 | MosPin_RUN6; 582 | break; 583 | case 3: //011 584 | MosPin_RUN1; 585 | break; 586 | case 1: //001 587 | MosPin_RUN2; 588 | break; 589 | default: MosPin_STOP; 590 | } 591 | } 592 | 593 | */ 594 | 595 | void MotorCCW(){ 596 | switch(HallValue){ 597 | case 5: //101 598 | MosPin_RUN4; 599 | break; 600 | case 4: //100 601 | MosPin_RUN5; 602 | break; 603 | case 6: //110 604 | MosPin_RUN6; 605 | break; 606 | case 2: //010 607 | MosPin_RUN1; 608 | break; 609 | case 3: //011 610 | MosPin_RUN2; 611 | break; 612 | case 1: //001 613 | MosPin_RUN3; 614 | break; 615 | default: MosPin_STOP; 616 | } 617 | } 618 | 619 | 620 | void CheckInputPin(){ 621 | RunPin=digitalRead(19); 622 | DirPin=digitalRead(18); 623 | //RunPin Check; 624 | if(RunEnable!=RunPin){ 625 | RunPinChangeCount++; 626 | if(RunPinChangeCount>_KeyMaxCount){ RunEnable=RunPin;RunPinChangeCount=0;} 627 | } 628 | else{ 629 | RunPinChangeCount=0; 630 | } 631 | 632 | //DirPin Check 633 | if(DirStatus!=DirPin){ 634 | DirPinChangeCount++; 635 | if(DirPinChangeCount>_KeyMaxCount){ DirStatus=DirPin;DirPinChangeCount=0;} 636 | } 637 | else{ 638 | DirPinChangeCount=0; 639 | } 640 | 641 | } 642 | 643 | void FlashError(int Err){ 644 | int TempDiff; 645 | if(FlashErr<(Err+1)){ 646 | if(FlashBegan==1){ThisFlashTime=millis();FlashBegan=0;} 647 | TempDiff=millis()-ThisFlashTime; 648 | if(TempDiff<100)digitalWrite(5,1); 649 | if(TempDiff>100&&TempDiff<500)digitalWrite(5,0); 650 | if(TempDiff>500){FlashBegan=1;FlashErr++;} 651 | } 652 | else{ 653 | if(FlashBegan==1){ThisFlashTime=millis();FlashBegan=0;} 654 | TempDiff=millis()-ThisFlashTime; 655 | if(TempDiff<1000){digitalWrite(5,0);}else{FlashErr=0;Serial.println(Error);Serial.println(ErrIshunt);} 656 | } 657 | 658 | } 659 | 660 | 661 | void CurrentCheck(){ 662 | 663 | //read the Current 664 | Ishunt=analogRead(A6); 665 | SubIshunt=SubIshunt-AvgIshunt+Ishunt; 666 | AvgIshunt=SubIshunt/5; 667 | //Startup Current 668 | if(AvgIshunt>_MaxUpIshunt&&RunStatus==1){Error=5;MosPin_STOP;ErrIshunt=AvgIshunt;} 669 | //Normal Current 670 | if(AvgIshunt>_MaxIshunt&&RunStatus>1){ 671 | CountErr++; 672 | if(CountErr>_MaxIshuntTime){Error=6;MosPin_STOP;ErrIshunt=AvgIshunt;} 673 | } 674 | else{ 675 | CountErr--; 676 | if(CountErr<0)CountErr=0; 677 | } 678 | //The peak current. 679 | if(Ishunt>_MaxMaxIshunt){Error=7;MosPin_STOP;ErrIshunt=Ishunt;} 680 | } 681 | 682 | void readEnvTemp(){ 683 | 684 | double Digital_Value=analogRead(A7); //读取串联电阻上的电压值（数字量） 685 | double Voltage_Value=(Digital_Value/1023)*5.00;//换算成模拟量的电压值 686 | double Rt_Value= Voltage_Value*82/(48-Voltage_Value); //计算出热敏电阻的阻值 687 | EnvTemperature=(double)1/(log(Rt_Value/10)/3980 + 1/( 25 + 273.15)) - 273.15; //计算所感知的温度并发送 688 | 689 | } 690 | 691 | 692 | void CheckMos(){ 693 | pwmWrite(10,100); 694 | //check UH 695 | digitalWrite(7,1);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 696 | delayMicroseconds(5); 697 | InitIshuntCheck(6); 698 | 699 | //check VH 700 | MosPin_STOP; 701 | delayMicroseconds(5); 702 | digitalWrite(7,0);digitalWrite(8,1);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 703 | delayMicroseconds(5); 704 | InitIshuntCheck(6); 705 | //Check WH 706 | MosPin_STOP; 707 | delayMicroseconds(5); 708 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,0); 709 | delayMicroseconds(5); 710 | InitIshuntCheck(6); 711 | //Check UL 712 | MosPin_STOP; 713 | delayMicroseconds(5); 714 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,1);digitalWrite(4,0);digitalWrite(6,0); 715 | delayMicroseconds(5); 716 | InitIshuntCheck(6); 717 | //Check VL 718 | MosPin_STOP; 719 | delayMicroseconds(5); 720 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,1);digitalWrite(6,0); 721 | delayMicroseconds(5); 722 | InitIshuntCheck(6); 723 | //Check WL 724 | MosPin_STOP; 725 | delayMicroseconds(5); 726 | digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,0); digitalWrite(3,0);digitalWrite(4,0);digitalWrite(6,1); 727 | delayMicroseconds(5); 728 | InitIshuntCheck(6); 729 | 730 | //Finish Test Close MosFET. 731 | MosPin_STOP; 732 | pwmWrite(10,0); 733 | } 734 | void InitIshuntCheck(int times){ 735 | for(int i=0;i_MaxIshuntCheck){while(1){MosPin_STOP;FlashError(1);};} 740 | } 741 | 742 | 743 | void SoundFreq(int ontime, int offtime) { 744 | int i=0; 745 | for(i=0; i1000)ReadPoint=1016; 790 | if(ReadPoint<50){Error=2;MotorStop();}else{if(Error==2)Error=0;} //when the pointmeter value <20, The motor will stop. 791 | TargetSpeed=map(ReadPoint,50,1016,M_MinSpeed,M_MaxSpeed); 792 | if(TargetSpeedM_MaxSpeed)TargetSpeed=M_MaxSpeed; 794 | } 795 | 796 | 797 | void MotorRunUp(){ 798 | int i; 799 | int j; 800 | if(SensorLess==1){ 801 | //Location the Roller; 802 | for(i=0;i<_MaxLocationPWM;i++){ 803 | if(Error==0){ 804 | NowPwm++; 805 | pwmWrite(10,NowPwm/4); 806 | MosPin_RUN1; 807 | delayMicroseconds(_MaxLocationTime); 808 | Ishunt=analogRead(A6); 809 | CurrentCheck(); 810 | 811 | }else{ 812 | break; 813 | } 814 | } 815 | MotorPhase=2; //The Next Phase; 816 | j=0; 817 | for(;;){ 818 | if(Error==0){ 819 | if(CurrentDirect==1){ 820 | SensorLessCW();} 821 | else{ 822 | SensorLessCCW(); 823 | } 824 | LastPhase=MotorPhase; 825 | if(j>(_MaxMotorUpStep)){ 826 | LastRunCycleTime=UpDelayTime[j]*_DelayTimes+_UpOffSetDelayTime; 827 | RunStatus=2; 828 | 829 | break; 830 | } 831 | for(i=0;i<_DelayTimes;i++){ 832 | delayMicroseconds(UpDelayTime[j]); 833 | delayMicroseconds(_UpOffSetDelayTime); 834 | } 835 | 836 | MotorPhase++; 837 | if(MotorPhase>6){ 838 | j++;MotorPhase=1; 839 | NowPwm++; 840 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 841 | pwmWrite(10,NowPwm/4); 842 | } 843 | CurrentCheck(); 844 | BEMFTimeStamp=micros(); 845 | }else 846 | { 847 | break; 848 | } 849 | } 850 | } 851 | else{ //Motor Up by Sensored method. 852 | if (UpdatePwm>_UpdatePWMTime){ 853 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; 854 | NowPwm=NowPwm+_PWMUpStep; 855 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 856 | pwmWrite(10,NowPwm/4); 857 | UpdatePwm=0; 858 | } 859 | ForceRun(); 860 | RightRun(); 861 | if(CountRun>18){ 862 | if(RightRunStatus==1){ 863 | RunStatus=2; 864 | } 865 | else{ 866 | Error=8; //Hall sequence wrong, 867 | } 868 | } 869 | 870 | if(NowPwm>_MaxUpPWM)NowPwm=_MaxUpPWM; 871 | if(CurrentDirect!=DirStatus){RunStatus=3; } 872 | } 873 | 874 | } 875 | 876 | void ForceRun(){ 877 | HallValue= digitalRead(11)*4+digitalRead(12)*2+digitalRead(13); 878 | if(HallValue==0||HallValue==7)Error=3; 879 | if(CurrentDirect==1){ 880 | MotorCCW(); 881 | } 882 | else{ 883 | MotorCW(); 884 | } 885 | CurrentCheck(); 886 | LastHallValue=HallValue; 887 | } 888 | 889 | 890 | void RightRun(){ 891 | switch(HallValue){ 892 | case 1: 893 | Hall_1=1; 894 | break; 895 | case 2: 896 | Hall_2=1; 897 | break; 898 | case 3: 899 | Hall_3=1; 900 | break; 901 | case 4: 902 | Hall_4=1; 903 | break; 904 | case 5: 905 | Hall_5=1; 906 | break; 907 | case 6: 908 | Hall_6=1; 909 | break; 910 | default: 911 | RightRunStatus=0; 912 | break; 913 | } 914 | RightRunStatus=Hall_1&&Hall_2&&Hall_3&&Hall_4&&Hall_5&&Hall_6; 915 | } 916 | 917 | void OpenRun(){ 918 | if (UpdatePwm>_UpdatePWMTime){ 919 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; 920 | if(ReadPoint>NowPwm){ 921 | NowPwm=NowPwm+_PWMUpStep; 922 | } 923 | else{ 924 | NowPwm=NowPwm-_PWMDownStep; 925 | } 926 | pwmWrite(10,NowPwm/4); 927 | UpdatePwm=0; 928 | if(CountRun>80&&M_LoopRun&&SensorLess==0)RunStatus=5; 929 | 930 | } 931 | } 932 | 933 | 934 | void LoopRun(){ 935 | 936 | if(DealLoop==1){ 937 | MotorActualSpeed=(60000000/AvgMotorStepTime)/_MotorPhasePairs; // 获得以RPM 为单位的速度，//MotorParameters[2]是极对数 938 | 939 | CEK = M_KP + M_KI + M_KD ; 940 | CEK1 = 0 - M_KP - 2*M_KD ; 941 | CEK2 = M_KD ; 942 | EK2 = EK1 ; 943 | EK1 = EK ; 944 | EK = TargetSpeed - MotorActualSpeed; 945 | 946 | YK = EK * CEK + EK1 * CEK1 + EK2 *CEK2 ; 947 | YK=YK/M_Scale; 948 | if(YK>50)YK=50; 949 | if(YK<-50)YK=-50; 950 | NowPwm=NowPwm+YK; 951 | 952 | if(NowPwm <= M_MinMinPWM) 953 | NowPwm = M_MinMinPWM; 954 | else if(NowPwm > M_MaxMaxPWM) 955 | NowPwm = M_MaxMaxPWM; 956 | pwmWrite(10,NowPwm/4); 957 | DealLoop=0; 958 | } 959 | } 960 | 961 | void MotorStop(){ 962 | NowPwm=0; 963 | pwmWrite(10,0); 964 | MosPin_STOP; 965 | RunStatus=0; 966 | CountRun=0; 967 | UpdateStallTime=0; 968 | if(Error==0)digitalWrite(5,0); 969 | } 970 | 971 | void loop(){ 972 | 973 | CheckInputPin(); 974 | if(RunEnable==0){ 975 | 976 | ReadPointValue(); 977 | if(Error==0){ 978 | switch(RunStatus){ 979 | case 0: 980 | // setP32( ); 981 | HallValue=0; 982 | HallValue= digitalRead(11)*4+digitalRead(12)*2+digitalRead(13); 983 | if(HallValue==0||HallValue==7){ 984 | SensorLess=1; 985 | if(SensorLessSound==1){ 986 | pwmWrite(10,60); 987 | Sound(3, 20, 80, 1200); 988 | pwmWrite(10,0); 989 | SensorLessSound=0; 990 | } 991 | } 992 | else{ 993 | SensorLess=0; 994 | SensorLessSound=1; 995 | } 996 | // FlexiTimer2::start(); 997 | CurrentDirect=DirStatus; 998 | UpdateStallTime=0; 999 | CountRun=0; 1000 | NowPwm=0; 1001 | Ishunt=3; //假设电流采样值 1002 | SubIshunt=15; //假设电流采样值 1003 | AvgIshunt=3; //假设电流采样值 1004 | MotorStepTime=0; 1005 | AvgMotorStepTime=0; 1006 | Hall_1=0;Hall_2=0;Hall_3=0;Hall_4=0;Hall_5=0;Hall_6=0;RightRunStatus=0; 1007 | RunStatus=1; 1008 | break; 1009 | case 1: 1010 | MotorRunUp(); 1011 | 1012 | break; 1013 | case 2: 1014 | OpenRun(); 1015 | if(CurrentDirect!=DirStatus)RunStatus=3; 1016 | break; 1017 | case 3: 1018 | // FlexiTimer2::stop(); 1019 | NowPwm=0; 1020 | pwmWrite(10,0); 1021 | MosPin_STOP; 1022 | delay(800); 1023 | MosPin_BRK; 1024 | delay(400); 1025 | MosPin_STOP; 1026 | delay(100); 1027 | CurrentDirect=DirStatus; 1028 | RunStatus=0; 1029 | break; 1030 | case 5: 1031 | LoopRun(); 1032 | if(CurrentDirect!=DirStatus)RunStatus=3; 1033 | break; 1034 | default: 1035 | Error=9; 1036 | MosPin_STOP; 1037 | break; 1038 | } 1039 | } 1040 | else{ 1041 | pwmWrite(10,0); 1042 | MosPin_STOP; 1043 | FlashError(Error); //flash the Error statues 1044 | } 1045 | if(UpdateStallTime>_MaxUpdateStallTime)Error=4; 1046 | if(UpdateStallTime>_MaxSensorLessUpdateStallTime&&SensorLess==1)Error=4; 1047 | } 1048 | else{ 1049 | MotorStop(); 1050 | } 1051 | CurrentCheck(); 1052 | readEnvTemp(); 1053 | //Debug for output data; 1054 | if((millis()-lastrtime)>1000){ 1055 | 1056 | Serial.print("ReadPoint"); 1057 | Serial.println(ReadPoint); 1058 | // Serial.print("Ishunt"); 1059 | // Serial.println(AvgIshunt); 1060 | // Serial.print("Cunrrent"); 1061 | // float tempi=AvgIshunt*125/256; 1062 | // Serial.println(tempi); 1063 | lastrtime=millis(); 1064 | } 1065 | 1066 | } 1067 | 1068 | 1069 | 1070 | --------------------------------------------------------------------------------