/*******************************************************************************
                            青岛市张彦欣单片机有限公司
                                版权所有  @ 2018
基于DDA算法的2轴插补代码
输出信号分为脉冲信号Pls和方向引脚Dir.必须接步进电机驱动器或伺服电机驱动器.

张彦欣
2018.02.08
*******************************************************************************/
#include "MaxXYHiSpd.h"



/*******************************************************************************
函 数: MaxXYHiSpd(void)
功 能: 构造函数.完成变量的初始化.

张彦欣
2018.02.08
*******************************************************************************/
MaxXYHiSpd::MaxXYHiSpd(void)
{
    system.RunDirX = DIR_ZHENG; //电机的转动方向.
    system.RunDirY = DIR_ZHENG;

    system.TotalSteps   = 0;    //总步数
    system.RemainSteps  = 0;    //剩余步数.
    system.CurrentSteps = 0;    //当前步数.

    system.x_TotalSteps = 0;
    system.y_TotalSteps = 0;
    
    system.x_CurrentSteps = 0;
    system.y_CurrentSteps = 0;
    
    system.x_location = 0;      //记录当前位置.
    system.y_location = 0;
    
	system.CurFrq = 0;          //记录当前速度.
	system.State  = MOTOR_STOP; //电机运行状态(加减速状态)

    config.PIN_StepX = 0;       //驱动引脚.脉冲引脚.
    config.PIN_DirX  = 0;       //驱动引脚.方向引脚.
    
    config.PIN_StepY = 0;
    config.PIN_DirY  = 0;
    
    config.PlsPerCir = 200;     //步进电机转动一圈,所需要的脉冲数量.
    
    config.DirOppX = false;     //运动方向是否反向.
    config.DirOppY = false;

    config.RPM          = 0;            //电机的转速.config.RPM.    
    config.TargetFrq    = 0;            //目标频率.Hz.
    config.MinPlsFrq    = MIN_PLS_FREQ; //
    config.StopLevel    = LOW;          //电机停止时输出的电平.

//  config.EnLsw      = false;      //限位开关使能标志位.
//  config.LswLevel   = LOW;        //当限位开关动作时的电平.默认高电平.
//  config.PIN_LswPos = NULL;       //增量限位开关引脚
//  config.PIN_LswNag = NULL;       //减量限位开关引脚
//  system.LSW_PosFlg = false;      //限位开关动作标志位(正方向)
//  system.LSW_NagFlg = false;      //限位开关动作标志位(反方向)
    
    ramp.EnRamp     = false;        //斜坡功能使能标志.
    ramp.RampLength = 200;			//斜坡函数--加速的长度.
    
    limitswitch.Enable   = false;    
    limitswitch.XFlg     = false;
    limitswitch.YFlg     = false;
    limitswitch.ActLevel = HIGH;
    
    limitswitch.PIN_XMax = 0;       //X轴最大限位
    limitswitch.PIN_XMin = 0;       //X轴最小限位
    limitswitch.PIN_YMax = 0;       //Y轴最大限位
    limitswitch.PIN_YMin = 0;       //Y轴最小限位
    
    locationtrig.Enable  = false;   //默认关闭脉冲触发功能
}

