#include "servostep.h"
#include "global.h"
StepperDataInfo tXAxisStepper @".ccram";
StepperDataInfo tYAxisStepper @".ccram";


word_bits_t cAxisDriveState @".ccram";
word_bits_t cAxisAlarmState @".ccram";

static StepperDataInfo *StepInfo[] = {
    &tXAxisStepper,
    &tYAxisStepper 
};

#define X_FreqTab_Number 150
#define Y_FreqTab_Number 150
#define DATA_SIZE 100

const unsigned long FreqTab_XX[] = {
    2000, 4000, 6774, 9440, 15998, 19998, 24002, 27997, 32009, 35994,
    39997, 44016, 47979, 51982, 55995, 60022, 64018, 67952, 72046, 76008,
    79994, 83940, 87945, 91968, 95958, 100083, 104086, 107894, 111991, 116107,
    119882, 123912, 127852, 132050, 136113, 139989, 144093, 147949, 152016, 155763,
    160278, 163840, 168200, 172127, 176242, 179824, 184320, 188241, 192333, 195738,
    200166, 203856, 207684, 211659, 215789, 220083, 224552, 228024, 231606, 236560,
    240417, 244402, 248521, 251345, 255704, 260216, 263314, 268101, 271391, 276480,
    279979, 283569, 287251, 292958, 296891, 300930, 302991, 307200, 311526, 315977,
    320556, 322896, 327680, 332607, 335127, 340283, 342920, 348321, 351085, 356748,
    359648, 362596, 368640, 371737, 374888, 381351, 384667, 388042, 391476, 394971,
    398529, 402152, 405842, 409600, 413428, 417328, 421302, 425353, 429483, 433694,
    437988, 442368,
};
unsigned short FreqTab_X[X_FreqTab_Number]@".ccram";
static unsigned short FreqTab_X2[X_FreqTab_Number]@".ccram";


const unsigned long FreqTab_YY[] = {
    2000, 4000, 9999, 11668, 13332, 15000, 16667, 18332, 19998, 21663,
    23331, 25006, 26664, 28338, 29991, 31665, 33335, 34997, 36680, 38333,
    39997, 41654, 43326, 45001, 46663, 48346, 49985, 51678, 53361, 55020,
    56641, 58359, 60022, 61697, 63376, 64958, 66621, 67331, 67952, 68690,
    69336, 69994, 70665, 71349, 72046, 72638, 73361, 73974, 74724, 75360,
    76008, 76666, 77337, 78019, 78713, 79277, 79994, 80724, 81317, 82071,
    82685, 83308, 83940, 84744, 85399, 86063, 86738, 87252, 87945, 88650,
    89367, 89912, 90649, 91398, 91968, 92739, 93326, 93921, 94725, 95337,
    95958, 96586, 97437, 98086, 98742, 99408, 100083, 100767, 101228, 101928,
    102637, 103357, 104086, 104578, 105325, 106083, 106594, 107370, 107894, 108689,
    109226, 110041, 110592, 111427, 111991,
};
static unsigned short FreqTab_Y[X_FreqTab_Number]@".ccram";
static unsigned short FreqTab_Y2[X_FreqTab_Number]@".ccram";

void CalculateSModelLine(unsigned short *fre,float  len,  float fre_max,  float fre_min, float flexible)
{
    int i=0;
    float deno ;
    float melo ;
    float delt = fre_max-fre_min;
    float freq_buff;
    
    for(i=0; i<len; i++)
    {
        melo = flexible * (i-len/2) / (len/2);
        deno = 1.0 / (1 + expf(-melo));   //expf is a library function of exponential(e) 
        freq_buff = delt * deno + fre_min;
        *(fre+i)  = 4000000/(unsigned short)freq_buff;
	}
	 
  		
	for(i=(int)len;i<DATA_SIZE;i++)
    {
        *(fre+i)  = *(fre+(unsigned short)(len - 1));
    }	
}
/*
void CalAccDecTab(unsigned char *acc_dec,unsigned short acc_dec_time,unsigned short max_speed)
{
    float acc_time_buff,deno;
    unsigned short i;
    acc_time_buff = acc_dec_time;
    acc_time_buff = acc_time_buff / (max_speed);
	
	for(i = 0;i<max_speed;i++)
	{
	    *(acc_dec+i) = (unsigned char)acc_time_buff;	
	}
	
	cPulseStepDEC = 0;
	
	for(i = 0;i<DATA_SIZE;i++)
	{
		if(*(acc_dec+i) == 0)
		    cPulseStepDEC += 1;
		else
		    cPulseStepDEC += *(acc_dec+i);
	}
	
	cDecPulseBuff = cPulseStepDEC;		
}*/

