编辑代码

	#define speed2mmps 50000 //1000000/20
	#define speed4mmps 25000 //1000000/40
	#define speed6mmps 16667 //1000000/60
	#define speed8mmps 12500 //1000000/80
	#define speed10mmps 10000 //1000000/100
	#define speed12mmps 8333 //1000000/120
	#define speed14mmps 7143 //1000000/140
	#define speed16mmps 6250 //1000000/160
	#define speed18mmps 5556 //1000000/180
	#define speed20mmps 5000 //1000000/200
	#define speed30mmps 3333
	#define speed32mmps 3125 //1000000/320
	#define speed40mmps 2500 //1000000/400
	#define speed50mmps 2000 //1000000/500
	#define speed60mmps 1667 //1000000/600

	#define speed64mmps 1562 //1000000/640
	#define speed70mmps 1428
	#define speed80mmps 1250 //1000000/800
	#define speed90mmps 1111
	#define speed100mmps 1000 //1000000/1000
	#define speed120mmps 833 //1000000/1200
	#define speed128mmps 781 //1000000/1280
	#define speed140mmps 714 //1000000/1400
	#define speed150mmps 682 //1000000/1500
	#define speed160mmps 625 //1000000/1600
	#define speed180mmps 556 //1000000/1800
	#define speed200mmps 500 //1000000/2000
	#define speed240mmps 417 //1000000/2400
	#define speed250mmps 400 //1000000/2500
	#define speed256mmps 390 //1000000/2560
	#define speed280mmps 357 //1000000/2800
	#define speed300mmps 333 //1000000/3000
	#define speed320mmps 313 //1000000/3200
	#define speed350mmps 286 //1000000/3500
	#define speed360mmps 273 //1000000/3600
	#define speed400mmps 250 //1000000/4000
	#define speed440mmps 228 //1000000/4400
	#define speed450mmps 222 //1000000/4500
	#define speed480mmps 208 //1000000/4800
	#define speed500mmps 200 //1000000/5000
	#define speed512mmps 195 //1000000/5120
	#define speed520mmps 190 //1000000/5200
	#define speed560mmps 178 //1000000/5600
	#define speed600mmps 167 //1000000/6000
	#define speed640mmps 156 //1000000/6400
	#define speed680mmps 147 //1000000/6800
	#define speed720mmps 140 //1000000/7200
	#define speed760mmps 133 //1000000/7600
	#define speed800mmps 125 //1000000/8000

//预设加速曲线
//20档速度
//200ms 每一段10ms
const unsigned short xCurve[20][2]=
{
////speed,pulseNum
// speed40mmps,4,//4=10000/speed40mmps
// speed80mmps,8,//8=10000/speed80mmps
// speed120mmps,12,//11=10000/speed120mmps
// speed160mmps,16,//16=10000/speed160mmps
// speed200mmps,20,//20=10000/speed200mmps
// speed240mmps,24,//24=10000/speed240mmps
// speed280mmps,28,//28=10000/speed280mmps
// speed320mmps,32,//32=10000/speed320mmps
// speed360mmps,36,//36=10000/speed360mmps
// speed400mmps,40,//40=10000/speed400mmps
// speed440mmps,44,//44=10000/speed440mmps
// speed480mmps,48,//48=10000/speed480mmps
// speed520mmps,52,
// speed560mmps,56,
// speed600mmps,60,
// speed640mmps,64,
// speed680mmps,68,
// speed720mmps,72,
// speed760mmps,76,
// speed800mmps,80,
////speed,pulseNum
 speed40mmps,2,//4=10000/speed40mmps
 speed80mmps,4,//8=10000/speed80mmps
 speed120mmps,6,//11=10000/speed120mmps
 speed160mmps,8,//16=10000/speed160mmps
 speed200mmps,10,//20=10000/speed200mmps
 speed240mmps,12,//24=10000/speed240mmps
 speed280mmps,14,//28=10000/speed280mmps
 speed320mmps,16,//32=10000/speed320mmps
 speed360mmps,18,//36=10000/speed360mmps
 speed400mmps,20,//40=10000/speed400mmps
 speed440mmps,22,//44=10000/speed440mmps
 speed480mmps,24,//48=10000/speed480mmps
 speed520mmps,26,
 speed560mmps,28,
 speed600mmps,30,
 speed640mmps,32,
 speed680mmps,34,
 speed720mmps,36,
 speed760mmps,38,
 speed800mmps,40,
};

unsigned short yCurve[20][2]=
{
 speed40mmps,2,//4=10000/speed40mmps
 speed80mmps,4,//8=10000/speed80mmps
 speed120mmps,6,//11=10000/speed120mmps
 speed160mmps,8,//16=10000/speed160mmps
 speed200mmps,10,//20=10000/speed200mmps
 speed240mmps,12,//24=10000/speed240mmps
 speed280mmps,14,//28=10000/speed280mmps
 speed320mmps,16,//32=10000/speed320mmps
 speed360mmps,18,//36=10000/speed360mmps
 speed400mmps,20,//40=10000/speed400mmps
 speed440mmps,22,//44=10000/speed440mmps
 speed480mmps,24,//48=10000/speed480mmps
 speed520mmps,26,
 speed560mmps,28,
 speed600mmps,30,
 speed640mmps,32,
 speed680mmps,34,
 speed720mmps,36,
 speed760mmps,38,
 speed800mmps,40,
};