/*******************************************************************************
函 数: void MaxXYHiSpd::begin(	TIM_TypeDef* myTimer,
                                Uint  myPlsPerCir,
                                Uint  myXPls,  Uint  myXDir,
                                Uint  myYPls,  Uint  myYDir,
                                Uchar PlsMod,  Uchar DirMod,
                                Uchar PlsStopLvl )
功 能: 初始化各种参数.
参 数: myTimer      -- 驱动电机使用的定时计数器.如:TIM1
       myPlsPerCir  -- 电机转动一圈所需要的脉冲数量.
       myXPls/myYPls-- X轴电机或Y轴电机的脉冲引脚.
       myXDir/myYDir-- X轴电机或Y轴电机的方向引脚.
       PlsMod/DirMod-- 脉冲引脚和方向引脚的IO口驱动方式 OUTPUT/OUTPUT_OD
       PlsStopLvl   -- 电机停止的时候IO输出的电平
张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::begin(	TIM_TypeDef* myTimer,
                        Uint  myPlsPerCir,
                        Uint  myXPls,  Uint  myXDir,
                        Uint  myYPls,  Uint  myYDir,
                        Uchar PlsMod,  Uchar DirMod,
                        Uchar PlsStopLvl )
{
	config.DrvTimer.begin(myTimer);         //步进电机使用的定时器.
    config.DrvTimer.set(1000);              //初始化设置为1000Hz中断频率(无所谓)
    config.DrvTimer.close();                //关闭定时器
    
    config.PlsPerCir = myPlsPerCir;
    
    config.PIN_StepX = myXPls;
    config.PIN_DirX  = myXDir;
    
    config.PIN_StepY = myYPls;
    config.PIN_DirY  = myYDir;
    
    config.StopLevel = PlsStopLvl;          //停止电平
    
    pinMode(config.PIN_StepX, PlsMod);      //脉冲引脚
    pinMode(config.PIN_StepY, PlsMod);
    digitalWrite(config.PIN_StepX, config.StopLevel);
    digitalWrite(config.PIN_StepY, config.StopLevel);
    
    pinMode(config.PIN_DirX, DirMod);       //方向引脚.
    pinMode(config.PIN_DirY, DirMod);
    digitalWrite(config.PIN_DirX, system.RunDirX);
    digitalWrite(config.PIN_DirY, system.RunDirY);

    system.CurFrq = 0;                      //当前定时器中断频率

    stop();                                 //停止电机.停止任何输出.
}

// 简化的begin()函数
void MaxXYHiSpd::begin( TIM_TypeDef* myTimer,
                        Uint  myPlsPerCir,
                        Uint  myXPls,  Uint  myXDir,
                        Uint  myYPls,  Uint  myYDir )
{
    begin( myTimer,myPlsPerCir,
           myXPls,myXDir,
           myYPls,myYDir,
           OUTPUT,OUTPUT,LOW );
}

/*******************************************************************************
函 数: void OS_Direction(void)
功 能: 电机运动方向控制.根据运动方向控制方向引脚的输出.

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_Direction(void)
{
	if( config.DirOppX )    //电机是否正反转对调
		digitalWrite( config.PIN_DirX,!system.RunDirX );
	else
		digitalWrite( config.PIN_DirX, system.RunDirX );

    if( config.DirOppY )    //电机是否正反转对调
		digitalWrite( config.PIN_DirY,!system.RunDirY );
	else
		digitalWrite( config.PIN_DirY, system.RunDirY );
}

/*******************************************************************************
函 数: void OS_LoadTimer(void)
功 能: 加载定时计数器频率.每次中断生成一个脉冲.

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_LoadTimer(void)
{
    config.DrvTimer.set(system.CurFrq);
}

/*******************************************************************************
函 数: Uchar CurDir(void)
功 能: 获取当前电机的运动方向.

张彦欣
2018.02.08
*******************************************************************************/
Uchar MaxXYHiSpd::CurDir(Uchar myMOTOR)
{
    if( myMOTOR==MOTOR_X )
        return (system.RunDirX);
    else
        return (system.RunDirY);
}