// APB2_TIM9 X
void CalFreqTab_X(void)
{
    short i = 0;
    short j = 0; 

    j = sizeof(FreqTab_XX) / 4;
    for (i = 0; i < j; i++)
    {
        if (FreqTab_XX[i] > 0)
        {
            FreqTab_X[i] = 4000000/(FreqTab_XX[i]/7);
        }
    }
}

void CalFreqTab_X_Free(unsigned char divide_var)
{
    short i = 0;
    short j = 0; 

    j = sizeof(FreqTab_XX) / 4;
    for (i = 0; i < j; i++)
    {
        if (FreqTab_XX[i] > 0)
        {
            FreqTab_X[i] = 4000000/(FreqTab_XX[i]/divide_var);
        }
    }
}

void CalFreqTab_X_Free_JY(unsigned char divide_var)
{
    short i = 0;
    short j = 0; 

    j = sizeof(FreqTab_XX) / 4;
    for (i = 0; i < j; i=i+3)
    {
        if (FreqTab_XX[i] > 0)
        {
            if(i >= j)i= j-1;
             FreqTab_X[i] = 4000000/(FreqTab_XX[3*i]/divide_var);
        }
    }
}
// APB1_TIM13 Y
void CalFreqTab_Y(void)
{
    short i = 0;
    short j = 0;

    j = sizeof(FreqTab_YY) / 4;
    for (i = 0; i < j; i++)
    {
        if (FreqTab_YY[i] > 0)
        {
            FreqTab_Y[i] = 4000000/(FreqTab_YY[i]/7);
        }
    }
}


//#pragma inline
void XAxis_PWMProc(void)
{
    if (bXDirction) tXAxisStepper.cRealPosi--;
    else tXAxisStepper.cRealPosi++;
    PWMAction();
    if (tXAxisStepper.bAuto_Mov == 1)
    {
        tXAxisStepper.cPmovPulse--;
        if (tXAxisStepper.cPmovPulse == 0)
        {
            hw_pwm_stop(axis_x->axis_no);
            tXAxisStepper.bAuto_Mov = 0;
            tXAxisStepper.cCurSpeed = 0;          
        }
        else if (tXAxisStepper.cPmovPulse > tXAxisStepper.cCalDecPulse)
        {
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.cCalSpeed)
            {
                tXAxisStepper.cAccDecVal++;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
        }
        else
        {
            if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
            {
                tXAxisStepper.cAccDecVal++;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
        }
        
    }
    else if(tXAxisStepper.bAuto_Mov == 2)
    {
        tXAxisStepper.cPmovPulse--;
        if (tXAxisStepper.cPmovPulse == 0)
        {
            hw_pwm_stop(axis_x->axis_no);
            tXAxisStepper.bAuto_Mov = 0;
            tXAxisStepper.cCurSpeed = 0;
        }
        else if (tXAxisStepper.cPmovPulse > tXAxisStepper.HighSpeedPulse + tXAxisStepper.cCalDecPulse)
        {
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.FirstLowSpeed)
            {
                tXAxisStepper.cAccDecVal++;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;

                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }            
            }
        }
        else if (tXAxisStepper.cPmovPulse > tXAxisStepper.cCalDecPulse)
        {
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.cCalSpeed)
            {
                tXAxisStepper.cAccDecVal++;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
        }
        else
        {
            if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
            {
                tXAxisStepper.cAccDecVal++;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;

                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
        }    
    }
    else
    {
        if (tXAxisStepper.State == 3)
        {
            if (hw_io_pin_input(5))
            {
                tXAxisStepper.cZeroFilter++;
                if (tXAxisStepper.cZeroFilter >= 3)
                {
                     hw_pwm_stop(axis_x->axis_no);
                    tXAxisStepper.cRealPosi = 0;
                    tXAxisStepper.State = 0;
                    tXAxisStepper.cCurSpeed = 0;
                }
            }
            else
            {
                tXAxisStepper.cZeroFilter = 0;
            }
            
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
            else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }            
            }
        }
        else if (tXAxisStepper.State == 1)
        {
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
            else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }            
            }
        }
        else if (tXAxisStepper.State == 2)
        {
            if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
            else
            {
                hw_pwm_stop(axis_x->axis_no);
                tXAxisStepper.State = 0;
                tXAxisStepper.cCurSpeed = 0;
            }
        }
        else if (tXAxisStepper.State == 4)
        {
            if(tXAxisStepper.cPmovPulse)tXAxisStepper.cPmovPulse--;
            
            if (hw_io_pin_input(4))
            {
                tXAxisStepper.cZeroFilter++;
                if (tXAxisStepper.cZeroFilter >= 3)
                {
                    hw_pwm_stop(axis_x->axis_no);
                    tXAxisStepper.State = 0;
                    tXAxisStepper.cCurSpeed = 0;
                }
            }
            else
            {
                tXAxisStepper.cZeroFilter = 0;
            }
            
            if(tXAxisStepper.cPmovPulse == 0)
            {
                if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
                {
                    ++tXAxisStepper.cAccDecVal;
                    if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                    {
                        tXAxisStepper.cAccDecVal = 0;
                        tXAxisStepper.cCurSpeed--;
                        hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                    }            
                }
            }
            else
            {
                if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
                {
                    ++tXAxisStepper.cAccDecVal;
                    if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                    {
                        tXAxisStepper.cAccDecVal = 0;
                        tXAxisStepper.cCurSpeed++;
                        hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                    }
                }
            }
        }
        else if (tXAxisStepper.State == 5)
        {
            tXAxisStepper.cPmovPulse--;
            if (tXAxisStepper.cPmovPulse == 0)
            {
                hw_pwm_stop(axis_x->axis_no);
                tXAxisStepper.bAuto_Mov = 0;
                tXAxisStepper.cCurSpeed = 0;          
            }
            if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }
            }
            else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
            {
                ++tXAxisStepper.cAccDecVal;
                if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
                {
                    tXAxisStepper.cAccDecVal = 0;
                    tXAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
                }            
            }        
        }
    }
}



