Sho Yama
LV8548 Motor Driver Stepper Motor Dc MOtor
Dependents: LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr
Diff: LV8548.cpp
- Revision:
- 0:04db82da014d
- Child:
- 1:e60c7c42e6fc
--- /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);
+ }
+ }
+}