/*******************************************************************************
函 数: void speed(float myRPM)
功 能: 设置步进电机的转速.
参 数: myRPM -- 步进电机转动速度.单位RPM.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::speed(float myRPM)
{
	if(myRPM<=0)                //设置的转速小于0.不正常.
	{
        stop();                 //停止电机.
        config.RPM=0;
        config.TargetFrq=0;
		return;
	}

    if( myRPM==config.RPM )     //要设置的速度值没有发生变化.
    {
		return;
    }

    config.RPM       = myRPM;   //转速 RPM
    config.TargetFrq = (config.RPM)*(float)(config.PlsPerCir)/60.00;
    
    // 用户设置的RampLength(加减速长度),是300RPM转速下的加减速长度
    // 要根据当前的速度,设置对应的实际加减速长度
    ramp.RampLength = (ramp.RampLengthSET)*(config.RPM)/300;
}

// 获取当前的电机设置速度(注意不是当前实际运行速度)
// 电机有可能处于加减速阶段或停止阶段
float MaxXYHiSpd::speed(void)
{
    return config.RPM;
}

/*******************************************************************************
函 数: void MinSpeed(float config.RPM)
功 能: 设置电机加减速阶段的起步频率.最小速度.电机要么停止,要么高于本速度运行.
参 数: config.RPM--步进电机起步频率.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::MinSpeed(float myRPM)
{
    float PlsFrq = (myRPM)*(float)(config.PlsPerCir)/60.0 ;
                                            // 根据RPM计算出定时器中断的频率
    if( PlsFrq>=MIN_PLS_FREQ )
        config.MinPlsFrq = PlsFrq;
    else
        config.MinPlsFrq = MIN_PLS_FREQ;
}

/*******************************************************************************
函 数: float CurSpeed(void)
功 能: 获取当前速度.当前电机正在运行的速度.RPM(转/分)
返 回: 当前速度.单位:config.RPM.

张彦欣
2018.02.08
*******************************************************************************/
float MaxXYHiSpd::CurSpeed()
{
    return (  (system.CurFrq)*60.0/(float)(config.PlsPerCir)  );
}

/*******************************************************************************
函 数: void Stop(void)
功 能: 立即停止步进电机的运行.立即停止输出.电机不会缓慢减速.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::stop(void)
{
    system.CurFrq = 0;              //当前速度.
    system.State  = MOTOR_STOP;     //电机状态:停止.    

	system.RemainSteps  = 0;
	system.CurrentSteps = 0;
    
    // 电机停止时输出电平
    digitalWrite(config.PIN_StepX, config.StopLevel);
    digitalWrite(config.PIN_StepY, config.StopLevel);

    config.DrvTimer.close();        //关闭定时器
}

/*******************************************************************************
函 数: void RStop(void)
功 能: 使用斜坡加减速功能停止步进电机.电机会缓慢停止.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::RStop(void)
{
    if(system.CurFrq==0)    //当前电机是停止的.已经停止了.
    {
        return;
    }

    if( !ramp.EnRamp )      //斜坡功能没有被启用.
    {                       //直接立即停止
        stop();
        return;
    }

    if(system.CurrentSteps<=system.RemainSteps)
    {                               //运动的前半部分
        if(system.CurrentSteps<ramp.RampLength)
        {                           //正在加速阶段.
            system.RemainSteps = system.CurrentSteps;
        }
        else                        //让电机进入减速阶段
        {
            system.RemainSteps = ramp.RampLength;
        }
    }
    else                            //运动的后半部分
    {
        if(system.RemainSteps<ramp.RampLength)
        {                           //已经在减速阶段了.不需要处理.
        }
        else                        //让电机进入减速阶段
        {
            system.RemainSteps = ramp.RampLength;
        }
    }
}

/*******************************************************************************
函 数: Uchar get(void)
功 能: 获取当前电机的运行状态.
参 数: None
返 回: MOTOR_STOP/MOTOR_UP/MOTOR_RUN/MOTOR_DOWN

张彦欣
2018.02.08
*******************************************************************************/
Uchar MaxXYHiSpd::state(void)
{
    return (system.State);
}

/*******************************************************************************
函 数: long location()
功 能: 获取全局位置/设置全局位置.
参 数: 全局坐标.
返 回: 全局坐标.

张彦欣
2018.02.08
*******************************************************************************/
long MaxXYHiSpd::location(Uchar myMOTOR)
{
    if( myMOTOR==MOTOR_X )  //获取X轴的全局位置
        return (system.x_location);
    else                    //获取Y轴的全局位置
        return (system.y_location);
}

