File content as of revision 2:627825272d30:

/**
[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 _DEBUG_     (0)

#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                             



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);
        }
    }
}