/********************************************

MTU9-PWM????????ж????

*********************************************/
//#pragma inline
void YAxis_PWMProc(void)
{ // y zhou
    if (bYDirction) tYAxisStepper.cRealPosi++;
    else tYAxisStepper.cRealPosi--;
    
    if (tYAxisStepper.bAuto_Mov)
    {
        if (--tYAxisStepper.cPmovPulse == 0)
        {
            hw_pwm_stop(axis_y->axis_no);
            Y_DRV = 0;
            tYAxisStepper.cCurSpeed = 0;
            tYAxisStepper.bAuto_Mov = 0;
        }
        else if (tYAxisStepper.cPmovPulse > tYAxisStepper.cCalDecPulse)
        {
            if (tYAxisStepper.cCurSpeed < tYAxisStepper.cCalSpeed)
            {
                if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetAccVal)
                {
                    tYAxisStepper.cAccDecVal = 0;
                    tYAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
                }
            }
        }
        else
        {
            if (tYAxisStepper.cCurSpeed > tYAxisStepper.cSetStartSpeed)
            {
                if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetDecVal)
                {
                    tYAxisStepper.cAccDecVal = 0;
                    tYAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
                }
            }
        }
    }
    else
    {
        if (tYAxisStepper.State == 3)
        {
            if (1)
            {
                tYAxisStepper.cZeroFilter++;
                if (tYAxisStepper.cZeroFilter >= 50)
                {
                    hw_pwm_stop(axis_y->axis_no);;
                    Y_DRV = 0;
                    tYAxisStepper.cRealPosi = 0;
                    tYAxisStepper.State = 0;
                    tYAxisStepper.cCurSpeed = 0;
                }
            }
            else tYAxisStepper.cZeroFilter = 0;
        }
        else if (tYAxisStepper.State == 1)
        {
            if (tYAxisStepper.cCurSpeed < tYAxisStepper.cSetSpeed)
            {
                if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetAccVal)
                {
                    tYAxisStepper.cAccDecVal = 0;
                    tYAxisStepper.cCurSpeed++;
                    hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
                }
            }
            else tYAxisStepper.State = 0;
        }
        else if (tYAxisStepper.State == 2)
        {
            if (tYAxisStepper.cCurSpeed > tYAxisStepper.cSetStartSpeed)
            {
                if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetDecVal)
                {
                    tYAxisStepper.cAccDecVal = 0;
                    tYAxisStepper.cCurSpeed--;
                    hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
                }
            }
            else
            {
                hw_pwm_stop(axis_y->axis_no);
                Y_DRV = 0;
                tYAxisStepper.State = 0;
                tYAxisStepper.cCurSpeed = 0;
            }
        }
    }
}