unsigned short xIndexNum;
unsigned char xDir3rd;
unsigned short xMoveCurve[41][2];

//查表法 T型加速
//速度档位
//speedLevel 0-19
void xTCurveCal(short targetPosition,unsigned char speedLevel)
{
	unsigned char i;
	unsigned short pulseNum;
	unsigned short pulseNumTemp;
	unsigned short pulseNumTempLast;
	unsigned short pulseNumHalf;
	unsigned char accelPulseIndexNum;
	unsigned char uniformPulseIndexNum;
	unsigned char decelPulseIndexNum;
	unsigned char totalPulseIndexNum;
	if(targetPosition < 0) 
	{
		xDir3rd = 0;
		pulseNum = -targetPosition;
	}
	else 
	{
		xDir3rd = 1;
		pulseNum = targetPosition;
	}
	pulseNumHalf = pulseNum/2;
	pulseNumTemp = 0;
	pulseNumTempLast = 0;
  for(i=0;i<41;i++)
	{
		xMoveCurve[i][0] = 0;
		xMoveCurve[i][1] = 0;		
	}
	//速度等级超限
	if(speedLevel > 19)
	{
		xIndexNum = 0;
		return ;
	}
    //如果总脉冲数为0
	if(pulseNum == 0)
	{
		xIndexNum = 0;
		return ;
	}
	//如果总脉冲数为1	
	else if(pulseNum == 1)
	{
		xMoveCurve[0][0] = xCurve[0][0];
		xMoveCurve[0][1] = pulseNum;
		xIndexNum = 1;
		return;
	}
	//如果总脉冲数超过1
	// 计算加速段
	for(i=0;i<(speedLevel+1);i++)
	{
		pulseNumTemp += xCurve[i][1];
		if(pulseNumTemp < pulseNumHalf)
		{
			xMoveCurve[i][0] = xCurve[i][0];
			xMoveCurve[i][1] = xCurve[i][1];
		}
		else
		{
			xMoveCurve[i][0] = xCurve[i][0];	
			xMoveCurve[i][1] = pulseNumHalf - pulseNumTempLast;
			pulseNumTemp = pulseNumHalf;
            i++;
			break;
		}
		pulseNumTempLast = pulseNumTemp;
	}
	accelPulseIndexNum = i;
	// 计算匀速段
	xMoveCurve[i][0] = xCurve[i-1][0];
	xMoveCurve[i][1] = pulseNum - pulseNumTemp - pulseNumTemp;
	if(xMoveCurve[i][1] != 0)
	{
		i++;
		uniformPulseIndexNum = 1;
	}
	else
	{
		uniformPulseIndexNum = 0;
	}

	//假如加速段ACCelPulseIndexNum为4
	//匀速段为1
	//则总共为4+1+4=9段

	//假如加速段ACCelPulseIndexNum为4
	//匀速段为0
	//则总共为4+0+4=8段

	// 计算减速段 为加速段的镜像
	decelPulseIndexNum = accelPulseIndexNum;
	totalPulseIndexNum = accelPulseIndexNum + uniformPulseIndexNum + decelPulseIndexNum;
	for(;i<totalPulseIndexNum;i++)
	{
		xMoveCurve[i][0] = xMoveCurve[totalPulseIndexNum-i-1][0];
		xMoveCurve[i][1] = xMoveCurve[totalPulseIndexNum-i-1][1];
	}
	

	xIndexNum = totalPulseIndexNum;
}






unsigned short yIndexNum;
unsigned char yDir3rd;
unsigned short yMoveCurve[41][2];

// 查表法 T 型加速

