LV8548 Motor Driver Stepper Motor Dc MOtor
Dependents: LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr
LV8548.cpp
- Committer:
- yamasho
- Date:
- 2018-11-17
- Revision:
- 1:e60c7c42e6fc
- Parent:
- 0:04db82da014d
- Child:
- 2:627825272d30
File content as of revision 1:e60c7c42e6fc:
/** [LV8548.cpp] Copyright (c) [2018] [YAMASHO] This software is released under the MIT License. http://opensource.org/licenses/mit-license.php */ #include "LV8548.h" #include "mbed.h" #define COIL_A (0X1) #define COIL_B (0X2) #define COIL_C (0X4) #define COIL_D (0X8) const uint8_t FullStepDrivePattern[STEPPHASEMAX] = #if 0 {(COIL_A | COIL_D), (COIL_A | COIL_D), (COIL_A | COIL_B), (COIL_A | COIL_B), (COIL_B | COIL_C), (COIL_B | COIL_C), (COIL_C | COIL_D), (COIL_C | COIL_D)}; #else /// orgin {(COIL_A | COIL_C), (COIL_A | COIL_C), (COIL_B | COIL_C), (COIL_B | COIL_C), (COIL_B | COIL_D), (COIL_B | COIL_D), (COIL_A | COIL_D), (COIL_A | COIL_D)}; #endif const uint8_t HaleStepDrivePattern[STEPPHASEMAX ] = #if 0 {(COIL_A | COIL_D), (COIL_A), (COIL_A | COIL_B), (COIL_B), (COIL_B | COIL_C), (COIL_C), (COIL_C | COIL_D), (COIL_D)}; #else /// orgin {(COIL_A), (COIL_A | COIL_C), (COIL_C), (COIL_B | COIL_C), (COIL_B), (COIL_B | COIL_D), (COIL_D), (COIL_A | COIL_D)}; #endif #define _DEBUG_ (0) LV8548::LV8548(PinName in1, PinName in2, PinName in3, PinName in4, DriverType drivertype,uint16_t baseus): _in1(in1),_in2(in2),_in3(in3),_in4(in4),_Drivertype(drivertype),_baseus(baseus) { _in1 = 0; _in2 = 0; _in3 = 0; _in4 = 0; _Pwmmode[MOTOR_A] = FWD_OPEN; _Pwmmode[MOTOR_B] = FWD_OPEN; _Pwmout[MOTOR_A] = OUTPUT_OFF; _Pwmout[MOTOR_B] = OUTPUT_OFF; _PwmPeriod[MOTOR_A] = 0.02f; // 20ms _PwmPeriod[MOTOR_B] = 0.02f; // 20ms _PwmDuty[MOTOR_A] = 1.0f; // Duyt 100% _PwmDuty[MOTOR_B] = 1.0f; // Duyt 100% if(_Drivertype == DCMOTOR ) return; // DC MotorにはTimerはない。 if(_baseus == 0) return; // 0us のタイマも無し. float TimeVal = (float)_baseus / 1000000; #if _DEBUG_ printf("TimerSetting %f",TimeVal); #endif _Lv8548BaseTimer.attach(this,&LV8548::SetStepTimerInt, TimeVal ); } void LV8548::SetDcMotorStop(uint8_t ch) { if( _Drivertype != DCMOTOR ) return; if(ch == MOTOR_A) { _in1.write(0.0f); _in2.write(0.0f); } else { _in3.write(0.0f); _in4.write(0.0f); } } void LV8548::SetDcMotor(uint8_t ch) { if( _Drivertype != DCMOTOR ) return; #if _DEBUG_ printf("Motor Set %2x %2x ",ch ,_Pwmout[ch]); #endif switch( _Pwmout[ch] ) { default: /* OUTPUT_OFF*/ if(ch == MOTOR_A) { _in1.write(0.0f); _in2.write(0.0f); #if _DEBUG_ printf("1:0.0 2:0.0 "); #endif } else if(ch == MOTOR_B) { _in3.write(0.0f); _in4.write(0.0f); #if _DEBUG_ printf("3:0.0 4:0.0 "); #endif } break; case OUTPUT_BRAKE: if(ch == MOTOR_A) { _in1.write(1.0f); _in2.write(1.0f); #if _DEBUG_ printf("1:1.0 2:1.0 "); #endif } else if(ch == MOTOR_B) { _in3.write(1.0f); _in4.write(1.0f); #if _DEBUG_ printf("3:1.0 4:1.0 "); #endif } break; case OUTPUT_START: if(ch == MOTOR_A) { switch(_Pwmmode[MOTOR_A]) { case FWD_BRAKE: /** FWD_BREAKE */ _in1.write(1.0f); _in2.write(1.0f-_PwmDuty[MOTOR_A]); #if _DEBUG_ printf("1:%f 2:1.0 ",_PwmDuty[MOTOR_A]); #endif break; case RVS_OPEN: /** REVERS OPEN */ _in1.write(0.0f); _in2.write(_PwmDuty[MOTOR_A]); #if _DEBUG_ printf("1:0.0 2:%f ",_PwmDuty[MOTOR_A]); #endif break; case RVS_BRAKE: /** REVSRS BRAKE */ _in1.write(1.0f-_PwmDuty[MOTOR_A]); _in2.write(1.0f); #if _DEBUG_ printf("1:1.0 2:%f ",_PwmDuty[MOTOR_A]); #endif break; default: /* FWD_OPEN */ _in1.write(_PwmDuty[MOTOR_A]); _in2.write(0.0f); #if _DEBUG_ printf("1:%f 2:0.0 ",_PwmDuty[MOTOR_A]); #endif break; } } else if(ch == MOTOR_B) { switch(_Pwmmode[MOTOR_B]) { case FWD_BRAKE: /** FWD_BREAKE */ _in3.write(1.0f); _in4.write(1.0f-_PwmDuty[MOTOR_B]); #if _DEBUG_ printf("3:%f 4:1.0 ",_PwmDuty[MOTOR_B]); #endif break; case RVS_OPEN: /** REVERS OPEN */ _in3.write(0.0f); _in4.write(_PwmDuty[MOTOR_B]); #if _DEBUG_ printf("3:0.0 4:%f",_PwmDuty[MOTOR_B]); #endif break; case RVS_BRAKE: /** REVSRS BRAKE */ _in3.write(1.0f-_PwmDuty[MOTOR_B]); _in4.write(1.0f); #if _DEBUG_ printf("3:1.0 4:%f ",_PwmDuty[MOTOR_B]); #endif break; default: /*FWD_OPEN FWD_OPEN */ _in3.write(_PwmDuty[MOTOR_B]); _in4.write(0.0f); #if _DEBUG_ printf("3:%f 4:0.0 ",_PwmDuty[1]); #endif break; } } break; } #if _DEBUG_ printf("\n"); #endif } /***************************************************************************************************/ /* FFFFFF DDDD MM MM */ /* FF DD DD MMM MMM tt */ /* FF oooo rrrrr DD DD cccc MMMMMMM oooo tttttt oooo rrrrr */ /* FFFF oo oo rr rr DD DD cc MM M MM oo oo tt oo oo rr rr */ /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */ /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */ /* FF oooo rr DDDD cccc MM MM oooo ttt oooo rr */ /* */ /***************************************************************************************************/ void LV8548::SetDcPwmFreqency( uint8_t ch ,float Freq ) { if( _Drivertype != DCMOTOR ) return; if(ch == MOTOR_A) { SetDcMotorStop(MOTOR_A); _PwmPeriod[MOTOR_A] = 1/Freq; #if USE_PWM_PORT _in1.period( _PwmPeriod[MOTOR_A] ); _in2.period( _PwmPeriod[MOTOR_A] ); #endif SetDcMotor(MOTOR_A); #if _DEBUG_ printf("freq0 : %f ",_PwmPeriod[MOTOR_A]); #endif }else if(ch == MOTOR_B) { SetDcMotorStop(MOTOR_B); _PwmPeriod[MOTOR_B] = 1/Freq; #if USE_PWM_PORT _in3.period( _PwmPeriod[MOTOR_B] ); _in4.period( _PwmPeriod[MOTOR_B] ); #endif #if _DEBUG_ printf("freq1 : %f ",_PwmPeriod[MOTOR_B]); #endif SetDcMotor(MOTOR_B); } } void LV8548::SetDcPwmMode( uint8_t ch ,DriverPwmMode Mode ) { if(ch < MOTOR_MAX) { #if _DEBUG_ printf("DC Pwm Set\n"); #endif _Pwmmode[ch] = Mode; SetDcMotor(ch); } } void LV8548::SetDcPwmDuty( uint8_t ch ,float Duty ) { if(ch < MOTOR_MAX) { #if _DEBUG_ printf("DC Duty Set\n"); #endif _PwmDuty[ch] = Duty; SetDcMotor(ch); } } void LV8548::SetDcOutPut( uint8_t ch ,DriverPwmOut Mode ) { if(ch < MOTOR_MAX) { #if _DEBUG_ printf("DC Out Set\n"); #endif _Pwmout[ch] = Mode; SetDcMotor(ch); } } /*******************************************************************************************************************/ /* FFFFFF SSSS MM MM */ /* FF SS SS tt MMM MMM tt */ /* FF oooo rrrrr SS tttttt eeee ppppp MMMMMMM oooo tttttt oooo rrrrr */ /* FFFF oo oo rr rr SSSS tt ee ee pp pp MM M MM oo oo tt oo oo rr rr */ /* FF oo oo rr SS tt eeeeee pp pp MM MM oo oo tt oo oo rr */ /* FF oo oo rr SS SS tt ee ppppp MM MM oo oo tt oo oo rr */ /* FF oooo rr SSSS ttt eeee pp MM MM oooo ttt oooo rr */ /* pp */ /*******************************************************************************************************************/ void LV8548::SetStepAngle( uint16_t Angle) { _StepDeg = Angle; #if _DEBUG_ printf("Set Angle Deg:%f \n",_StepDeg); #endif } /// 周波数1~4800 /// DEG 0~65535 /// rotation 0:CW 1:CCW /// EXP 0:FullStep 1:HalfStep void LV8548::SetStepDeg ( uint16_t frequency , uint16_t deg , uint8_t Direction, uint8_t StepMode) { #if _DEBUG_ printf("Deg Commnad freq:%d deg:%d dir:%d Mode%d\n",frequency , deg , Direction, StepMode); #endif if ( frequency == 0 ) return; if( frequency > STEPMAX_FREQ ) { frequency = STEPMAX_FREQ; } SetStepStop(); /* Step Motor 初期化 */ _TragetStepDirection = (DriverMotorDirction )Direction; // CW/CCW _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; if(_TargetStepMode != _NowStepMode) { /* 動作モード変更の為初期化 */ _StepPhase = 0; #if _DEBUG_ printf("Stepper Mode Change Then StepPhase 0\n"); #endif } #if 0 // 意味の無い処理なのでコメント化 if( _TragetStepDirection != _NowStepDirection ) { // 方向の変更があった時 /// if( _TragetStepDirection == DIR_CW ) { _StepPhase++; //なぜ2パターン?? _StepPhase++; // 元のソースは2パターン戻すが、必要なもないので削除 } else if( _TragetStepDirection == DIR_CCW ) { _StepPhase--; //なぜ2パターン?? _StepPhase--; // 元のソースは2パターン戻すが、必要なもないので削除 } _StepPhase &= STEPPHASEMAX; // 0-7に! } #endif _StepFreq = frequency ; if(_StepDeg) { _TargetStep = ( deg / _StepDeg); if(_TargetStepMode) { /* HALF時2倍 */ _TargetStep = _TargetStep + _TargetStep; } } else { _TargetStep = 0; } #if _DEBUG_ printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); #endif SetStepMotor(); } void LV8548::SetStepTime ( uint16_t frequency , uint16_t time , uint8_t Direction, uint8_t StepMode) { #if _DEBUG_ printf("Time Commnad freq:%d Time:%d dir:%d Mode%d\n",frequency ,time, Direction, StepMode); #endif if ( frequency == 0 ) return; if( frequency > STEPMAX_FREQ ) { frequency = STEPMAX_FREQ; } SetStepStop(); /* Step Motor 初期化 */ _TragetStepDirection = (DriverMotorDirction )Direction; _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; if(_TargetStepMode != _NowStepMode) { /* 動作モード変更の為初期化 */ _StepPhase = 0; #if _DEBUG_ printf("Stepper Mode Change Then StepPhase 0\n"); #endif } _StepFreq = frequency ; _TargetStep = ( time * frequency ); #if _DEBUG_ printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); #endif SetStepMotor(); } void LV8548::SetStepStep ( uint16_t frequency , uint16_t step , uint8_t Direction, uint8_t StepMode) { #if _DEBUG_ printf("Step Commnad freq:%d step:%d dir:%d Mode%d\n",frequency , step , Direction, StepMode); #endif if ( frequency == 0 ) return; if( frequency > STEPMAX_FREQ ) { frequency = STEPMAX_FREQ; } SetStepStop(); /* Step Motor 初期化 */ _TragetStepDirection = (DriverMotorDirction )Direction; _TargetStepMode = (DriverStepMode )StepMode; // Full/Half; if(_TargetStepMode != _NowStepMode) { /* 動作モード変更の為初期化 */ _StepPhase = 0; #if _DEBUG_ printf("Stepper Mode Change Then StepPhase 0\n"); #endif } #if 0 if( _TragetStepDirection != _NowStepDirection ) { // 方向の変更があった時 /// if( _TragetStepDirection == DIR_CW ) { _StepPhase++; //なぜ2パターン?? _StepPhase++; } else if( _TragetStepDirection == DIR_CCW ) { _StepPhase--; //なぜ2パターン?? _StepPhase--; } _StepPhase &= STEPPHASEMAX; // 0-7に! } #endif _StepFreq = frequency; _TargetStep = step; #if _DEBUG_ printf("Analyze StepFreq:%f %f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase); #endif SetStepMotor(); } void LV8548::SetStepStop ( void ) { if( _Drivertype != STEPERMOTOR ) return; /* 割込内の終了処理と同じ */ _StepOperation = false; _TimerCounter = 0.0f; _NowStep = 0; #if _DEBUG_ printf("StepStop:%d \n",_StepPhase ); #endif } void LV8548::SetStepFree ( void ) { #if _DEBUG_ printf("StepFree:%d \n",_StepPhase ); #endif if( _Drivertype != STEPERMOTOR ) return; SetStepStop(); _StepPhase = 0; _in1.write(0.0f); _in2.write(0.0f); _in3.write(0.0f); _in4.write(0.0f); } void LV8548::SetStepMotor(void) { _StepOperation = false; // _NowStepDirection = _TragetStepDirection; _NowStepMode = _TargetStepMode; _IntTimerCount = _StepTimeCount; /* 即初回次のシーケンス起動 なんで初回の1回も普通にカウントしているの?*/ if( _NowStepMode == FULLSTEP ) { _NowStep = 1; /* 波形比較により1/2シフト */ _StepTimeCount = 500000.0F / _StepFreq ; _TargetStep = _TargetStep + _TargetStep; /* FULL時は2倍 */ } else { _NowStep = 0; _StepTimeCount = 1000000.0F / _StepFreq; } #if _DEBUG_ printf("StepTimeCount:%d \n",_StepTimeCount ); #endif _StepOperation = true; } void LV8548::SetStepTimerInt(void) { uint8_t OutputImage; if(!_StepOperation ) return; _IntTimerCount += _baseus; if( _IntTimerCount >= _StepTimeCount) { _IntTimerCount = 0; if( _TargetStep ) { if(_NowStep >= _TargetStep) { // ステップ数よる停止条件の場合 */ /* Stop処理と同じ */ _StepOperation = false; _TimerCounter = 0.0f; _NowStep = 0; return; /* 元ソースと同じ動作にあわせる為 */ } /// Image 作成 ///////////////////// ++_NowStep; switch(_NowStepMode) { case FULLSTEP: OutputImage = FullStepDrivePattern[ _StepPhase ]; break; case HALFSTEP: OutputImage = HaleStepDrivePattern[ _StepPhase ]; break; default: OutputImage = 0; break; } #if _DEBUG_ printf("Ot:%x %x %x\n",OutputImage,_StepPhase,_TargetStepMode); #endif #if USE_PWM_PORT if( OutputImage & COIL_A ) _in1.write(1.0f); else _in1.write(0.0f); if( OutputImage & COIL_B ) _in2.write(1.0f); else _in2.write(0.0f); if( OutputImage & COIL_C ) _in3.write(1.0f); else _in3.write(0.0f); if( OutputImage & COIL_D ) _in4.write(1.0f); else _in4.write(0.0f); #else _in1 = (( OutputImage & COIL_A ) != 0); _in2 = (( OutputImage & COIL_B ) != 0); _in3 = (( OutputImage & COIL_C ) != 0); _in4 = (( OutputImage & COIL_D ) != 0); #endif if(_NowStepDirection) _StepPhase --; else _StepPhase ++; _StepPhase = _StepPhase & (STEPPHASEMAX -1); } } }