void StartPWM(unsigned short cAxisNo)
{
    if (cAxisNo == X_AXIS)
    {
        tXAxisStepper.cCurSpeed = tXAxisStepper.cSetStartSpeed;

        hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
//        X_CNT = 0;
        hw_pwm_start(axis_x->axis_no);
//        X_DRV = 1;
    }
    else if (cAxisNo == Y_AXIS)
    {
        tYAxisStepper.cCurSpeed = tYAxisStepper.cSetStartSpeed;

        hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
//        Y_CNT = 0;
        hw_pwm_start(axis_y->axis_no);
    }
}


void SetAxisDir_Move(StepperDataInfo *tCurSetStpper, unsigned short dir)
{
    switch (tCurSetStpper->cAxisNo)
    {
    case X_AXIS:
         hw_pwm_set_dir(axis_x->axis_no, dir);
        break;
    case Y_AXIS:
         hw_pwm_set_dir(axis_y->axis_no, dir);
        break;
    default:
        tCurSetStpper->cPmovPulse = 0;
        break;
    }
}


void LCalRunPulse(StepperDataInfo *tCurSetStpper)
{
    if (tCurSetStpper->cDestPosi & 0x80000000)
    {
        tCurSetStpper->cPmovPulse = 0 - tCurSetStpper->cDestPosi;
        SetAxisDir_Move(tCurSetStpper, 1);
    }
    else
    {
        tCurSetStpper->cPmovPulse = tCurSetStpper->cDestPosi;
        SetAxisDir_Move(tCurSetStpper, 0);
    }

    tCurSetStpper->cCalAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
    tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
    tCurSetStpper->cCurSpeed = tCurSetStpper->cSetStartSpeed;
    tCurSetStpper->cAccDecVal = 0;

    if (tCurSetStpper->cPmovPulse < (tCurSetStpper->cCalAccPulse + tCurSetStpper->cCalDecPulse))
    {
        tCurSetStpper->cCalSpeed = tCurSetStpper->cPmovPulse / (tCurSetStpper->cSetAccVal + tCurSetStpper->cSetDecVal);
        tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cCalSpeed - tCurSetStpper->cSetStartSpeed);
    }
    else
    {
        tCurSetStpper->cCalSpeed = tCurSetStpper->cSetSpeed;
    }
}

void LCalRunPulse2(StepperDataInfo *tCurSetStpper)
{
    if (tCurSetStpper->cDestPosi & 0x80000000)
    {
        tCurSetStpper->cPmovPulse = 0 - tCurSetStpper->cDestPosi;
        SetAxisDir_Move(tCurSetStpper, 0);
    }
    else
    {
        tCurSetStpper->cPmovPulse = tCurSetStpper->cDestPosi;
        SetAxisDir_Move(tCurSetStpper, 1);
    }
    
    tCurSetStpper->FirstAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->FirstLowSpeed - tCurSetStpper->cSetStartSpeed) + tCurSetStpper->LowSpeedPulse;
    tCurSetStpper->cCalAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->FirstLowSpeed);
    tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
    tCurSetStpper->cCurSpeed = tCurSetStpper->cSetStartSpeed;
    tCurSetStpper->cAccDecVal = 0;

    if (tCurSetStpper->cPmovPulse < (tCurSetStpper->cCalAccPulse + tCurSetStpper->cCalDecPulse + tCurSetStpper->FirstAccPulse))
    {
        tCurSetStpper->cCalSpeed = tCurSetStpper->FirstLowSpeed;
        tCurSetStpper->FirstAccPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->FirstLowSpeed - tCurSetStpper->cSetStartSpeed);
        tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cCalSpeed - tCurSetStpper->FirstLowSpeed);
        tCurSetStpper->HighSpeedPulse = tCurSetStpper->cPmovPulse - tCurSetStpper->cCalDecPulse;
    }
    else
    {
        tCurSetStpper->HighSpeedPulse = tCurSetStpper->cPmovPulse - tCurSetStpper->cCalAccPulse - tCurSetStpper->FirstAccPulse - tCurSetStpper->cCalDecPulse;
        tCurSetStpper->cCalSpeed = tCurSetStpper->cSetSpeed;
    }
}