void MaxXYHiSpd::location(Uchar myMOTOR,long CurLocation)
{
    if( myMOTOR==MOTOR_X )  //设置X轴的全局位置
        system.x_location = CurLocation;
    else                    //设置Y轴的全局位置
        system.y_location = CurLocation;
}

/*******************************************************************************
函 数: void MoveTo(long,long)
功 能: 电机运动到指定全局位置.
参 数: TargetLocation--全局位置坐标.(X,Y)
说 明: 例如当前位置是(100,200),当执行MoveTo(300,800)时,X轴电机往正方向移动200脉冲,
       Y轴电机往正方向移动600脉冲.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::MoveTo(long TCX,long TCY)
{
    // Serial.print("MoveTo:(");   //调试信息
    // Serial.print(TCX);
    // Serial.print(",");
    // Serial.print(TCY);
    // Serial.println(")");
    
    long x_cur_location = (system.x_location);
    long y_cur_location = (system.y_location);
    
    long delta_x = TCX - x_cur_location;    //X轴变化值
    long delta_y = TCY - y_cur_location;    //Y轴变化值
    
    // Serial.print("delta:(");    //调试信息
    // Serial.print(delta_x);
    // Serial.print(",");
    // Serial.print(delta_y);
    // Serial.println(")");
    
    Ulong abs_x = (delta_x>0)?(delta_x):(-delta_x); //X轴变化值(绝对值)
    Ulong abs_y = (delta_y>0)?(delta_y):(-delta_y); //Y轴变化值(绝对值)
    
    // Serial.print("abs:(");      //调试信息
    // Serial.print(abs_x);
    // Serial.print(",");
    // Serial.print(abs_y);
    // Serial.println(")");
    
    Ulong max_delta = (abs_x>abs_y)?(abs_x):(abs_y);//XY轴变化最大值
    
    system.TotalSteps     = max_delta;      //总步数
    system.RemainSteps    = max_delta;      //剩余步数.
    system.CurrentSteps   = 0;              //当前步数.
    
    // Serial.print("TotalSteps:");         //调试信息
    // Serial.println(system.TotalSteps);
    // Serial.print("RemainSteps:");        //调试信息
    // Serial.println(system.RemainSteps);
    
    system.x_TotalSteps   = abs_x;
    system.y_TotalSteps   = abs_y;
    
    system.x_CurrentSteps = 0;
    system.y_CurrentSteps = 0;
    
    // Serial.print("max_delta:");      //调试信息
    // Serial.println(max_delta);
    
    // Serial.print("x_TotalSteps:");   //调试信息
    // Serial.println(system.x_TotalSteps);
    // Serial.print("y_TotalSteps:");   //调试信息
    // Serial.println(system.y_TotalSteps);
    
    if( delta_x>0 )
        system.RunDirX = DIR_ZHENG;
    else
        system.RunDirX = DIR_FAN;
    
    if( delta_y>0 )
        system.RunDirY = DIR_ZHENG;
    else
        system.RunDirY = DIR_FAN;
    
    // Serial.print("RunDirX:");        //调试信息
    // Serial.println(system.RunDirX);
    // Serial.print("RunDirY:");
    // Serial.println(system.RunDirY);
    
    OS_Direction();                     //方向引脚输出
    delay(2);
    
    if( system.TotalSteps==0 )          //总运行步数为零
    {                                   //不需要移动
        return;
    }

    if(ramp.EnRamp)                     //加减速开启.
    {                                   //按理说应该从MinFrq频率开始中断.
        system.CurFrq= config.TargetFrq;//第一次中断越快越好.在定时器中断里会自动调节速度
        system.State = MOTOR_UP;        //状态:加速
    }
    else                                //加减速关闭
    {
        system.CurFrq = config.TargetFrq;
        system.State  = MOTOR_RUN;      //状态:恒速运行
    }

    OS_LoadTimer();                     //设置定时计数器频率.
    config.DrvTimer.open();             //开启定时计数器.
    return;
}

/*******************************************************************************
函 数: void DirOpp(Uchar _Cmd)
功 能: 运动方向是否取反.
参 数: _Cmd--运动方向是否取反.false--不取反;true--取反.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::DirOppX(Uchar _Cmd)
{
	if( _Cmd )  //X轴的方向定义是否取反(反过来)
		config.DirOppX = true;
	else
		config.DirOppX = false;
}

void MaxXYHiSpd::DirOppY(Uchar _Cmd)
{
	if( _Cmd )  //Y轴的方向定义是否取反(反过来)
		config.DirOppY = true;
	else
		config.DirOppY = false;
}

/*******************************************************************************
函 数: void wait(void)
功 能: 一直等待,直到电机运行停止.
       这是阻塞函数,一般测试用,正式项目中尽量避免使用.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::wait(void)
{
    while( system.State!=MOTOR_STOP )
    {
        KickDog();
    }
}

void MaxXYHiSpd::wait(Ulong OVTime)
{
    Ulong StartTime=millis();
    
    for(;;)
    {
        if(system.State==MOTOR_STOP)
        {
            return;
        }
        if( MaxTimePass(StartTime)>=OVTime )
        {
            return;
        }
        KickDog();
    }
}

/*******************************************************************************
函 数: void swait(Uchar Port,Uchar Lvl)
功 能: 一直等待,直到电机运行停止或指定端口动作,退出.阻塞函数,谨慎使用.
参 数: Port--指定端口;Lvl--端口动作时电平.

张彦欣
2018.02.08
*******************************************************************************/
Uchar MaxXYHiSpd::swait(Uchar ulPin, Uchar Level, Uint FilterTime)
{
    unsigned int FilterCount=0;
    
    for(;;)
    {
        if(system.State==MOTOR_STOP)
        {
            return 0;
        }
        if(digitalRead(ulPin)==Level)           //满足停止条件.
        {
            FilterCount++;
            if(FilterCount>=FilterTime)         //等待确认时间.ms.
            {
                stop();                         //停止电机.
                return 1;                       //返回1.
            }
        }
        delay(1);
        KickDog();
    }    
}

