LV8548 Motor Driver Stepper Motor Dc MOtor
Dependents: LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr
Revision 0:04db82da014d, committed 2018-11-16
- Comitter:
- yamasho
- Date:
- Fri Nov 16 16:45:46 2018 +0000
- Child:
- 1:e60c7c42e6fc
- Commit message:
- DC Motor And StepMotor Support Fist Commit
Changed in this revision
LV8548.cpp | Show annotated file Show diff for this revision Revisions of this file |
LV8548.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LV8548.cpp Fri Nov 16 16:45:46 2018 +0000 @@ -0,0 +1,578 @@ +/** +[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; + _in1.period( _PwmPeriod[MOTOR_A] ); + _in2.period( _PwmPeriod[MOTOR_A] ); + 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; + _in2.period( _PwmPeriod[MOTOR_B] ); + _in3.period( _PwmPeriod[MOTOR_B] ); +#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( 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); + + + if(_NowStepDirection) _StepPhase --; + else _StepPhase ++; + _StepPhase = _StepPhase & (STEPPHASEMAX -1); + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LV8548.h Fri Nov 16 16:45:46 2018 +0000 @@ -0,0 +1,119 @@ +/** +[LV8548.h] +Copyright (c) [2018] [YAMASHO] +This software is released under the MIT License. +http://opensource.org/licenses/mit-license.php +*/ + +#ifndef MBED_LV8548_H +#define MBED_LV8548_H + +#include "mbed.h" + +#define MAXCH (2) + +#define STEPMAX_FREQ (4800) + +/** Motor Use */ + enum DriverType { + DCMOTOR /** DC Motor x2 */ + , STEPERMOTOR /** STEPER x1 */ +}; + +enum DriverPwmMode { + FWD_OPEN, /** FWD_OPEN */ + FWD_BRAKE, /** FWD_BREAKE */ + RVS_OPEN, /** REVERS OPEN */ + RVS_BRAKE /** REVSRS BRAKE */ +}; + +enum DriverPwmOut { + OUTPUT_OFF, /* OFF */ + OUTPUT_START, /* START */ + OUTPUT_BRAKE /* BRAAKE */ +}; + +/** Motor Number */ + enum DriverMotorNumber { + MOTOR_A = 0 + , MOTOR_B = 1 + , MOTOR_MAX +}; + + +/** Motor Dirction */ + enum DriverMotorDirction { + DIR_CW = 0 + , DIR_CCW = 1 + , DIR_MAX +}; + +/** Motor Step Mode */ + enum DriverStepMode { + FULLSTEP = 0 + , HALFSTEP = 1 + , MOTORSTEP_MAX +}; + +/** Motor Step Mode */ + enum DriverPhaseStep { + STEPPHASE0 = 0, + STEPPHASE1 = 1, + STEPPHASE2 = 2, + STEPPHASE3 = 3, + STEPPHASE4 = 4, + STEPPHASE5 = 5, + STEPPHASE6 = 6, + STEPPHASE7 = 7, + STEPPHASEMAX = 8 /* 必ず8 */ +}; + +class LV8548 { +public: + LV8548(PinName in1, PinName in2, PinName in3, PinName in4, DriverType drivertype = DCMOTOR ,uint16_t baseus = 64); + PwmOut _in1,_in2,_in3,_in4; + DriverType _Drivertype; + uint16_t _baseus; +/***************************************************************************************************/ + DriverPwmMode _Pwmmode[MOTOR_MAX]; // pwmのモード FWD.OPEN FWD.BRAKE RVS.OPEN REV.BRAKE + DriverPwmOut _Pwmout [MOTOR_MAX]; // Start Stop Breke + float _PwmPeriod [MOTOR_MAX]; // ポートのPwm周波数 + float _PwmDuty [MOTOR_MAX]; // 各Motor のduty 1.0f-0.0fまで +// uint8_t _testx; + + Ticker _Lv8548BaseTimer; + + void SetDcPwmFreqency( uint8_t ch ,float Freq ); + void SetDcPwmMode( uint8_t ch ,DriverPwmMode Mode ); + void SetDcPwmDuty( uint8_t ch ,float Duty ); // 01まで + void SetDcOutPut ( uint8_t ch ,DriverPwmOut Mode ); + void SetDcMotor ( uint8_t ch ); + void SetDcMotorStop( uint8_t ch ); +/*******************************************************************************************************************/ + float _StepAngle; + float _StepDeg; // 角度 0-360度 + float _StepFreq; // 周波数 + float _TimerCounter; + volatile uint8_t _StepPhase; // Step Motorのphase + volatile uint32_t _IntTimerCount; +// volatile uint32_t _PhaseCounter; + volatile uint32_t _StepTimeCount; + volatile bool _StepOperation; // 割込内の動作状況 + volatile DriverStepMode _TargetStepMode; // 目標のStep状態 Full(0)/Half(1) + volatile DriverStepMode _NowStepMode; // 動作中のStep状態 Full(0)/Half(1) + volatile DriverMotorDirction _TragetStepDirection; // 目標のDirection Cw/Ccw + volatile DriverMotorDirction _NowStepDirection; // 動作中のDirection cw/CCW + volatile uint32_t _NowStep; // 現状のStep + volatile uint32_t _TargetStep; // ターゲットのStep + void SetStepAngle( uint16_t Angle); // 角度の設定 + void SetStepDeg ( uint16_t frequency , uint16_t deg , uint8_t Direction, uint8_t StepMode); + void SetStepTime ( uint16_t frequency , uint16_t time , uint8_t Direction, uint8_t StepMode); + void SetStepStep ( uint16_t frequency , uint16_t step , uint8_t Direction, uint8_t StepMode); + void SetStepStop ( void ); + void SetStepFree ( void ); + void SetStepMotor( void ); + void SetStepTimerInt ( void ); +}; + + +#endif