void MV_Set_Startv_CPU(unsigned short cAxisNo, unsigned short cSpeed)
{
     StepInfo[cAxisNo]->cSetStartSpeed = cSpeed;
}

void MV_Set_Speed_CPU(unsigned short cAxisNo, unsigned short cSpeed)
{
    if (cSpeed < StepInfo[X_AXIS]->cSetStartSpeed)
    {
        cSpeed = StepInfo[X_AXIS]->cSetStartSpeed;
    }

    StepInfo[cAxisNo]->cSetSpeed = cSpeed;         
}

void MoveChangSpeed(unsigned short cAxisNo, unsigned short cSpeed)
{
   MV_Set_Speed_CPU(cAxisNo, cSpeed);
}

void MoveChangSpeedPos(unsigned short cAxisNo, unsigned short cSpeed,long pulse)
{
   StepInfo[cAxisNo]->cCalDecPulse = StepInfo[cAxisNo]->cCalDecPulse + pulse;
   StepInfo[cAxisNo]->cSetStartSpeed = cSpeed;
   //MV_Set_Speed_CPU(cAxisNo, cSpeed);
}
void MV_Set_FirstSpeed(unsigned short cAxisNo, unsigned short cSpeed)
{
    StepInfo[cAxisNo]->FirstLowSpeed = cSpeed;
}

void MV_Set_FirstLowPulse(unsigned short cAxisNo,long pulse)
{
    StepInfo[cAxisNo]->LowSpeedPulse = pulse;
}

void MV_Set_Acc_CPU(unsigned short cAxisNo, unsigned short cSetAcc)
{
    StepInfo[cAxisNo]->cSetAccVal = cSetAcc;
    StepInfo[cAxisNo]->cAccDecVal = 0;
}

void MV_AccDec_Set_CPU(unsigned short cAxisNo, unsigned short cSetAcc, unsigned short cSetDec)
{
    StepInfo[cAxisNo]->cSetAccVal = cSetAcc;
    StepInfo[cAxisNo]->cSetDecVal = cSetDec;
    StepInfo[cAxisNo]->cAccDecVal = 0;
}

void MV_Set_Dec_CPU(unsigned short cAxisNo, unsigned short cSetDec)
{
    StepInfo[cAxisNo]->cSetDecVal = cSetDec;
    StepInfo[cAxisNo]->cAccDecVal = 0;
}

long MV_Get_Command_Pos_CPU(unsigned short cAxisNo)
{
    long cRealPosi = 0;
    cRealPosi = StepInfo[cAxisNo]->cRealPosi;
    return cRealPosi;
}

void MV_Set_Command_Pos_CPU(unsigned short cAxisNo, long pos)
{
    StepInfo[cAxisNo]->cRealPosi = pos;
}

void MV_Pmove_CPU(unsigned short cAxisNo, long dwPosi)
{
    StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
    //StepInfo[cAxisNo]->cSetStartSpeed = 3;
    StepInfo[cAxisNo]->State = 0;
    StepInfo[cAxisNo]->cAccDecVal = 0;
    LCalRunPulse(StepInfo[cAxisNo]);

    if (StepInfo[cAxisNo]->cPmovPulse)
    {
        StepInfo[cAxisNo]->bAuto_Mov = 1;
        StartPWM(cAxisNo);
    }
}

void MV_Set_Command_SlMTP_CPU(unsigned short cAxisNo, unsigned long dwMaxLen)
{
    StepInfo[cAxisNo]->cMaxPulse = dwMaxLen;
}

void MV_Set_Command_SlMTN_CPU(unsigned short cAxisNo, unsigned long dwMinLen)
{
    StepInfo[cAxisNo]->cMinPulse = dwMinLen;
}

void MV_Suddent_Stop(unsigned short cAxisNo)
{
    switch (cAxisNo)
    {
    case X_AXIS:
        hw_pwm_stop(axis_x->axis_no);
        X_DRV = 0;
        tXAxisStepper.bAuto_Mov = 0;
        break;
    case Y_AXIS:
        hw_pwm_stop(axis_y->axis_no);
        Y_DRV = 0;
        tYAxisStepper.bAuto_Mov = 0;
        break;
    }
    StepInfo[cAxisNo]->bAuto_Mov = 0;
    StepInfo[cAxisNo]->cRstStep = 0;
    StepInfo[cAxisNo]->State = 0;
    StepInfo[cAxisNo]->cCurSpeed = 0;
}