// 速度档位
void yTCurveCal(short targetPosition,unsigned char speedLevel)
{
    unsigned char i;
		unsigned short pulseNum;
    unsigned short pulseNumTemp;
    unsigned short pulseNumTempLast;
    unsigned short pulseNumHalf;
    unsigned char accelPulseIndexNum;
    unsigned char uniformPulseIndexNum;
    unsigned char decelPulseIndexNum;
    unsigned char totalPulseIndexNum;
		if(targetPosition < 0) 
		{
			yDir3rd = 0;
			pulseNum = -targetPosition;
		}
		else 
		{
			yDir3rd = 1;
			pulseNum = targetPosition;
		}
    pulseNumHalf = pulseNum / 2;
    pulseNumTemp = 0;
    pulseNumTempLast = 0;
    for(i=0;i<41;i++)
	{
		yMoveCurve[i][0] = 0;
		yMoveCurve[i][1] = 0;		
	}   
    // 速度等级超限
    if (speedLevel > 19)
    {
        yIndexNum = 0;
        return;
    }
    if (pulseNum == 0)
    {
        yIndexNum = 0;
        return;
    }
    // 如果总脉冲数为 1
    else if (pulseNum == 1)
    {
        yMoveCurve[0][0] = yCurve[0][0];
        yMoveCurve[0][1] = pulseNum;
        yIndexNum = 1;
        return;
    }
    // 如果总脉冲数超过 1
    // 计算加速段
    for (i = 0; i < (speedLevel + 1); i++)
    {
        pulseNumTemp += yCurve[i][1];
        if (pulseNumTemp < pulseNumHalf)
        {
            yMoveCurve[i][0] = yCurve[i][0];
            yMoveCurve[i][1] = yCurve[i][1];
        }
        else
        {
            yMoveCurve[i][0] = yCurve[i][0];
            yMoveCurve[i][1] = pulseNumHalf - pulseNumTempLast;
            pulseNumTemp = pulseNumHalf;
            i++;
            break;
        }
        pulseNumTempLast = pulseNumTemp;
    }
    accelPulseIndexNum = i;
    // 计算匀速段
    yMoveCurve[i][0] = yCurve[i - 1][0];
    yMoveCurve[i][1] = pulseNum - pulseNumTemp - pulseNumTemp;
    if (yMoveCurve[i][1] != 0)
    {
        i++;
        uniformPulseIndexNum = 1;
    }
    else
    {
        uniformPulseIndexNum = 0;
    }

    // 计算减速段 为加速段的镜像
    decelPulseIndexNum = accelPulseIndexNum;
    totalPulseIndexNum = accelPulseIndexNum + uniformPulseIndexNum + decelPulseIndexNum;
    for (; i < totalPulseIndexNum; i++)
    {
        yMoveCurve[i][0] = yMoveCurve[totalPulseIndexNum - i - 1][0];
        yMoveCurve[i][1] = yMoveCurve[totalPulseIndexNum - i - 1][1];
    }
    yIndexNum = totalPulseIndexNum;
}

#include <stdio.h>
//xy曲线同步
void xyTCurveSync()
{
    unsigned char i;

	//计算t型曲线所需要的时间
	int xTime = 0;
	int yTime = 0;
	float ratio;
	unsigned int temp;
     printf("temp: %d\n", temp);
	for(i=0;i<41;i++)
	{
		xTime += xMoveCurve[i][0]*xMoveCurve[i][1];
		yTime += yMoveCurve[i][0]*yMoveCurve[i][1];
	}
printf("xtime: %d, ytime: %d\n", xTime, yTime);
	//速度匹配计算
	if(xTime > yTime)
	{
		if(yTime != 0)
		{
            ratio = (float)xTime / yTime;
			for(i=0;i<41;i++)
			{
                temp = yMoveCurve[i][0] * ratio;
                printf("temp: %d\n", temp);
                yMoveCurve[i][0] = (temp > 65535) ? 65535 : temp;
			}
		}
	}
	else if(yTime > xTime)
	{
		if(xTime != 0)
		{
			for(i=0;i<41;i++)
			{
                ratio = (float)yTime / xTime;
			    for(i=0;i<41;i++)
			    {
                    temp = xMoveCurve[i][0] * ratio;
                    printf("temp: %d\n", temp);
                    xMoveCurve[i][0] = (temp > 65535) ? 65535 : temp;
			    }
			}
		}
	}
}



int main() {

    // 测试 xTCurveCal 函数
    short targetPositionX = 1; // 目标位置
    unsigned char speedLevelX = 10; // 速度等级
    xTCurveCal(targetPositionX, speedLevelX);

    // 打印 xMoveCurve 的结果
    printf("xMoveCurve:\n");
    for (unsigned char i = 0; i < xIndexNum; i++) {
        printf("Speed: %d, Pulse: %d\n", xMoveCurve[i][0], xMoveCurve[i][1]);
    }

    // 测试 yTCurveCal 函数
    short targetPositionY = 30000; // 目标位置
    unsigned char speedLevelY = 15; // 速度等级
    yTCurveCal(targetPositionY, speedLevelY);

    // 打印 yMoveCurve 的结果
    printf("yMoveCurve:\n");
    for (unsigned char i = 0; i < yIndexNum; i++) {
        printf("Speed: %d, Pulse: %d\n", yMoveCurve[i][0], yMoveCurve[i][1]);
    }

    // 测试 xyTCurveSync 函数
    xyTCurveSync();

    // 打印同步后的 xMoveCurve 和 yMoveCurve 结果
    printf("Synchronized xMoveCurve:\n");
    for (unsigned char i = 0; i < xIndexNum; i++) {
        printf("Speed: %d, Pulse: %d\n", xMoveCurve[i][0], xMoveCurve[i][1]);
    }

    printf("Synchronized yMoveCurve:\n");
    for (unsigned char i = 0; i < yIndexNum; i++) {
        printf("Speed: %d, Pulse: %d\n", yMoveCurve[i][0], yMoveCurve[i][1]);
    }
    // 测试 xyTCurveSync 函数
    xyTCurveSync();
    return 0;
}