Sho Yama
LV8548 Motor Driver Stepper Motor Dc MOtor
Dependents: LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr
Embed:
(wiki syntax)
Show/hide line numbers
LV8548.cpp
00001 /** 00002 [LV8548.cpp] 00003 Copyright (c) [2018] [YAMASHO] 00004 This software is released under the MIT License. 00005 http://opensource.org/licenses/mit-license.php 00006 */ 00007 00008 #include "LV8548.h" 00009 #include "mbed.h" 00010 00011 #define _DEBUG_ (0) 00012 00013 #define COIL_A (0X1) 00014 #define COIL_B (0X2) 00015 #define COIL_C (0X4) 00016 #define COIL_D (0X8) 00017 00018 00019 00020 const uint8_t FullStepDrivePattern[STEPPHASEMAX] = 00021 #if 0 00022 {(COIL_A | COIL_D), 00023 (COIL_A | COIL_D), 00024 (COIL_A | COIL_B), 00025 (COIL_A | COIL_B), 00026 (COIL_B | COIL_C), 00027 (COIL_B | COIL_C), 00028 (COIL_C | COIL_D), 00029 (COIL_C | COIL_D)}; 00030 #else 00031 /// orgin 00032 {(COIL_A | COIL_C), 00033 (COIL_A | COIL_C), 00034 (COIL_B | COIL_C), 00035 (COIL_B | COIL_C), 00036 (COIL_B | COIL_D), 00037 (COIL_B | COIL_D), 00038 (COIL_A | COIL_D), 00039 (COIL_A | COIL_D)}; 00040 00041 #endif 00042 const uint8_t HaleStepDrivePattern[STEPPHASEMAX ] = 00043 #if 0 00044 {(COIL_A | COIL_D), 00045 (COIL_A), 00046 (COIL_A | COIL_B), 00047 (COIL_B), 00048 (COIL_B | COIL_C), 00049 (COIL_C), 00050 (COIL_C | COIL_D), 00051 (COIL_D)}; 00052 #else 00053 /// orgin 00054 {(COIL_A), 00055 (COIL_A | COIL_C), 00056 (COIL_C), 00057 (COIL_B | COIL_C), 00058 (COIL_B), 00059 (COIL_B | COIL_D), 00060 (COIL_D), 00061 (COIL_A | COIL_D)}; 00062 00063 #endif 00064 00065 00066 00067 LV8548::LV8548(PinName in1, PinName in2, PinName in3, PinName in4, DriverType drivertype,uint16_t baseus): 00068 _in1(in1),_in2(in2),_in3(in3),_in4(in4),_Drivertype(drivertype),_baseus(baseus) { 00069 00070 _in1 = 0; 00071 _in2 = 0; 00072 _in3 = 0; 00073 _in4 = 0; 00074 00075 _Pwmmode[MOTOR_A] = FWD_OPEN; 00076 _Pwmmode[MOTOR_B] = FWD_OPEN; 00077 00078 _Pwmout[MOTOR_A] = OUTPUT_OFF; 00079 _Pwmout[MOTOR_B] = OUTPUT_OFF; 00080 00081 _PwmPeriod[MOTOR_A] = 0.02f; // 20ms 00082 _PwmPeriod[MOTOR_B] = 0.02f; // 20ms 00083 00084 _PwmDuty[MOTOR_A] = 1.0f; // Duyt 100% 00085 _PwmDuty[MOTOR_B] = 1.0f; // Duyt 100% 00086 00087 if(_Drivertype == DCMOTOR ) return; // DC MotorにはTimerはない。 00088 if(_baseus == 0) return; // 0us のタイマも無し. 00089 00090 float TimeVal = (float)_baseus / 1000000; 00091 #if _DEBUG_ 00092 printf("TimerSetting %f",TimeVal); 00093 #endif 00094 _Lv8548BaseTimer.attach(this,&LV8548::SetStepTimerInt, TimeVal ); 00095 } 00096 00097 void LV8548::SetDcMotorStop(uint8_t ch) 00098 { 00099 if( _Drivertype != DCMOTOR ) return; 00100 if(ch == MOTOR_A) 00101 { 00102 _in1.write(0.0f); 00103 _in2.write(0.0f); 00104 } 00105 else 00106 { 00107 _in3.write(0.0f); 00108 _in4.write(0.0f); 00109 } 00110 } 00111 00112 void LV8548::SetDcMotor(uint8_t ch) 00113 { 00114 if( _Drivertype != DCMOTOR ) return; 00115 #if _DEBUG_ 00116 printf("Motor Set %2x %2x ",ch ,_Pwmout[ch]); 00117 #endif 00118 switch( _Pwmout[ch] ) 00119 { 00120 default: /* OUTPUT_OFF*/ 00121 if(ch == MOTOR_A) 00122 { 00123 _in1.write(0.0f); 00124 _in2.write(0.0f); 00125 #if _DEBUG_ 00126 printf("1:0.0 2:0.0 "); 00127 #endif 00128 } 00129 else 00130 if(ch == MOTOR_B) 00131 { 00132 _in3.write(0.0f); 00133 _in4.write(0.0f); 00134 #if _DEBUG_ 00135 printf("3:0.0 4:0.0 "); 00136 #endif 00137 } 00138 break; 00139 case OUTPUT_BRAKE: 00140 if(ch == MOTOR_A) 00141 { 00142 _in1.write(1.0f); 00143 _in2.write(1.0f); 00144 #if _DEBUG_ 00145 printf("1:1.0 2:1.0 "); 00146 #endif 00147 } 00148 else 00149 if(ch == MOTOR_B) 00150 { 00151 _in3.write(1.0f); 00152 _in4.write(1.0f); 00153 #if _DEBUG_ 00154 printf("3:1.0 4:1.0 "); 00155 #endif 00156 } 00157 break; 00158 case OUTPUT_START: 00159 if(ch == MOTOR_A) 00160 { 00161 switch(_Pwmmode[MOTOR_A]) 00162 { 00163 case FWD_BRAKE: /** FWD_BREAKE */ 00164 _in1.write(1.0f); 00165 _in2.write(1.0f-_PwmDuty[MOTOR_A]); 00166 #if _DEBUG_ 00167 printf("1:%f 2:1.0 ",_PwmDuty[MOTOR_A]); 00168 #endif 00169 break; 00170 case RVS_OPEN: /** REVERS OPEN */ 00171 _in1.write(0.0f); 00172 _in2.write(_PwmDuty[MOTOR_A]); 00173 #if _DEBUG_ 00174 printf("1:0.0 2:%f ",_PwmDuty[MOTOR_A]); 00175 #endif 00176 break; 00177 case RVS_BRAKE: /** REVSRS BRAKE */ 00178 _in1.write(1.0f-_PwmDuty[MOTOR_A]); 00179 _in2.write(1.0f); 00180 #if _DEBUG_ 00181 printf("1:1.0 2:%f ",_PwmDuty[MOTOR_A]); 00182 #endif 00183 break; 00184 default: /* FWD_OPEN */ 00185 _in1.write(_PwmDuty[MOTOR_A]); 00186 _in2.write(0.0f); 00187 #if _DEBUG_ 00188 printf("1:%f 2:0.0 ",_PwmDuty[MOTOR_A]); 00189 #endif 00190 break; 00191 } 00192 } 00193 else 00194 if(ch == MOTOR_B) 00195 { 00196 switch(_Pwmmode[MOTOR_B]) 00197 { 00198 case FWD_BRAKE: /** FWD_BREAKE */ 00199 _in3.write(1.0f); 00200 _in4.write(1.0f-_PwmDuty[MOTOR_B]); 00201 #if _DEBUG_ 00202 printf("3:%f 4:1.0 ",_PwmDuty[MOTOR_B]); 00203 #endif 00204 break; 00205 case RVS_OPEN: /** REVERS OPEN */ 00206 _in3.write(0.0f); 00207 _in4.write(_PwmDuty[MOTOR_B]); 00208 #if _DEBUG_ 00209 printf("3:0.0 4:%f",_PwmDuty[MOTOR_B]); 00210 #endif 00211 break; 00212 case RVS_BRAKE: /** REVSRS BRAKE */ 00213 _in3.write(1.0f-_PwmDuty[MOTOR_B]); 00214 _in4.write(1.0f); 00215 #if _DEBUG_ 00216 printf("3:1.0 4:%f ",_PwmDuty[MOTOR_B]); 00217 #endif 00218 break; 00219 default: /*FWD_OPEN FWD_OPEN */ 00220 _in3.write(_PwmDuty[MOTOR_B]); 00221 _in4.write(0.0f); 00222 #if _DEBUG_ 00223 printf("3:%f 4:0.0 ",_PwmDuty[1]); 00224 #endif 00225 break; 00226 } 00227 } 00228 break; 00229 } 00230 #if _DEBUG_ 00231 printf("\n"); 00232 #endif 00233 00234 } 00235 /***************************************************************************************************/ 00236 /* FFFFFF DDDD MM MM */ 00237 /* FF DD DD MMM MMM tt */ 00238 /* FF oooo rrrrr DD DD cccc MMMMMMM oooo tttttt oooo rrrrr */ 00239 /* FFFF oo oo rr rr DD DD cc MM M MM oo oo tt oo oo rr rr */ 00240 /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */ 00241 /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */ 00242 /* FF oooo rr DDDD cccc MM MM oooo ttt oooo rr */ 00243 /* */ 00244 /***************************************************************************************************/ 00245 void LV8548::SetDcPwmFreqency( uint8_t ch ,float Freq ) 00246 { 00247 if( _Drivertype != DCMOTOR ) return; 00248 00249 if(ch == MOTOR_A) 00250 { 00251 SetDcMotorStop(MOTOR_A); 00252 _PwmPeriod[MOTOR_A] = 1/Freq; 00253 #if USE_PWM_PORT 00254 _in1.period( _PwmPeriod[MOTOR_A] ); 00255 _in2.period( _PwmPeriod[MOTOR_A] ); 00256 #endif 00257 SetDcMotor(MOTOR_A); 00258 #if _DEBUG_ 00259 printf("freq0 : %f ",_PwmPeriod[MOTOR_A]); 00260 #endif 00261 }else 00262 if(ch == MOTOR_B) 00263 { 00264 SetDcMotorStop(MOTOR_B); 00265 _PwmPeriod[MOTOR_B] = 1/Freq; 00266 #if USE_PWM_PORT 00267 _in3.period( _PwmPeriod[MOTOR_B] ); 00268 _in4.period( _PwmPeriod[MOTOR_B] ); 00269 #endif 00270 #if _DEBUG_ 00271 printf("freq1 : %f ",_PwmPeriod[MOTOR_B]); 00272 #endif 00273 SetDcMotor(MOTOR_B); 00274 } 00275 00276 } 00277 00278 void LV8548::SetDcPwmMode( uint8_t ch ,DriverPwmMode Mode ) 00279 { 00280 if(ch < MOTOR_MAX) 00281 { 00282 #if _DEBUG_ 00283 printf("DC Pwm Set\n"); 00284 #endif 00285 00286 _Pwmmode[ch] = Mode; 00287 SetDcMotor(ch); 00288 } 00289 } 00290 00291 void LV8548::SetDcPwmDuty( uint8_t ch ,float Duty ) 00292 { 00293 if(ch < MOTOR_MAX) 00294 { 00295 #if _DEBUG_ 00296 printf("DC Duty Set\n"); 00297 #endif 00298 _PwmDuty[ch] = Duty; 00299 SetDcMotor(ch); 00300 } 00301 } 00302 00303 00304 void LV8548::SetDcOutPut( uint8_t ch ,DriverPwmOut Mode ) 00305 { 00306 if(ch < MOTOR_MAX) 00307 { 00308 #if _DEBUG_ 00309 printf("DC Out Set\n"); 00310 #endif 00311 _Pwmout[ch] = Mode; 00312 SetDcMotor(ch); 00313 } 00314 } 00315 00316 /*******************************************************************************************************************/ 00317 /* FFFFFF SSSS MM MM */ 00318 /* FF SS SS tt MMM MMM tt */ 00319 /* FF oooo rrrrr SS tttttt eeee ppppp MMMMMMM oooo tttttt oooo rrrrr */ 00320 /* FFFF oo oo rr rr SSSS tt ee ee pp pp MM M MM oo oo tt oo oo rr rr */ 00321 /* FF oo oo rr SS tt eeeeee pp pp MM MM oo oo tt oo oo rr */ 00322 /* FF oo oo rr SS SS tt ee ppppp MM MM oo oo tt oo oo rr */ 00323 /* FF oooo rr SSSS ttt eeee pp MM MM oooo ttt oooo rr */ 00324 /* pp */ 00325 /*******************************************************************************************************************/ 00326 void LV8548::SetStepAngle( uint16_t Angle) 00327 { 00328 _StepDeg = Angle; 00329 #if _DEBUG_ 00330 printf("Set Angle Deg:%f \n",_StepDeg); 00331 #endif 00332 } 00333 /// 周波数1～4800 00334 /// DEG 0～65535 00335 /// rotation 0:CW 1:CCW 00336 /// EXP 0:FullStep 1:HalfStep 00337 void LV8548::SetStepDeg ( uint16_t frequency , uint16_t deg , uint8_t Direction, uint8_t StepMode) 00338 { 00339 #if _DEBUG_ 00340 printf("Deg Commnad freq:%d deg:%d dir:%d Mode%d\n",frequency , deg , Direction, StepMode); 00341 #endif 00342 00343 if ( frequency == 0 ) return; 00344 00345 if( frequency > STEPMAX_FREQ ) 00346 { 00347 frequency = STEPMAX_FREQ; 00348 } 00349 00350 SetStepStop(); /* Step Motor 初期化 */ 00351 00352 _TragetStepDirection = (DriverMotorDirction )Direction; // CW/CCW 00353 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; 00354 if(_TargetStepMode != _NowStepMode) 00355 { /* 動作モード変更の為初期化 */ 00356 _StepPhase = 0; 00357 #if _DEBUG_ 00358 printf("Stepper Mode Change Then StepPhase 0\n"); 00359 #endif 00360 } 00361 #if 0 // 意味の無い処理なのでコメント化 00362 if( _TragetStepDirection != _NowStepDirection ) 00363 { // 方向の変更があった時 /// 00364 if( _TragetStepDirection == DIR_CW ) 00365 { 00366 _StepPhase++; //なぜ2パターン？？ 00367 _StepPhase++; // 元のソースは2パターン戻すが、必要なもないので削除 00368 } 00369 else 00370 if( _TragetStepDirection == DIR_CCW ) 00371 { 00372 _StepPhase--; //なぜ2パターン？？ 00373 _StepPhase--; // 元のソースは2パターン戻すが、必要なもないので削除 00374 } 00375 _StepPhase &= STEPPHASEMAX; // 0-7に！ 00376 } 00377 #endif 00378 _StepFreq = frequency ; 00379 if(_StepDeg) 00380 { 00381 _TargetStep = ( deg / _StepDeg); 00382 if(_TargetStepMode) 00383 { /* HALF時2倍 */ 00384 _TargetStep = _TargetStep + _TargetStep; 00385 } 00386 } 00387 else 00388 { 00389 _TargetStep = 0; 00390 } 00391 00392 #if _DEBUG_ 00393 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); 00394 #endif 00395 SetStepMotor(); 00396 } 00397 00398 void LV8548::SetStepTime ( uint16_t frequency , uint16_t time , uint8_t Direction, uint8_t StepMode) 00399 { 00400 #if _DEBUG_ 00401 printf("Time Commnad freq:%d Time:%d dir:%d Mode%d\n",frequency ,time, Direction, StepMode); 00402 #endif 00403 if ( frequency == 0 ) return; 00404 00405 if( frequency > STEPMAX_FREQ ) 00406 { 00407 frequency = STEPMAX_FREQ; 00408 } 00409 SetStepStop(); /* Step Motor 初期化 */ 00410 00411 _TragetStepDirection = (DriverMotorDirction )Direction; 00412 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; 00413 00414 if(_TargetStepMode != _NowStepMode) 00415 { /* 動作モード変更の為初期化 */ 00416 _StepPhase = 0; 00417 #if _DEBUG_ 00418 printf("Stepper Mode Change Then StepPhase 0\n"); 00419 #endif 00420 } 00421 00422 _StepFreq = frequency ; 00423 _TargetStep = ( time * frequency ); 00424 #if _DEBUG_ 00425 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); 00426 #endif 00427 SetStepMotor(); 00428 } 00429 00430 void LV8548::SetStepStep ( uint16_t frequency , uint16_t step , uint8_t Direction, uint8_t StepMode) 00431 { 00432 #if _DEBUG_ 00433 printf("Step Commnad freq:%d step:%d dir:%d Mode%d\n",frequency , step , Direction, StepMode); 00434 #endif 00435 if ( frequency == 0 ) return; 00436 00437 if( frequency > STEPMAX_FREQ ) 00438 { 00439 frequency = STEPMAX_FREQ; 00440 } 00441 SetStepStop(); /* Step Motor 初期化 */ 00442 00443 _TragetStepDirection = (DriverMotorDirction )Direction; 00444 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; 00445 00446 if(_TargetStepMode != _NowStepMode) 00447 { /* 動作モード変更の為初期化 */ 00448 _StepPhase = 0; 00449 #if _DEBUG_ 00450 printf("Stepper Mode Change Then StepPhase 0\n"); 00451 #endif 00452 } 00453 #if 0 00454 if( _TragetStepDirection != _NowStepDirection ) 00455 { // 方向の変更があった時 /// 00456 if( _TragetStepDirection == DIR_CW ) 00457 { 00458 _StepPhase++; //なぜ2パターン？？ 00459 _StepPhase++; 00460 } 00461 else 00462 if( _TragetStepDirection == DIR_CCW ) 00463 { 00464 _StepPhase--; //なぜ2パターン？？ 00465 _StepPhase--; 00466 } 00467 _StepPhase &= STEPPHASEMAX; // 0-7に！ 00468 } 00469 #endif 00470 _StepFreq = frequency; 00471 _TargetStep = step; 00472 #if _DEBUG_ 00473 printf("Analyze StepFreq:%f %f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); 00474 #endif 00475 SetStepMotor(); 00476 } 00477 void LV8548::SetStepStop ( void ) 00478 { 00479 if( _Drivertype != STEPERMOTOR ) return; 00480 /* 割込内の終了処理と同じ */ 00481 _StepOperation = false; 00482 _TimerCounter = 0.0f; 00483 _NowStep = 0; 00484 #if _DEBUG_ 00485 printf("StepStop:%d \n",_StepPhase ); 00486 #endif 00487 00488 } 00489 void LV8548::SetStepFree ( void ) 00490 { 00491 #if _DEBUG_ 00492 printf("StepFree:%d \n",_StepPhase ); 00493 #endif 00494 if( _Drivertype != STEPERMOTOR ) return; 00495 00496 SetStepStop(); 00497 00498 _StepPhase = 0; 00499 _in1.write(0.0f); 00500 _in2.write(0.0f); 00501 _in3.write(0.0f); 00502 _in4.write(0.0f); 00503 00504 } 00505 00506 void LV8548::SetStepMotor(void) 00507 { 00508 _StepOperation = false; // 00509 00510 _NowStepDirection = _TragetStepDirection; 00511 _NowStepMode = _TargetStepMode; 00512 _IntTimerCount = _StepTimeCount; /* 即初回次のシーケンス起動 なんで初回の1回も普通にカウントしているの？*/ 00513 if( _NowStepMode == FULLSTEP ) 00514 { 00515 _NowStep = 1; /* 波形比較により1/2シフト */ 00516 _StepTimeCount = 500000.0F / _StepFreq ; 00517 _TargetStep = _TargetStep + _TargetStep; /* FULL時は2倍 */ 00518 } 00519 else 00520 { 00521 _NowStep = 0; 00522 _StepTimeCount = 1000000.0F / _StepFreq; 00523 } 00524 #if _DEBUG_ 00525 printf("StepTimeCount:%d \n",_StepTimeCount ); 00526 #endif 00527 _StepOperation = true; 00528 } 00529 00530 void LV8548::SetStepTimerInt(void) 00531 { 00532 uint8_t OutputImage; 00533 if(!_StepOperation ) return; 00534 _IntTimerCount += _baseus; 00535 00536 if( _IntTimerCount >= _StepTimeCount) 00537 { 00538 _IntTimerCount = 0; 00539 if( _TargetStep ) 00540 { 00541 if(_NowStep >= _TargetStep) 00542 { // ステップ数よる停止条件の場合 */ 00543 /* Stop処理と同じ */ 00544 _StepOperation = false; 00545 _TimerCounter = 0.0f; 00546 _NowStep = 0; 00547 return; /* 元ソースと同じ動作にあわせる為 */ 00548 } 00549 /// Image 作成 ///////////////////// 00550 ++_NowStep; 00551 switch(_NowStepMode) 00552 { 00553 case FULLSTEP: 00554 OutputImage = FullStepDrivePattern[ _StepPhase ]; 00555 break; 00556 case HALFSTEP: 00557 OutputImage = HaleStepDrivePattern[ _StepPhase ]; 00558 break; 00559 default: 00560 OutputImage = 0; 00561 break; 00562 } 00563 #if _DEBUG_ 00564 printf("Ot:%x %x %x\n",OutputImage,_StepPhase,_TargetStepMode); 00565 #endif 00566 #if USE_PWM_PORT 00567 if( OutputImage & COIL_A ) _in1.write(1.0f); 00568 else _in1.write(0.0f); 00569 if( OutputImage & COIL_B ) _in2.write(1.0f); 00570 else _in2.write(0.0f); 00571 if( OutputImage & COIL_C ) _in3.write(1.0f); 00572 else _in3.write(0.0f); 00573 if( OutputImage & COIL_D ) _in4.write(1.0f); 00574 else _in4.write(0.0f); 00575 #else 00576 _in1 = (( OutputImage & COIL_A ) != 0); 00577 _in2 = (( OutputImage & COIL_B ) != 0); 00578 _in3 = (( OutputImage & COIL_C ) != 0); 00579 _in4 = (( OutputImage & COIL_D ) != 0); 00580 #endif 00581 if(_NowStepDirection) _StepPhase --; 00582 else _StepPhase ++; 00583 _StepPhase = _StepPhase & (STEPPHASEMAX -1); 00584 } 00585 } 00586 }
Generated on Wed Jul 13 2022 23:00:39 by
1.7.2