void MV_Dec_Stop_CPU(unsigned short cAxisNo)
{
    StepInfo[cAxisNo]->bAuto_Mov = 0;
    StepInfo[cAxisNo]->State = 2;
}

unsigned long MV_Cal_Dec_pulse(unsigned short high_speed,unsigned short low_speed,unsigned short dec_time)
{
    if(high_speed > low_speed)
        return ((high_speed - low_speed)*dec_time);
    else 
        return 0;
}

void MV_Continue_Move_CPU(unsigned short cAxisNo, unsigned short dir)
{
    StepInfo[cAxisNo]->bAuto_Mov = 0;
    switch (cAxisNo)
    {
    case X_AXIS:
        if(SET_PULSE_TYPE != X_AXIS_PULSE_Y_ON)hw_pwm_set_dir(axis_x->axis_no, dir);
        break;
    case Y_AXIS:
        hw_pwm_set_dir(axis_y->axis_no, dir);
        break;
    }
    StepInfo[cAxisNo]->State = 1;
    StepInfo[cAxisNo]->cAccDecVal = 0;
    StartPWM(cAxisNo);
}

void MV_Continue_Move_CPU_With_Stop(unsigned short cAxisNo, unsigned short dir)
{
    StepInfo[cAxisNo]->bAuto_Mov = 0;
    switch (cAxisNo)
    {
    case X_AXIS:
        hw_pwm_set_dir(axis_x->axis_no, dir);
        break;
    case Y_AXIS:
        hw_pwm_set_dir(axis_y->axis_no, dir);
        break;
    }
    StepInfo[cAxisNo]->State = 3;
    StepInfo[cAxisNo]->cAccDecVal = 0;
    StartPWM(cAxisNo);
}

void MV_Pmove2(unsigned short cAxisNo, unsigned long dwPosi,long back_lowpos)
{
    StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
    StepInfo[cAxisNo]->State = 0;
    StepInfo[cAxisNo]->cAccDecVal = 0;
    LCalRunPulse2(StepInfo[cAxisNo]);
    StepInfo[cAxisNo]->cCalDecPulse += back_lowpos;
    if (StepInfo[cAxisNo]->cPmovPulse)
    {
        StepInfo[cAxisNo]->bAuto_Mov = 2;
        StartPWM(cAxisNo);
    }
}
/**
 * 连续走
 *
 * @author xt (2019/7/23)
 *
 * @param cAxisNo
 * @param dir
 * @param startv
 * @param cSpeed
 */
void MoveAction_Const(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(cAxisNo, 20);
        MV_AccDec_Set_CPU(cAxisNo, 10, 30);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Continue_Move_CPU(cAxisNo,dir);
    }
    else if (cAxisNo == Y_AXIS)
    {
        MV_Set_Startv_CPU(Y_AXIS, 1);
        MV_AccDec_Set_CPU(Y_AXIS, 10, 10);
        MV_Set_Speed_CPU(Y_AXIS, cSpeed);
        MV_Continue_Move_CPU(cAxisNo,dir);
    }
}

void MoveAction_Const_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short start_speed,unsigned short accpulse,unsigned short dec_pulse)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(cAxisNo, start_speed);
        MV_AccDec_Set_CPU(cAxisNo, accpulse, dec_pulse);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Continue_Move_CPU(cAxisNo,dir);
    }
    else if (cAxisNo == Y_AXIS)
    {
        MV_Set_Startv_CPU(Y_AXIS, start_speed);
        MV_AccDec_Set_CPU(Y_AXIS, accpulse, dec_pulse);
        MV_Set_Speed_CPU(Y_AXIS, cSpeed);
        MV_Continue_Move_CPU(cAxisNo,dir);
    }
}
void MoveAction_Const_Back(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short low_speed,unsigned long pulse)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(cAxisNo, low_speed);
        MV_AccDec_Set_CPU(cAxisNo, 10, 30);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        StepInfo[cAxisNo]->bAuto_Mov = 0;
        switch (cAxisNo)
        {
            case X_AXIS:
                hw_pwm_set_dir(axis_x->axis_no, dir);
            break;
        }
        StepInfo[cAxisNo]->State = 4;
        StepInfo[cAxisNo]->cAccDecVal = 0;
        tXAxisStepper.cPmovPulse = pulse;
        StartPWM(cAxisNo);
    }
}