Uchar MaxXYHiSpd::swait(unsigned char ulPin, unsigned char Level)
{
    swait(ulPin,Level,10);
}

/*******************************************************************************
函 数: void LSWLevel(Uchar mySetLevel)
功 能: 设置限位开关动作时的电平.
参 数: mySetLevel -- 电平状态. 0--低电平;1--高电平.
返 回: None

张彦欣
2018.02.08
*******************************************************************************/
// void MaxXYHiSpd::LSWLevel(Uchar mySetLevel)
// {
// if(mySetLevel)
// {
// config.LswLevel = HIGH;        
// }
// else
// {
// config.LswLevel = LOW;        
// }
// }

/*******************************************************************************
函 数: void EnLSW(void)
功 能: 开启/关闭限位开关功能.
参 数: None
返 回: None

张彦欣
2018.02.08
*******************************************************************************/
// void MaxXYHiSpd::LSW(Uchar _Cmd)
// {
// if(_Cmd)
// {
// config.EnLsw = true;
// }
// else
// {
// config.EnLsw = false;
// }
// }

/*******************************************************************************
函 数: void LSWPin(Uchar myPin1, Uchar myPin2)
功 能: 设置限位开关引脚.
参 数: myPin1 -- 限位开关增量限位.
       myPin2 -- 限位开关减量限位.
返 回: None

张彦欣
2018.02.08
*******************************************************************************/
// void MaxXYHiSpd::LSWPin(Uchar myPin1, Uchar myPin2)
// {
// config.PIN_LswPos = myPin1;
// config.PIN_LswNag = myPin2;
// pinMode(config.PIN_LswPos, INPUT);
// pinMode(config.PIN_LswNag, INPUT);
// }