void MoveAction_Const_Back_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short low_speed,unsigned long pulse,unsigned short start_speed,unsigned short cAccPulse,unsigned short cDecPulse)
{
    MV_Set_Startv_CPU(cAxisNo, start_speed);
    MV_AccDec_Set_CPU(cAxisNo, cAccPulse, cDecPulse);
    MV_Set_Speed_CPU(X_AXIS, cSpeed);
    StepInfo[cAxisNo]->bAuto_Mov = 0;
    hw_pwm_set_dir(axis_x->axis_no, dir);
    StepInfo[cAxisNo]->State = 4;
    StepInfo[cAxisNo]->cAccDecVal = 0;
    tXAxisStepper.cPmovPulse = pulse;
    StartPWM(cAxisNo);
}
void MoveAction_Const_Stop(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(cAxisNo, 20);
        MV_AccDec_Set_CPU(cAxisNo, 10, 30);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Continue_Move_CPU_With_Stop(cAxisNo,dir);
    }
}

void MoveAction_Const_Stop_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short cStartSpeed,unsigned short cAccPulse,unsigned short cDecPulse)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(cAxisNo, cStartSpeed);
        MV_AccDec_Set_CPU(cAxisNo, cAccPulse, cDecPulse);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Continue_Move_CPU_With_Stop(cAxisNo,dir);
    }
}

// 走相对位置脉冲
void MoveAction_Pulse(unsigned short cAxisNo, long dwPosi, unsigned short cSpeed)
{
    if (cAxisNo == X_AXIS)
    {      
        MV_Set_Startv_CPU(X_AXIS, 2);
        MV_Set_Acc_CPU(X_AXIS, 10);
        MV_Set_Dec_CPU(X_AXIS, 5);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Pmove_CPU(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
    }
    else if (cAxisNo == Y_AXIS)
    {
        unsigned short acc = 10;

        MV_Set_Startv_CPU(Y_AXIS, 3);
        MV_Set_Acc_CPU(Y_AXIS, acc);
        MV_Set_Dec_CPU(Y_AXIS, acc);
        MV_Set_Speed_CPU(Y_AXIS, cSpeed);
        MV_Pmove_CPU(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
    }
}

// 走相对位置脉冲
void MoveAction_Pulse_AccDec(unsigned short cAxisNo, long dwPosi, unsigned short cSpeed,unsigned short cStartSpeed,unsigned short cAccPulse,unsigned short cDecPulse)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(X_AXIS, cStartSpeed);
        MV_Set_Acc_CPU(X_AXIS, cAccPulse);
        MV_Set_Dec_CPU(X_AXIS, cDecPulse);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Pmove_CPU(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
    }
}

// 两段速走相对位置脉冲最后一段慢速长度
void MoveAction_Pulse2(unsigned short cAxisNo, long dwPosi,long lowpos,long back_lowpos, unsigned short cSpeed,unsigned short low_speed)
{
    if (cAxisNo == X_AXIS)
    {
        unsigned short acc = 10;

        MV_Set_Startv_CPU(X_AXIS, 3);
        MV_Set_Acc_CPU(X_AXIS, acc);
        MV_Set_Dec_CPU(X_AXIS, acc);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Set_FirstSpeed(X_AXIS, low_speed);
        MV_Set_FirstLowPulse(X_AXIS,lowpos);
        MV_Pmove2(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS),back_lowpos);
    }
    else if (cAxisNo == Y_AXIS)
    {
        unsigned short acc = 10;
        MV_Set_Startv_CPU(Y_AXIS, 3);
        MV_Set_Acc_CPU(Y_AXIS, acc);
        MV_Set_Dec_CPU(Y_AXIS, acc);
        MV_Set_Speed_CPU(Y_AXIS, cSpeed);
        MV_Set_FirstSpeed(Y_AXIS, low_speed);
        MV_Set_FirstLowPulse(Y_AXIS,lowpos);
        MV_Pmove2(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(Y_AXIS),back_lowpos);
    }
}