/*******************************************************************************
函 数: void Ramp(long myRampLength)
功 能: 斜坡函数设置函数.设置加加速的长度.长度越长加减速越慢.
参 数: myRampLength -- 300RPM转速(5RPS)下,电动机加减速的长度.单位:脉冲.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::RampLength(Ulong myRampLength)
{
    if(myRampLength==0)
        return;
    ramp.RampLengthSET = myRampLength;
    
    // 用户设置的RampLength(加减速长度),是300RPM转速下的加减速长度
    // 要根据当前的速度,设置对应的实际加减速长度
    ramp.RampLength = (ramp.RampLengthSET)*(config.RPM)/300;
    
    // Serial.print("RampLength:");        //调试信息
    // Serial.println(ramp.RampLength);
}

/*******************************************************************************
函 数: void MaxXYHiSpd::OpenRamp(unsigned char myCMD)
功 能: 开启/关闭斜坡函数功能.
参 数: false--关闭;true--开启.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::EnRamp(Uchar myCMD)
{
	if(myCMD)
        ramp.EnRamp = true;
	else
        ramp.EnRamp = false;
}

/*******************************************************************************
函 数: void OS_LRamp(void)
功 能: 直线加减速函数.

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_LRamp(void)
{
    if( ramp.RampLength<=0 )                //加减速距离不能太小
        ramp.RampLength=10;
        
    if( config.TargetFrq<config.MinPlsFrq ) //目标速度不能低于最小速度
        config.TargetFrq=config.MinPlsFrq;
    
    if( (system.CurrentSteps>ramp.RampLength)&&(system.RemainSteps>ramp.RampLength) )
    {                                       //不在加减速的范围之内.
        system.State  = MOTOR_RUN;          //恒速运行
        system.CurFrq = config.TargetFrq;
        OS_LoadTimer();
        return;
    }
    else                                    //加速或减速阶段.
    {
        if( system.CurrentSteps<=system.RemainSteps )
        {                                   //已经运行的步数<=剩余的步数.
            system.State  = MOTOR_UP;       //运动的前半部分.加速过程中.           
            system.CurFrq = (Ulong)(config.TargetFrq)*(system.CurrentSteps)/(ramp.RampLength) + (Ulong)(config.MinPlsFrq);
            
            if( system.CurFrq>config.TargetFrq )
                system.CurFrq=config.TargetFrq;
            
            OS_LoadTimer();
            return;
        }
        else                                //已经运行的步数>剩余的步数.
        {                                   //运动的后半部分.减速过程中.
            system.State  = MOTOR_DOWN;
            system.CurFrq = (Ulong)(config.TargetFrq)*(system.RemainSteps)/(ramp.RampLength) + (Ulong)(config.MinPlsFrq);
            
            if( system.CurFrq>config.TargetFrq )
                system.CurFrq=config.TargetFrq;
            
            OS_LoadTimer();
            return;
        }
    }
}

/*******************************************************************************
函 数: void OS_NRamp(void)
功 能: 无加减速.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::OS_NRamp(void)
{   
    system.State  = MOTOR_RUN;  //恒速运行
    system.CurFrq = config.TargetFrq;
    OS_LoadTimer();
    return;
}

/*******************************************************************************
函 数: void OS_StepOUT(void)
功 能: 脉冲输出.DDA算法核心.

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_StepOUT(void)
{
    if( system.TotalSteps==0 )
    {
        return;
    }
    
    // BUG修复
    // 此处使用float,不再使用Ulong,因为使用Ulong的时候,
    // 如果运行步数超过65535,那么此处应该使用Ullong类型.运行速度也会降低.
    // 在x_TotalSteps或y_TotalSteps超过Uint后,其与CurrentSteps乘积会超出Ulong范围
    // 如果移动范围不超过65536还是使用Ulong速度更快
    float XT = system.x_TotalSteps;
    float YT = system.y_TotalSteps;
    float CT = system.CurrentSteps;
    float TS = system.TotalSteps;
    float Tmp= CT/TS;

    if( system.x_CurrentSteps< (Ulong)(XT*Tmp) )
    {                       //system.x_TotalSteps*system.CurrentSteps/system.TotalSteps
        system.x_CurrentSteps++;
        
        digitalWrite( config.PIN_StepX,!(config.StopLevel) );
        
        if( system.RunDirX==DIR_ZHENG )
            system.x_location++;
        else
            system.x_location--;
    }

    if( system.y_CurrentSteps< (Ulong)(YT*Tmp) )
    {                       //system.y_TotalSteps*system.CurrentSteps/system.TotalSteps
        system.y_CurrentSteps++;
        
        digitalWrite( config.PIN_StepY,!(config.StopLevel) );
        
        if( system.RunDirY==DIR_ZHENG )
            system.y_location++;
        else
            system.y_location--;
    }
}

/*******************************************************************************
函 数: void OS_ChkRemainSteps(void)
功 能: 判断剩余步数.如果脉冲输出完毕,就停止中断.

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_ChkRemainSteps(void)
{
    if( system.RemainSteps>0 )
        system.RemainSteps--;

	if( system.RemainSteps==0 )
    {
        // Serial.print("CurrentSteps:");          //调试信息
        // Serial.println(system.CurrentSteps);
        // Serial.print("x_CurrentSteps:");        //调试信息
        // Serial.println(system.x_CurrentSteps);
        // Serial.print("y_CurrentSteps:");        //调试信息
        // Serial.println(system.y_CurrentSteps);
        
        stop();
        return;
    }
}

/*******************************************************************************
函 数: void LSW(Uchar IfEnable)
功 能: 初始化限位功能.获取限位状态.硬件限位功能.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::LSW(Uchar IfEnable)
{
    limitswitch.Enable = !!IfEnable;
}

void MaxXYHiSpd::LSW(  Uchar IfEnable, Uint XMaxPin, Uint XMinPin,
                                  Uint YMaxPin, Uint YMinPin, Uchar ActLvl )
{
    limitswitch.Enable = !!IfEnable;

    limitswitch.PIN_XMax = XMaxPin;
    limitswitch.PIN_XMin = XMinPin;
    limitswitch.PIN_YMax = YMaxPin;
    limitswitch.PIN_YMin = YMinPin;

    limitswitch.ActLevel = ActLvl;
}

Uchar MaxXYHiSpd::LSW(void)
{
    if( limitswitch.XFlg || limitswitch.YFlg )
        return true;

    return false;
}

/*******************************************************************************
函 数: void ResetLsw(void)
功 能: 复位限位标识

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::ResetLsw(void)
{
    limitswitch.XFlg = false;
    limitswitch.YFlg = false;
}

/*******************************************************************************
函 数: Uchar OS_LimitSwitch_One(  Uchar myRunDir,
                                  Uint myMaxPin, Uint myMinPin)
功 能: 限位开关检查函数.在插补过程中不断检查.内部函数.

张彦欣
2018.02.08
*******************************************************************************/
Uchar MaxXYHiSpd::OS_LimitSwitch_One(  Uchar myRunDir,
                                       Uint myMaxPin, Uint myMinPin)
{
    if( myRunDir==DIR_ZHENG )   //正方向.检查最大限位.
    {
        if( digitalRead(myMaxPin)==limitswitch.ActLevel 
         && digitalRead(myMaxPin)==limitswitch.ActLevel )
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else                        //正在往反方向运动.检查最小限位.
    {
        if( digitalRead(myMinPin)==limitswitch.ActLevel
         && digitalRead(myMinPin)==limitswitch.ActLevel )
        {
            return true;
        }
        else
        {
            return false;
        }
    }
}

Uchar MaxXYHiSpd::OS_LimitSwitch(void)
{
    limitswitch.XFlg = OS_LimitSwitch_One( system.RunDirX,
                                           limitswitch.PIN_XMax,
                                           limitswitch.PIN_XMin  );
    
    limitswitch.YFlg = OS_LimitSwitch_One( system.RunDirY,
                                           limitswitch.PIN_YMax,
                                           limitswitch.PIN_YMin  );
    
    if( limitswitch.XFlg || limitswitch.YFlg )
        return true;
    
    return false;
}

/*******************************************************************************
函 数: void LocationTrigEnable(Uchar myEnable)
功 能: 使能位置触发功能.根据位置触发相机等.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::LocationTrigEnable(Uchar myEnable)
{
    locationtrig.Enable = myEnable;
}

void MaxXYHiSpd::LocationTrigSET( Uchar myMotor,long myStart,long myStop )
{
    if( myMotor==MOTOR_X )  //X轴电机
    {
        locationtrig.StartX = myStart;
        locationtrig.StopX  = myStop;
    }
    else                    //Y轴电机
    {
        locationtrig.StartY = myStart;
        locationtrig.StopY  = myStop;
    }
}

/*******************************************************************************
函 数: void OS_LocationTRIG(void)
功 能: 根据全局位置触发IO

张彦欣
2018.02.08
*******************************************************************************/
inline void MaxXYHiSpd::OS_LocationTRIG(void)
{
    if( locationtrig.Enable==false )
    {
        return;
    }
    
    if( locationtrig.Motor==MOTOR_X )   //X轴电机
    {
        if( system.x_location==locationtrig.StartX )
        {
            MaxXYHiSpd_LocationTrigStart();
        }
        else if( system.x_location==locationtrig.StopX )
        {
            locationtrig.Enable=false;  //必须放在下一行语句之前.
                                        //因为用户可能会在下一行语句重新开启触发
                                        //否则的话,用户开启的触发将被该语句关闭
            MaxXYHiSpd_LocationTrigStop();
        }
    }
    else                                //Y轴电机
    {
        if( system.y_location==locationtrig.StartY )
        {
            MaxXYHiSpd_LocationTrigStart();
        }
        else if( system.y_location==locationtrig.StopY )
        {
            locationtrig.Enable=false;
            MaxXYHiSpd_LocationTrigStop();
        }
    }
}

/*******************************************************************************
函 数: void System_MotorISR(void)
功 能: 这个函数是由定时器中断调用的函数.

张彦欣
2018.02.08
*******************************************************************************/
void MaxXYHiSpd::scan(void)
{
    // 再次检查RemainSteps,如果已经运行指定步数,直接退出.
    // BUG修复
    if( system.RemainSteps==0 )
    {
        stop();
        return;
    }
    
    // 方向控制
    // OS_Direction();
    
    //-----------------
    //限位开关
    //
    if( OS_LimitSwitch() )
    {
        stop();
        return;
    }
    
    system.CurrentSteps++;  //步进电机已经运行的步数.
                            //必须在OS_StepOUT()函数前调用
    
    OS_StepOUT();           //输出脉冲,并记录全局位置
    
    //-----------------
    //速度斜坡函数动作
    //
    if(ramp.EnRamp)         //开启斜坡功能.
        OS_LRamp();
    else                    //关闭斜坡功能.
        OS_NRamp();
    
    OS_ChkRemainSteps();    //判断剩余步数,是否关闭定时器.

    OS_LocationTRIG();
    
    //停止输出
    digitalWrite(config.PIN_StepX, config.StopLevel);
    digitalWrite(config.PIN_StepY, config.StopLevel);
}