// 两段速走相对位置脉冲最后一段慢速长度
void MoveAction_Pulse2_AccDec(unsigned short cAxisNo, long dwPosi,long lowpos,long back_lowpos, unsigned short cSpeed,unsigned short low_speed,unsigned short start_speed, unsigned short acc_pulse,unsigned short dec_pulse)
{
    if (cAxisNo == X_AXIS)
    {
        MV_Set_Startv_CPU(X_AXIS, start_speed);
        MV_Set_Acc_CPU(X_AXIS, acc_pulse);
        MV_Set_Dec_CPU(X_AXIS, dec_pulse);
        MV_Set_Speed_CPU(X_AXIS, cSpeed);
        MV_Set_FirstSpeed(X_AXIS, low_speed);
        MV_Set_FirstLowPulse(X_AXIS,lowpos);
        MV_Pmove2(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS),back_lowpos);
    }
    else if (cAxisNo == Y_AXIS)
    {
        MV_Set_Startv_CPU(Y_AXIS, start_speed);
        MV_Set_Acc_CPU(Y_AXIS, acc_pulse);
        MV_Set_Dec_CPU(Y_AXIS, dec_pulse);
        MV_Set_Speed_CPU(Y_AXIS, cSpeed);
        MV_Set_FirstSpeed(Y_AXIS, low_speed);
        MV_Set_FirstLowPulse(Y_AXIS,lowpos);
        MV_Pmove2(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(Y_AXIS),back_lowpos);
    }
}
/**
 * 必须与当前运动方向相同
 *
 * @author zhang (2019/12/6)
 *
 * @param cAxisNo
 * @param dwPosi
 */
void MV_Move_To_Position(unsigned short cAxisNo, long dwPosi)
{
    if (dwPosi < StepInfo[cAxisNo]->cRealPosi)
    {
        StepInfo[cAxisNo]->cDestPosi = StepInfo[cAxisNo]->cRealPosi - dwPosi;
    }
    else
    {
        StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
    }

    {
        //StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
        StepInfo[cAxisNo]->cPmovPulse = StepInfo[cAxisNo]->cDestPosi;
        StepInfo[cAxisNo]->State = 0;
        StepInfo[cAxisNo]->cAccDecVal = 0;
        StepInfo[cAxisNo]->cCalSpeed = StepInfo[cAxisNo]->cSetSpeed;
        StepInfo[cAxisNo]->cCalDecPulse = (StepInfo[cAxisNo]->cSetSpeed - StepInfo[cAxisNo]->cSetStartSpeed);
        StepInfo[cAxisNo]->cCalDecPulse *= StepInfo[cAxisNo]->cSetDecVal;
        StepInfo[cAxisNo]->cCalDecPulse += 50;

        StepInfo[cAxisNo]->bAuto_Mov = 1;
    }
}

void ResetServoDrv_CPU(unsigned short axis)
{
    if (axis == X_AXIS)
    {
        hw_pwm_set_enable(axis_x->axis_no,1);
        CalFreqTab_X();
        tXAxisStepper.cAxisNo = X_AXIS;
        tXAxisStepper.cDestPosi = 0;
        tXAxisStepper.cSetStartSpeed = 0;
        tXAxisStepper.cSetSpeed = 20;
        tXAxisStepper.cSetAccVal = 50;
        tXAxisStepper.cSetDecVal = 50;
        tXAxisStepper.State = 0;
        tXAxisStepper.cAccDecVal = 0;
        tXAxisStepper.PulseOffset = 0;
    }
    else if (axis == Y_AXIS)
    {
        hw_pwm_set_enable(axis_y->axis_no,1);
        CalFreqTab_Y();
        tYAxisStepper.cAxisNo = Y_AXIS;
        tYAxisStepper.cDestPosi = 0;
        tYAxisStepper.cSetStartSpeed = 0;
        tYAxisStepper.cSetSpeed = 20;
        tYAxisStepper.cSetAccVal = 50;
        tYAxisStepper.cSetDecVal = 50;
        tYAxisStepper.State = 0;
        tYAxisStepper.cAccDecVal = 0;
        tYAxisStepper.PulseOffset = 0;
    }
}

void SetRemainLength(unsigned short cAxisNo, long dwPosi,unsigned short set_speed)
{
     if(dwPosi == 0)
     {
         AxisEgmStop(X_AXIS);     
     }
     else
     {
         StepInfo[cAxisNo]->bAuto_Mov = 0;
         StepInfo[cAxisNo]->State = 5;
         StepInfo[cAxisNo]->cSetSpeed = set_speed;
         StepInfo[cAxisNo]->cPmovPulse = PosToPulse(cAxisNo,dwPosi);  
     }
     
}