1820 lines
44 KiB
C
1820 lines
44 KiB
C
|
||
#define _IN_MOTOS_C
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#include "motos.h"
|
||
#include "stm32motos.h"
|
||
#include "inout.h"
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
const u16 g_axisIDList[AXIS_NUM] =
|
||
{
|
||
AXIS_ID1,
|
||
AXIS_ID2,
|
||
AXIS_ID3,
|
||
AXIS_ID4,
|
||
AXIS_ID5,
|
||
AXIS_ID6,
|
||
};
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 初始化电机
|
||
void InitMotos(void)
|
||
{
|
||
InitSTM32Motos();
|
||
|
||
#if (STM32_MOTOS_NUM >= 1)
|
||
InitStm32Moto1OutFunc(SetMoto1PulseOn, SetMoto1PulseOff, SetMoto1SignOn, SetMoto1SignOff);
|
||
#endif
|
||
|
||
#if (STM32_MOTOS_NUM >= 2)
|
||
InitStm32Moto2OutFunc(SetMoto2PulseOn, SetMoto2PulseOff, SetMoto2SignOn, SetMoto2SignOff);
|
||
#endif
|
||
|
||
#if (STM32_MOTOS_NUM >= 3)
|
||
InitStm32Moto3OutFunc(SetMoto3PulseOn, SetMoto3PulseOff, SetMoto3SignOn, SetMoto3SignOff);
|
||
#endif
|
||
|
||
#if (STM32_MOTOS_NUM >= 4)
|
||
InitStm32Moto4OutFunc(SetMoto4PulseOn, SetMoto4PulseOff, SetMoto4SignOn, SetMoto4SignOff);
|
||
#endif
|
||
|
||
#if (STM32_MOTOS_NUM >= 5)
|
||
InitStm32Moto5OutFunc(SetMoto5PulseOn, SetMoto5PulseOff, SetMoto5SignOn, SetMoto5SignOff);
|
||
#endif
|
||
|
||
#if (STM32_MOTOS_NUM >= 6)
|
||
InitStm32Moto6OutFunc(SetMoto6PulseOn, SetMoto6PulseOff, SetMoto6SignOn, SetMoto6SignOff);
|
||
#endif
|
||
|
||
memset(&g_motosPara, 0, sizeof(MotosPara)); // 设置电机参数
|
||
}
|
||
|
||
// 读取电机位置
|
||
int GetMotosCounter(s32 * position)
|
||
{
|
||
int i;
|
||
|
||
if (position != NULL)
|
||
{
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
position[i] = GetAxisCounter(g_axisIDList[i]);
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 设置电机位置
|
||
void SetMotosCounter(s32 * position)
|
||
{
|
||
int i;
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
if (position != NULL)
|
||
{
|
||
SetAxisCounter(g_axisIDList[i], position[i]);
|
||
}
|
||
else
|
||
{
|
||
SetAxisCounter(g_axisIDList[i], 0);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 刷新坐标
|
||
void RefreshMotosPos(s32 * poslist)
|
||
{
|
||
// 全部刷新
|
||
int i;
|
||
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
g_motosPara.motosPos[i] = poslist[i];
|
||
}
|
||
g_motosPara.enFlag = DATA_VALID;
|
||
}
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
s32 GetMotoCounter(int axisIdx)
|
||
{
|
||
assert_param(axisIdx < AXIS_NUM);
|
||
return GetAxisCounter(g_axisIDList[axisIdx]);
|
||
}
|
||
|
||
void SetMotoCounter(int axisIdx, s32 pos)
|
||
{
|
||
assert_param(axisIdx < AXIS_NUM);
|
||
SetAxisCounter(g_axisIDList[axisIdx], pos);
|
||
}
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
void MotoServoCtrl(int axisIdx, int onoff)
|
||
{
|
||
assert_param(onoff == SERVO_ON || onoff == SERVO_OFF);
|
||
|
||
if (axisIdx < AXIS_NUM)
|
||
{
|
||
SetAxisConfig(g_axisIDList[axisIdx], onoff);
|
||
}
|
||
}
|
||
|
||
// 设置电机配置
|
||
void SetMotosConfig(MotosConfig * pConfig)
|
||
{
|
||
int i;
|
||
u16 axisID;
|
||
if (pConfig == NULL)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (pConfig->newConfig == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 停止运行
|
||
StopVAxisRun(VAXIS_ID_ALL);
|
||
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
axisID = g_axisIDList[i];
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & CONFIG_EN) != 0) // 配置允许
|
||
{
|
||
if ((pConfig->axisConfig[i].axisConfig & POA_EN) != 0) // A允许输出
|
||
{
|
||
SetAxisConfig(axisID, POUTA_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUTA_DIS);
|
||
}
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & POB_EN) != 0) // B允许输出
|
||
{
|
||
SetAxisConfig(axisID, POUTB_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUTB_DIS);
|
||
}
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & SON_EN) != 0) // 伺服ON允许
|
||
{
|
||
SetAxisConfig(axisID, SERVO_ON);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, SERVO_OFF);
|
||
}
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & ALM_LEVH) != 0) // 报警电平为高
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
|
||
}
|
||
else
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
|
||
}
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & ALM_EN) != 0) // 报警停车允许
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_STOP_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
|
||
}
|
||
|
||
if ((pConfig->axisConfig[i].axisConfig & POUT_EN) != 0) // 输出允许
|
||
{
|
||
SetAxisConfig(axisID, pConfig->axisConfig[i].poutType); // 脉冲输出模式 脉冲方式输出
|
||
SetAxisConfig(axisID, pConfig->axisConfig[i].spdSource); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
SetAxisConfig(axisID, pConfig->axisConfig[i].datSource); // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUT_DISABLE); // 禁止脉冲输出
|
||
}
|
||
}
|
||
}
|
||
|
||
SetVAxisConfig(VCTR_AXIS_ALM_INT); // 清除报警状态
|
||
}
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 计算加减速需要的位移
|
||
// 参数: pps1 速度1
|
||
// pps2 速度2
|
||
// calcTime 速度变化时间，单位为1/72us
|
||
// 返回:
|
||
// 位移值
|
||
// 条件，速度变化量固定为1ppg
|
||
u32 CalcDisplacement(u32 pps1, u32 pps2, u32 calcTime)
|
||
{
|
||
// 速度变化量固定为1ppg
|
||
// 公式: d=(v2+v1)*(v2-v1)/(2a)
|
||
// v1 = pps1
|
||
// v2 = pps2
|
||
// a = 1ppg/(calcTime/FPGA_CLK)
|
||
// 最终 d=(v2+v1)*(v2-v1)*calcTime/(2*FPGA_CLK)
|
||
double temp;
|
||
if (pps1 < pps2)
|
||
{
|
||
temp = (pps2-pps1);
|
||
}
|
||
else
|
||
{
|
||
temp = (pps1-pps2);
|
||
}
|
||
temp *= (pps1+pps2);
|
||
temp *= calcTime;
|
||
temp /= (2*FPGA_CLK);
|
||
return (u32)(temp+0.5);
|
||
}
|
||
|
||
|
||
// 功能：计算某位移下速度变化需要的时间间隔参数（单位为1/72us）
|
||
// 参数：
|
||
// disp 位移
|
||
// pps1 速度1
|
||
// pps2 速度2
|
||
//
|
||
// 返回:
|
||
// 位移值
|
||
// 条件，速度变化量固定为1ppg
|
||
|
||
u32 CalcGapTime(u32 disp, u32 pps1, u32 pps2)
|
||
{
|
||
double temp;
|
||
// 速度变化量固定为1ppg
|
||
// 公式: a=(v2+v1)*(v2-v1)/(2*d)
|
||
// v1 = pps1
|
||
// v2 = pps2
|
||
// d = disp
|
||
// a = 1ppg/(calcTime/FPGA_CLK)
|
||
// 最终 calcTime= (2*FPGA_CLK)*d/((v2+v1)*(v2-v1))
|
||
|
||
if (disp == 0 || pps1 == pps2)
|
||
{
|
||
return 0;
|
||
}
|
||
if (pps1 < pps2)
|
||
{
|
||
temp = (pps2-pps1);
|
||
}
|
||
else
|
||
{
|
||
temp = (pps1-pps2);
|
||
}
|
||
temp *= (pps1+pps2);
|
||
temp = (2.0*FPGA_CLK)/temp;
|
||
temp *= disp;
|
||
return (u32)(temp+0.5);
|
||
}
|
||
|
||
// 计算某位移和某加速度下，最终的速度值
|
||
// 参数：
|
||
// disp 位移
|
||
// pps1 速度1
|
||
// addgap 时间间隔
|
||
//
|
||
// 返回:
|
||
// 位移值
|
||
// 条件，速度变化量固定为1ppg
|
||
|
||
u32 CalcEndSpd(u32 disp, u32 pps1, s32 addgap)
|
||
{
|
||
double temp;
|
||
// 速度变化量固定为1ppg
|
||
// 公式: a=(v2+v1)*(v2-v1)/(2*d)
|
||
// v1 = pps1
|
||
// d = disp
|
||
// a = 1ppg/(addgap/FPGA_CLK)
|
||
// 最终 pps2 = sqrt(2*FPGA_CLK*disp/addgap + pps1*pps1)
|
||
|
||
temp = 2.0*FPGA_CLK*disp/addgap;
|
||
temp += 1.0*pps1*pps1;
|
||
if (temp > 0)
|
||
{
|
||
temp = sqrt(temp);
|
||
}
|
||
else
|
||
{
|
||
temp = 0;
|
||
}
|
||
|
||
return (u32)(temp+0.5);
|
||
}
|
||
|
||
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||
// 功能：多轴直线插补运动控制(硬件实现)，最多支持6轴直线插补，支持虚轴1或虚轴2控制
|
||
// 参数：插补控制结构指针
|
||
// 结果: 0，运动执行完成。
|
||
// -1，输入参数错误
|
||
// -2，执行过程中填充数据不及时错误退出
|
||
// -3 fpga 读写错误
|
||
// -4 FPGA 版本错误
|
||
// =1, 驱动器报警停车
|
||
// =2, 紧急停止条件触发停车
|
||
// =3, 快速停止条件触发停车
|
||
// =4, 普通停车条件触发停车
|
||
|
||
#if (1)
|
||
|
||
int InterpolationMotion(InterMoveCtrl * pCtrl)
|
||
{
|
||
int i, rslt;
|
||
u32 totalnum, circlenum, circlesize, lastsize; // 运动总量，循环总次数，切换表个数，一般表的长度，最后一个表的长度
|
||
u32 runcircle, execcircle, runrepeat, exerepeat; // 运动段数，执行段数，运动重复次数，执行重复次数
|
||
u32 runcount;
|
||
u32 pulsePerSegment;
|
||
u16 vAxisId;
|
||
u16 vAxisCleanOv1, vAxisCleanOv2;
|
||
u16 vAxisBuf1Finish, vAxisBuf2Finish;
|
||
u16 vAxisBuf1Running, vAxisBuf2Running;
|
||
// u16 vAxisBuf1En, vAxisBuf2En;
|
||
u16 vAxisDisBuf1, vAxisDisBuf2;
|
||
|
||
u32 movement[AXIS_NUM], count[AXIS_NUM];
|
||
s32 val32, position[AXIS_NUM];
|
||
int dir[AXIS_NUM]; // 各个轴的运动方向
|
||
int runsta, bufsta, bufsel;
|
||
int slowSta;
|
||
int brksta, inbrk; // 刹车状态，正在刹车标志
|
||
int nstpsta, innstp; // 刹车状态，正在刹车标志
|
||
int stopfill; // 停止填充数据标志
|
||
u16 tmp16, vrsta, err;
|
||
double calc, len;
|
||
u16 outNum;
|
||
|
||
u32 startpps, runpps, slowpps, stoppps; // 速度
|
||
u32 addppsg, decppsg, brkppsg, tslppsg; // 加速度
|
||
u32 num, addnum, decnum, jumpnum; // 加减速需要的脉冲个数
|
||
u32 curtab1num, curtab2num;
|
||
|
||
VAxisCmdStr vAxisCmd;
|
||
InterData idata;
|
||
|
||
num = 0;
|
||
|
||
// ------------------------------------------------------------------------
|
||
// 参数合法性判断
|
||
if (pCtrl == NULL)
|
||
{
|
||
printf("para error in MotosRun\r\n");
|
||
rslt = -1; // 输入参数错误
|
||
return rslt;
|
||
}
|
||
|
||
if (pCtrl->vAxisId != VAXIS_ID1)
|
||
{
|
||
printf("para vAxisId error in MotosRun\r\n");
|
||
rslt = -1; // 输入参数错误
|
||
return rslt;
|
||
}
|
||
|
||
for (i = 0; i < AXIS_NUM; i++) // 统一实轴速度来源
|
||
{
|
||
pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
|
||
}
|
||
|
||
// 选择参与电机
|
||
SetMotosConfig(&(pCtrl->motosConfig));
|
||
|
||
// 等待驱动器执行
|
||
if (pCtrl->Delay != NULL && pCtrl->motosConfig.newConfig != 0)
|
||
{
|
||
pCtrl->Delay(100);
|
||
}
|
||
|
||
// ------------------------------------------------------------------------
|
||
|
||
if (pCtrl->GetEmergencyStop != NULL) // 紧急停止条件函数
|
||
{
|
||
err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
printf("emergency stop 1 in InterpolationMotion, err=0x%x\r\n", err);
|
||
rslt = 2; // 紧急停止条件触发停车，立刻停止脉冲发送
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetQuickStop != NULL) // 出错停止条件函数
|
||
{
|
||
err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
printf("quick stop 1 in InterpolationMotion, err=0x%x\r\n", err);
|
||
rslt = 3; // 快速停止条件触发停车
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetNormalStop != NULL) // 一般停止
|
||
{
|
||
err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
printf("normal stop 1 in InterpolationMotion, err=0x%x\r\n", err);
|
||
rslt = 4; // 普通停车条件触发停车
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
// 速度控制参数
|
||
startpps = pCtrl->spdPara.startPPS;
|
||
runpps = pCtrl->spdPara.runPPS;
|
||
slowpps = pCtrl->spdPara.slowPPS;
|
||
stoppps = pCtrl->spdPara.stopPPS;
|
||
addppsg = pCtrl->spdPara.addPPSG;
|
||
decppsg = pCtrl->spdPara.decPPSG;
|
||
brkppsg = pCtrl->spdPara.brkPPSG;
|
||
tslppsg = pCtrl->spdPara.brkPPSG*TOSLOWMUTI;
|
||
|
||
if (startpps == 0 || runpps == 0 || slowpps == 0 || stoppps == 0 || addppsg == 0 || decppsg == 0 || brkppsg == 0 || tslppsg == 0)
|
||
{
|
||
printf("one of speed para is zero \r\n");
|
||
printf("startpps = %d, runpps = %d, slowpps = %d, stoppps = %d, addppsg = %d, decppsg = %d, brkppsg = %d\r\n", startpps, runpps, slowpps, stoppps, addppsg, decppsg, brkppsg);
|
||
|
||
rslt = -1;
|
||
return rslt;
|
||
}
|
||
|
||
vAxisId = pCtrl->vAxisId;
|
||
|
||
if (vAxisId == VAXIS_ID1)
|
||
{
|
||
vAxisCleanOv1 = VCTR_VAXIS1_CLEAN_OV1;
|
||
vAxisCleanOv2 = VCTR_VAXIS1_CLEAN_OV2;
|
||
|
||
vAxisBuf1Finish = VAXIS1_BUF1_FINISH;
|
||
vAxisBuf2Finish = VAXIS1_BUF2_FINISH;
|
||
|
||
vAxisBuf1Running = VAXIS1_BUF1_RUNNING;
|
||
vAxisBuf2Running = VAXIS1_BUF2_RUNNING;
|
||
|
||
// vAxisBuf1En = VAXIS1_BUF1_ENABLE;
|
||
// vAxisBuf2En = VAXIS1_BUF2_ENABLE;
|
||
|
||
vAxisDisBuf1 = VCTR_VAXIS1_DIS_BUF1;
|
||
vAxisDisBuf2 = VCTR_VAXIS1_DIS_BUF2;
|
||
}
|
||
else
|
||
{
|
||
printf("not support vaxis\r\n");
|
||
}
|
||
// 计算最大位移
|
||
totalnum = 0; // 脉冲个数
|
||
circlenum = 0; // 周期数
|
||
circlesize = 0; // 每个循环周期的数量
|
||
lastsize = 0; // 最后一个周期的数量
|
||
|
||
runrepeat = pCtrl->mvmtPara.extraRepeat;
|
||
exerepeat = pCtrl->mvmtPara.extraRepeat;
|
||
|
||
// 得到实轴最大位移，作为最长轴
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
if (pCtrl->mvmtPara.movement[i] >= 0)
|
||
{
|
||
movement[i] = (u32)(pCtrl->mvmtPara.movement[i]);
|
||
dir[i] = 1;
|
||
}
|
||
else
|
||
{
|
||
movement[i] = (u32)(-(pCtrl->mvmtPara.movement[i]));
|
||
dir[i] = -1;
|
||
}
|
||
|
||
// printf("axis %d movement = %d.\r\n", i+1, movement[i]);
|
||
|
||
if (totalnum < movement[i])
|
||
{
|
||
totalnum = movement[i];
|
||
}
|
||
/*
|
||
if (pCtrl->interLongSel == LAS_COMPOSITE) // 几个轴的合成长度
|
||
{
|
||
calc = movement[i];
|
||
calc *= calc;
|
||
len += calc;
|
||
}
|
||
else
|
||
{
|
||
|
||
}
|
||
*/
|
||
count[i] = 0;
|
||
}
|
||
|
||
totalnum = pCtrl->mvmtPara.interLong;
|
||
pulsePerSegment = pCtrl->mvmtPara.pulsePerSegment;
|
||
/*
|
||
if (pCtrl->interLong == LAS_COMPOSITE) // 几个轴的合成长度
|
||
{
|
||
len = sqrt(len);
|
||
totalnum = (u32)(len+0.5);
|
||
}
|
||
*/
|
||
if (totalnum == 0)
|
||
{
|
||
printf("move count = 0. run over\r\n");
|
||
return -1;
|
||
}
|
||
else if (totalnum < 2)
|
||
{
|
||
circlenum = 1;
|
||
circlesize = totalnum;
|
||
lastsize = totalnum;
|
||
}
|
||
else if (totalnum < pulsePerSegment*2) // 小于2个最大位图周期, 将其分为均匀的两个部分
|
||
{
|
||
circlenum = 2;
|
||
circlesize = (totalnum)/2;
|
||
lastsize = totalnum-circlesize;
|
||
}
|
||
else
|
||
{
|
||
circlenum = (totalnum + pulsePerSegment-1)/pulsePerSegment;
|
||
circlesize = pulsePerSegment;
|
||
lastsize = totalnum-circlesize*(circlenum-1);
|
||
}
|
||
|
||
|
||
#if (0)
|
||
if (pCtrl->stopMode == STOP_MODE_TAB) // 整点停止模式下，长轴数据大于一个表时，必须为整表
|
||
{
|
||
if (lastsize != circlesize && totalnum > )
|
||
{
|
||
printf("totalnum is not equal to circle nun\r\n");
|
||
return 0;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
// printf("calc para. circlenum=%d, circlesize=%d, lastsize=%d\r\n", circlenum, circlesize, lastsize);
|
||
|
||
if (circlenum == 0 || circlesize == 0 || lastsize == 0)
|
||
{
|
||
printf("calc error. circlenum=%d, circlesize=%d, lastsize=%d\r\n", circlenum, circlesize, lastsize);
|
||
return -1;
|
||
}
|
||
|
||
// 速度控制参数
|
||
addnum = CalcDisplacement(startpps, runpps, addppsg); // 加速脉冲数
|
||
decnum = CalcDisplacement(stoppps, runpps, decppsg); // 减速脉冲数
|
||
|
||
// printf("before run, start=%d, stop=%d, runing=%d, \r\n\taddppsg=%d, addnum=%d, decppsg=%d, decnum=%d\r\n",startpps, stoppps, runpps, addppsg, addnum, decppsg, decnum);
|
||
|
||
if ((totalnum < addnum + decnum) ||
|
||
((totalnum < (addnum + decnum + pulsePerSegment)) && (addnum%pulsePerSegment) != 0) ||
|
||
0 )
|
||
{
|
||
// 重新计算 加速脉冲个数，最高速度，和减速脉冲个数
|
||
calc = addnum;
|
||
calc /= (addnum + decnum);
|
||
calc *= totalnum; // 按照比例，加速脉冲的个数
|
||
calc /= pulsePerSegment; // 加速圈数
|
||
addnum = (u32)calc;
|
||
if (addnum != 0)
|
||
{
|
||
addnum *= pulsePerSegment; // 取整的脉冲数
|
||
}
|
||
else
|
||
{
|
||
addnum = totalnum / 2;
|
||
}
|
||
|
||
len = addnum*2;
|
||
len *= FPGA_CLK;
|
||
len /= addppsg;
|
||
len -= startpps;
|
||
calc = startpps;
|
||
calc *= startpps;
|
||
len += calc;
|
||
calc = sqrt(len); // 加速脉冲数
|
||
runpps = (u32)(calc+0.5);
|
||
decnum = CalcDisplacement(stoppps, runpps, decppsg); // 重新计算减速脉冲数
|
||
|
||
// printf("recalc addnum=%d, runpps=%d, decnum=%d\r\n", addnum, runpps, decnum);
|
||
}
|
||
|
||
// 控制循环
|
||
runsta = 0;
|
||
bufsel = 0;
|
||
bufsta = 0;
|
||
slowSta = 0;
|
||
brksta = 0;
|
||
inbrk = 0;
|
||
nstpsta = 0;
|
||
innstp = 0;
|
||
stopfill = 0; // 停止填充数据标志
|
||
|
||
runcircle = circlenum; // 计算计数
|
||
execcircle = circlenum; // 执行计数
|
||
runcount = 0;
|
||
jumpnum = 0;
|
||
pCtrl->errInfo = 0;
|
||
curtab1num = 0;
|
||
curtab2num = 0;
|
||
rslt = 0;
|
||
|
||
if (pCtrl->WorkBeforeRun != NULL)
|
||
{
|
||
pCtrl->WorkBeforeRun(pCtrl->workPara1, pCtrl->workPara2);
|
||
}
|
||
|
||
SetVAxisConfig(VCTR_AXIS_ALM_INT); // 清除报警状态
|
||
|
||
memset(&idata, 0, sizeof(InterData));
|
||
|
||
while(execcircle > 0 || exerepeat > 0) // 执行未完成
|
||
{
|
||
if ( stopfill == 0 && // 允许填充数据
|
||
(bufsta & 0x03) != 0x03 && // 有buff空闲
|
||
(runcircle != 0 || runrepeat != 0) && // 数据未完成
|
||
1 )
|
||
{
|
||
if (runcircle == 1) // 最后一圈计算
|
||
{
|
||
num = lastsize;
|
||
if (runrepeat == 0)
|
||
{
|
||
runsta = 1; // 最后一圈减速
|
||
}
|
||
}
|
||
else
|
||
{
|
||
num = circlesize;
|
||
if (runrepeat == 0 && (runsta == 0 && decnum > lastsize)) // 如果减速需要脉冲个数大于最后一圈的位移
|
||
{
|
||
if ((runcircle-1) <= (decnum-lastsize+circlesize-1)/circlesize)
|
||
{
|
||
runsta = 1; // 减速标志
|
||
}
|
||
}
|
||
}
|
||
runcount += num;
|
||
|
||
if (runsta == 1) // 计算减速点
|
||
{
|
||
if (decnum > lastsize)
|
||
{
|
||
jumpnum = num - (decnum - lastsize) % num;
|
||
}
|
||
else
|
||
{
|
||
jumpnum = lastsize - decnum;
|
||
}
|
||
if (jumpnum >= num)
|
||
{
|
||
jumpnum = 0;
|
||
}
|
||
if (decnum != 0)
|
||
{
|
||
decnum = decnum - (num - jumpnum);
|
||
}
|
||
// printf("jumpnum=%d, decnum=%d\r\n", jumpnum, decnum);
|
||
}
|
||
|
||
for (i = 0; i < AXIS_NUM; i++) // 写各个轴的插补数据
|
||
{
|
||
if (movement[i] != 0)
|
||
{
|
||
if (bufsel == 0)
|
||
{
|
||
idata.bufSel = BUF_SEL1;
|
||
}
|
||
else
|
||
{
|
||
idata.bufSel = BUF_SEL2;
|
||
}
|
||
|
||
// 计算该段插补需要的实轴数据
|
||
//--------------------------
|
||
idata.vaxisId = vAxisId; // 使用虚轴1
|
||
if (dir[i] >= 0) // 方向
|
||
{
|
||
idata.dirAttr = DIR_POSPOS; // 正向
|
||
}
|
||
else
|
||
{
|
||
idata.dirAttr = DIR_NEGNEG; // 反向
|
||
}
|
||
idata.datBufLen = 1+1; // 有效段个数: 1
|
||
|
||
idata.axisId = g_axisIDList[i];
|
||
idata.startEscNum = 0;
|
||
idata.partVxNum = num;
|
||
calc = movement[i];
|
||
calc *= runcount;
|
||
calc /= totalnum;
|
||
calc -= count[i];
|
||
outNum = (u16)(calc+0.5);
|
||
count[i] += outNum;
|
||
idata.datBuff[0] = outNum; // 第一段数据
|
||
idata.datBuff[1] = 0; // 结束段写0
|
||
idata.sineOut = 0;
|
||
|
||
//printf("%d.datBuff[0]=%d\r\n", i+1, idata.datBuff[0]);
|
||
rslt = WriteInterData(&idata);
|
||
if (rslt < 0)
|
||
{
|
||
return rslt;
|
||
}
|
||
|
||
// printf("axis %d wt, c=%d, ac=%d\r\n", i+1, outNum, count[i]);
|
||
|
||
if (runcount >= totalnum)
|
||
{
|
||
count[i] = 0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (bufsel == 0)
|
||
{
|
||
CleanAxisBuff(g_axisIDList[i], BUF_SEL1);
|
||
}
|
||
else
|
||
{
|
||
CleanAxisBuff(g_axisIDList[i], BUF_SEL2);
|
||
}
|
||
}
|
||
}
|
||
if (runcount >= totalnum)
|
||
{
|
||
runcount = 0;
|
||
}
|
||
|
||
// 启动运动, 虚轴命令
|
||
vAxisCmd.vaxisId = vAxisId; // 使用虚轴
|
||
if (bufsel == 0)
|
||
{
|
||
bufsta |= 0x01;
|
||
vAxisCmd.bufSel = BUF_SEL1;
|
||
curtab1num = num;
|
||
// printf("write buff1\r\n");
|
||
}
|
||
else
|
||
{
|
||
bufsta |= 0x02;
|
||
vAxisCmd.bufSel = BUF_SEL2;
|
||
curtab2num = num;
|
||
// printf("write buff2\r\n");
|
||
}
|
||
|
||
if (runsta == 0)
|
||
{
|
||
// printf("write add vtable\r\n");
|
||
|
||
vAxisCmd.startPPS = startpps; // 启动速度
|
||
vAxisCmd.runPPS = runpps; // 运行速度
|
||
vAxisCmd.jumpNum = 0; // 跳过脉冲数
|
||
vAxisCmd.calcGap = addppsg; // 计算时间
|
||
}
|
||
else
|
||
{
|
||
// printf("write dec vtable, jump=%d\r\n", jumpnum);
|
||
|
||
vAxisCmd.startPPS = stoppps; // 启动速度
|
||
vAxisCmd.runPPS = stoppps; // 运行速度
|
||
vAxisCmd.jumpNum = jumpnum; // 跳过脉冲数
|
||
vAxisCmd.calcGap = decppsg; // 计算时间
|
||
}
|
||
|
||
vAxisCmd.outNum = num; // 表的脉冲个数
|
||
|
||
StartVAxisRun(&vAxisCmd); // 写运行命令
|
||
|
||
// 得到虚轴运行状态
|
||
// printf("At%d, StartVAxisRun, vrsta=0x%x, vrsta1=0x%x\r\n", Get100UsSoftTimer(), GetRunStatus(), GetRunStatus1());
|
||
runcircle--; // 计算计数减小
|
||
if (runcircle == 0)
|
||
{
|
||
if (runrepeat != 0)
|
||
{
|
||
runrepeat--;
|
||
runcircle = circlenum;
|
||
}
|
||
}
|
||
bufsel = ~bufsel; // 切换buf
|
||
}
|
||
|
||
// 等待运动结束
|
||
rslt = 0;
|
||
do
|
||
{
|
||
err = 0;
|
||
//----------
|
||
if (slowSta != 0 && slowpps != pCtrl->spdPara.slowPPS)
|
||
{
|
||
printf("reset slowsta when speed change\r\n");
|
||
slowSta = 0;
|
||
}
|
||
slowpps = pCtrl->spdPara.slowPPS; // 更新降速速度
|
||
//----------
|
||
|
||
{
|
||
int count = 100;
|
||
do
|
||
{
|
||
// 得到虚轴运行状态
|
||
tmp16 = GetRunStatus();
|
||
vrsta = GetRunStatus();
|
||
|
||
// printf("1. run sta =0x%x\r\n", vrsta);
|
||
|
||
while(tmp16 != vrsta) // 稳定的值
|
||
{
|
||
// printf("1. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
|
||
tmp16 = vrsta;
|
||
vrsta = GetRunStatus();
|
||
}
|
||
|
||
if ( (vrsta & (vAxisBuf1Finish|vAxisBuf1Running)) != (vAxisBuf1Finish|vAxisBuf1Running) &&
|
||
(vrsta & (vAxisBuf2Finish|vAxisBuf2Running)) != (vAxisBuf2Finish|vAxisBuf2Running) ) // 完成标志和运行标志不可能同时有效，这时有可能是读取错误
|
||
{
|
||
break;
|
||
}
|
||
|
||
printf("1. rd sta err, sta=0x%x, count=%d\r\n", vrsta, 100-count);
|
||
|
||
}while(count--);
|
||
|
||
if (count == 0)
|
||
{
|
||
printf("rd fpga data timout\r\n");
|
||
StopVAxisRun(vAxisId);
|
||
rslt = -3; // fpga 读写错误
|
||
break;
|
||
}
|
||
|
||
|
||
if ((vrsta & vAxisBuf1Finish) != 0) // 虚轴缓冲区1数据执行完成标志
|
||
{
|
||
SetVAxisConfig(vAxisCleanOv1); // 清除表1完成标志
|
||
bufsta &= 0xFE; // 释放buf1
|
||
execcircle--;
|
||
if (execcircle == 0)
|
||
{
|
||
if (exerepeat != 0)
|
||
{
|
||
exerepeat--;
|
||
execcircle = circlenum;
|
||
}
|
||
}
|
||
curtab1num = 0;
|
||
// printf("At%d, table 1 ok, vrsta=0x%x, vsta=0x%x, vrsta1=0x%x\r\n", GetUsSoftTimer(), vrsta, GetRunStatus(), GetRunStatus1());
|
||
}
|
||
if ((vrsta & vAxisBuf2Finish) != 0) // 虚轴缓冲区2数据执行完成标志
|
||
{
|
||
SetVAxisConfig(vAxisCleanOv2); // 清除表2完成标志
|
||
bufsta &= 0xFD; // 释放buf2
|
||
execcircle--;
|
||
if (execcircle == 0)
|
||
{
|
||
if (exerepeat != 0)
|
||
{
|
||
exerepeat--;
|
||
execcircle = circlenum;
|
||
}
|
||
}
|
||
curtab2num = 0;
|
||
|
||
// printf("At%d, table 2 ok, vrsta=0x%x, vsta=0x%x, vrsta1=0x%x\r\n", GetUsSoftTimer(), vrsta, GetRunStatus(), GetRunStatus1());
|
||
}
|
||
|
||
if ((vrsta & AXIS_ALARM_FLAG) != 0) // 实轴报警标志
|
||
{
|
||
err = GetAlarmValue(); // 得到轴报警信息
|
||
SetVAxisConfig(VCTR_AXIS_ALM_INT); // 清除状态
|
||
StopVAxisRun(vAxisId);
|
||
if (err != 0)
|
||
{
|
||
printf("1. axis alarm value=0x%x\r\n", err);
|
||
}
|
||
rslt = 1; // 驱动器报警停车
|
||
break;
|
||
}
|
||
else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0) // 两个缓冲区的数据都执行完成，没有有效数据
|
||
{
|
||
#if (0)
|
||
temp16 = GetRunStatus1();
|
||
if ((temp16 & (vAxisBuf1En|vAxisBuf2En)) == 0)
|
||
#endif
|
||
{
|
||
// printf("At%d, no more data in two buff in InterpolationMotion\r\n", GetUsSoftTimer());
|
||
StopVAxisRun(vAxisId);
|
||
if (pCtrl->stopMode == STOP_MODE_TAB)
|
||
{
|
||
// printf("stop at tab end\r\n");
|
||
|
||
if (brksta == 1)
|
||
{
|
||
rslt = 3; // 快速停止条件触发停车，执行完一个缓冲区数据
|
||
}
|
||
else // if (nstpsta == 1)
|
||
{
|
||
rslt = 4; // 普通停车条件触发停车，执行完一个缓冲区数据
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// printf("stop when spd low, execcircle=%d,exerepeat=%d\r\n", execcircle, exerepeat);
|
||
if (execcircle == 0 && exerepeat == 0)
|
||
{
|
||
/*
|
||
* 20221124
|
||
* 数据执行完成的优先级高
|
||
* 如果停止条件触发,但降速完成后数据恰好执行完成,则认为是正常停车,而非停止条件停车
|
||
*/
|
||
rslt = 0;
|
||
}
|
||
else
|
||
{
|
||
rslt = -2; // 无有效数据，退出
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 停止条件判断
|
||
if (pCtrl->GetEmergencyStop != NULL) // 急停条件函数
|
||
{
|
||
err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
StopVAxisRun(vAxisId);
|
||
printf("1. emergency stop 3, err=0x%x\r\n", err);
|
||
rslt = 2; // 紧急停止条件触发停车，立刻停止脉冲发送
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (inbrk != 0 || (brksta != 1 && pCtrl->GetQuickStop != NULL)) // 快停条件函数
|
||
{
|
||
if (inbrk == 0)
|
||
{
|
||
err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
// printf("quick stop in InterpolationMotion, err=0x%x\r\n", err);
|
||
}
|
||
}
|
||
|
||
if (err != 0 || inbrk != 0)
|
||
{
|
||
// 计算从当前速度降速到停止需要的位移
|
||
if (pCtrl->stopMode == STOP_MODE_TAB)
|
||
{
|
||
u32 curpps, curmvmt, decCount, resdcount;
|
||
curpps = GetVAxisPPS(vAxisId); // 当前速度
|
||
decCount = CalcDisplacement(curpps, stoppps, brkppsg); // 计算以当前运行速度减速到停止速度需要的位移
|
||
curmvmt = GetVAxisCounter(vAxisId); // 当前表运行进度
|
||
|
||
// 运行状态
|
||
tmp16 = GetRunStatus();
|
||
vrsta = GetRunStatus();
|
||
while(tmp16 != vrsta)
|
||
{
|
||
// printf("3. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
|
||
tmp16 = vrsta;
|
||
vrsta = GetRunStatus();
|
||
}
|
||
|
||
// printf("cur vaxis counter=%d, vrsta=0x%x\r\n", curmvmt, vrsta);
|
||
resdcount = 0;
|
||
if (decCount > (curtab1num+curtab2num-curmvmt)) // 减速需要脉冲数大于已填充数据
|
||
{
|
||
inbrk = 1; // 设置进入降速状态，但需要继续填充数据和计算减速点
|
||
}
|
||
else
|
||
{
|
||
// 判断当前运行的表是那个表
|
||
if ((vrsta & vAxisBuf1Running) != 0) // 表1正在执行
|
||
{
|
||
if (decCount < (curtab1num-curmvmt))
|
||
{
|
||
resdcount = (curtab1num-curmvmt);
|
||
SetVAxisConfig(vAxisDisBuf2); // 取消掉表2的已填充数据
|
||
}
|
||
}
|
||
else if ((vrsta & vAxisBuf2Running) != 0) // 表2正在执行
|
||
{
|
||
if (decCount < (curtab2num-curmvmt))
|
||
{
|
||
resdcount = (curtab2num-curmvmt);
|
||
SetVAxisConfig(vAxisDisBuf1); // 取消掉表1的已填充数据
|
||
}
|
||
}
|
||
|
||
if (resdcount == 0)
|
||
{
|
||
resdcount = (curtab1num+curtab2num-curmvmt);
|
||
}
|
||
|
||
brkppsg = CalcGapTime(decCount*num, curpps, stoppps); // 按照新位移，重新计算刹车加速度
|
||
|
||
inbrk = 0; // 不再进入
|
||
stopfill = 1; // 不再填充数据
|
||
}
|
||
}
|
||
|
||
// 立即降速
|
||
brksta = 1; // 设置刹车车状态
|
||
|
||
vAxisCmd.vaxisId = vAxisId;
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = brkppsg;
|
||
vAxisCmd.runPPS = stoppps;
|
||
vAxisCmd.startPPS = stoppps; // 进入快速降速，停止过程
|
||
StartVAxisRun(&vAxisCmd);
|
||
}
|
||
}
|
||
|
||
if (innstp != 0 || nstpsta != 1) // 一般停止条件函数
|
||
{
|
||
if (innstp == 0 && nstpsta != 1)
|
||
{
|
||
if (pCtrl->GetNormalStop != NULL)
|
||
{
|
||
err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->errInfo = err;
|
||
// printf("normal stop 2 in InterpolationMotion, err=0x%x\r\n", err);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (err != 0 || innstp != 0)
|
||
{
|
||
// 计算从当前速度降速到停止需要的位移
|
||
if (pCtrl->stopMode == STOP_MODE_TAB)
|
||
{
|
||
u32 curpps, curmvmt, decCount, resdcount;
|
||
curpps = GetVAxisPPS(vAxisId); // 当前速度
|
||
decCount = CalcDisplacement(curpps, stoppps, decppsg); // 计算以当前运行速度减速到停止速度需要的位移
|
||
curmvmt = GetVAxisCounter(vAxisId); // 当前表运行进度
|
||
// 运行状态
|
||
|
||
tmp16 = GetRunStatus();
|
||
vrsta = GetRunStatus();
|
||
while(tmp16 != vrsta)
|
||
{
|
||
// printf("4. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
|
||
tmp16 = vrsta;
|
||
vrsta = GetRunStatus();
|
||
}
|
||
// printf("cur vaxis counter=%d, vrsta=0x%x\r\n", curmvmt, vrsta);
|
||
resdcount = 0;
|
||
if (decCount > (curtab1num+curtab2num-curmvmt)) // 减速需要脉冲数大于已填充数据
|
||
{
|
||
innstp = 1; // 设置进入降速状态，但需要继续填充数据和计算减速点
|
||
}
|
||
else
|
||
{
|
||
// 判断当前运行的表是那个表
|
||
if ((vrsta & vAxisBuf1Running) != 0) // 表1正在执行
|
||
{
|
||
if (decCount < (curtab1num-curmvmt))
|
||
{
|
||
resdcount = (curtab1num-curmvmt);
|
||
SetVAxisConfig(vAxisDisBuf2); // 取消掉表2的已填充数据
|
||
}
|
||
}
|
||
else if ((vrsta & vAxisBuf2Running) != 0) // 表2正在执行
|
||
{
|
||
if (decCount < (curtab2num-curmvmt))
|
||
{
|
||
resdcount = (curtab2num-curmvmt);
|
||
SetVAxisConfig(vAxisDisBuf1); // 取消掉表1的已填充数据
|
||
}
|
||
}
|
||
|
||
if (resdcount == 0)
|
||
{
|
||
resdcount = (curtab1num+curtab2num-curmvmt);
|
||
}
|
||
|
||
decppsg = CalcGapTime(decCount*num, curpps, stoppps); // 按照新位移，重新计算刹车加速度
|
||
|
||
innstp = 0; // 不再进入
|
||
stopfill = 1; // 不再填充数据
|
||
}
|
||
}
|
||
|
||
// 立即降速
|
||
nstpsta = 1; // 设置停车状态
|
||
|
||
vAxisCmd.vaxisId = vAxisId;
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = decppsg;
|
||
vAxisCmd.runPPS = stoppps;
|
||
vAxisCmd.startPPS = stoppps; // 进入快速降速，停止过程
|
||
StartVAxisRun(&vAxisCmd);
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetSlowDown != NULL) // 降速
|
||
{
|
||
tmp16 = pCtrl->GetSlowDown(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (tmp16 != 0)
|
||
{
|
||
if (slowSta == 0) // 非降速状态
|
||
{
|
||
slowSta = 1; // 设置为降速状态
|
||
// printf("in slow down sta, slowpps=%d, tslppsg=%d\r\n", slowpps, tslppsg);
|
||
vAxisCmd.vaxisId = vAxisId; //使用虚轴
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = tslppsg;
|
||
vAxisCmd.runPPS = slowpps;
|
||
vAxisCmd.startPPS = slowpps;
|
||
StartVAxisRun(&vAxisCmd); // 处理降速
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (slowSta == 1) // 在降速状态
|
||
{
|
||
slowSta = 0;
|
||
// printf("out slow down sta\r\n");
|
||
StopSpdCtrl(vAxisId); // 恢复正常速度
|
||
}
|
||
}
|
||
}
|
||
|
||
// 刹车
|
||
if ((brksta == 1 || nstpsta == 1) && pCtrl->stopMode == STOP_MODE_SPD) // 停止刹车状态，且是速度低于停止频率立刻停车模式
|
||
{
|
||
val32 = GetVAxisPPS(vAxisId); // 读取当前虚轴的运行频率
|
||
if (val32 <= stoppps) // 频率小于停止频率，立刻停止
|
||
{
|
||
if (brksta == 1)
|
||
{
|
||
rslt = 3; // 快速停止条件触发停车，速度低于停车速度
|
||
}
|
||
else // if (nstpsta == 1)
|
||
{
|
||
rslt = 4; // 普通停车条件触发停车，速度低于停车速度
|
||
}
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
// printf(">stoppps\r\n");
|
||
}
|
||
}
|
||
|
||
// 更新坐标
|
||
if (GetMotosCounter(position) == 0)
|
||
{
|
||
RefreshMotosPos(position); // 更新各个轴的位置
|
||
if (pCtrl->RefreshPosition != NULL)
|
||
{
|
||
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = 5;
|
||
}
|
||
|
||
}while(0); // exerepeat == 0 && execcircle == 1);
|
||
|
||
if (rslt != 0)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
StopVAxisRun(vAxisId);
|
||
|
||
if (pCtrl->WorkAfterRun != NULL)
|
||
{
|
||
pCtrl->WorkAfterRun(pCtrl->workPara1, pCtrl->workPara2);
|
||
}
|
||
|
||
if (pCtrl->Delay != NULL)
|
||
{
|
||
// pCtrl->Delay(100);
|
||
}
|
||
|
||
// 更新坐标
|
||
if (GetMotosCounter(position) == 0)
|
||
{
|
||
RefreshMotosPos(position); // 更新各个轴的位置
|
||
|
||
if (pCtrl->RefreshPosition != NULL)
|
||
{
|
||
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = 5;
|
||
}
|
||
|
||
if (rslt == 0)
|
||
{
|
||
pCtrl->errInfo = 0;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
#endif
|
||
|
||
//------------------------------------------------------------------------------------------------------
|
||
|
||
|
||
//------------------------------------------------------------------------------------------------------
|
||
|
||
// 功能：多轴连续直线插补运动控制，最多支持6轴的运动。 支持虚轴1或虚轴2控制
|
||
// 参数：连续插补控制结构指针
|
||
// 结果: 0，运动执行完成。
|
||
// -1，输入参数错误
|
||
// -2，执行过程中填充数据不及时错误退出
|
||
// -3 FPGA 读写错误
|
||
// -4 FPGA 版本错误
|
||
// =1, 驱动器报警停车
|
||
// =2, 紧急停止条件触发停车
|
||
// =3, 快速停止条件触发停车
|
||
// =4, 普通停车条件触发停车
|
||
// =5,电机坐标获取错误
|
||
|
||
#if (1)
|
||
|
||
int ContinuousInterpolation(ContinuousInterMoveCtrl * pCtrl)
|
||
{
|
||
int i, j, rslt, err;
|
||
int bufsta, bufsel; // 缓冲区状态,缓冲区选择
|
||
u16 vrsta, tmp16; // 虚轴运行状态
|
||
u16 vAxisId;
|
||
u32 decPPSG, decCount;
|
||
|
||
u16 vAxisCleanOv1, vAxisCleanOv2;
|
||
u16 vAxisBuf1Finish, vAxisBuf2Finish; // 虚轴缓冲区1完成标志,虚轴缓冲区2完成标志
|
||
u16 vAxisBuf1Running, vAxisBuf2Running; // 虚轴缓冲区1正在执行标志,虚轴缓冲区2正在执行标志
|
||
// u16 vAxisDisBuf1, vAxisDisBuf2;
|
||
|
||
s32 position[AXIS_NUM];
|
||
|
||
VAxisCmdStr vAxisCmd;
|
||
InterData idata;
|
||
|
||
// ------------------------------------------------------------------------
|
||
// 参数合法性判断
|
||
if (pCtrl == NULL)
|
||
{
|
||
printf("para error in ContinuousInterpolation\r\n");
|
||
rslt = -1; // 输入参数错误
|
||
return rslt;
|
||
}
|
||
|
||
if (pCtrl->vAxisId != VAXIS_ID1)
|
||
{
|
||
printf("para vAxisId error in ContinuousInterpolation\r\n");
|
||
rslt = -1; // 输入参数错误
|
||
return rslt;
|
||
}
|
||
|
||
for (i = 0; i < AXIS_NUM; i++) // 统一实轴速度来源
|
||
{
|
||
pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
|
||
}
|
||
|
||
// ------------------------------------------------------------------------
|
||
|
||
if (pCtrl->GetEmergencyStop != NULL) // 紧急停止条件函数
|
||
{
|
||
err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->mvData.errInfo = err;
|
||
printf("emergency stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
|
||
rslt = 2; // 紧急停止条件触发停车，立刻停止脉冲发送
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetQuickStop != NULL) // 出错停止条件函数
|
||
{
|
||
err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->mvData.errInfo = err;
|
||
printf("quick stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
|
||
rslt = 3; // 快速停止条件触发停车
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetNormalStop != NULL) // 一般停止
|
||
{
|
||
err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->mvData.errInfo = err;
|
||
printf("normal stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
|
||
rslt = 4; // 普通停车条件触发停车
|
||
return rslt;
|
||
}
|
||
}
|
||
|
||
// 选择参与电机
|
||
SetMotosConfig(&(pCtrl->motosConfig));
|
||
|
||
// 等待驱动器执行
|
||
if (pCtrl->Delay != NULL && pCtrl->motosConfig.newConfig != 0)
|
||
{
|
||
pCtrl->Delay(100);
|
||
}
|
||
|
||
bufsta = 0;
|
||
bufsel = 0;
|
||
decCount = 0;
|
||
pCtrl->mvData.fillLimit = 0;
|
||
|
||
decPPSG = MIN_GAP;
|
||
|
||
pCtrl->mvData.errInfo = 0;
|
||
|
||
vAxisId = pCtrl->vAxisId;
|
||
|
||
if (vAxisId == VAXIS_ID1)
|
||
{
|
||
vAxisCleanOv1 = VCTR_VAXIS1_CLEAN_OV1;
|
||
vAxisCleanOv2 = VCTR_VAXIS1_CLEAN_OV2;
|
||
|
||
vAxisBuf1Finish = VAXIS1_BUF1_FINISH;
|
||
vAxisBuf2Finish = VAXIS1_BUF2_FINISH;
|
||
|
||
vAxisBuf1Running = VAXIS1_BUF1_RUNNING;
|
||
vAxisBuf2Running = VAXIS1_BUF2_RUNNING;
|
||
|
||
// vAxisDisBuf1 = VCTR_VAXIS1_DIS_BUF1;
|
||
// vAxisDisBuf2 = VCTR_VAXIS1_DIS_BUF2;
|
||
}
|
||
else
|
||
{
|
||
printf("not support vaxis\r\n");
|
||
}
|
||
|
||
if (pCtrl->WorkBeforeRun != NULL)
|
||
{
|
||
pCtrl->WorkBeforeRun(pCtrl->workPara1, pCtrl->workPara2);
|
||
}
|
||
|
||
SetVAxisConfig(VCTR_AXIS_ALM_INT); // 清除报警状态
|
||
memset(&idata, 0, sizeof(InterData));
|
||
|
||
do
|
||
{
|
||
pCtrl->mvData.bufsta = bufsta;
|
||
// 刷新数据
|
||
if (pCtrl->RefreshData != NULL)
|
||
{
|
||
rslt = pCtrl->RefreshData(pCtrl->refDatPara, (u32)(&(pCtrl->mvData)));
|
||
}
|
||
|
||
// 更新坐标
|
||
if (GetMotosCounter(position) == 0)
|
||
{
|
||
RefreshMotosPos(position); // 更新各个轴的位置
|
||
if (pCtrl->RefreshPosition != NULL)
|
||
{
|
||
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = 5;
|
||
break;
|
||
}
|
||
|
||
// 填充数据
|
||
if ((bufsta & 0x03) != 0x03 && pCtrl->mvData.mvmtStatus != 0) // 当Buff有空闲，并且还有数据时
|
||
{
|
||
if (pCtrl->mvData.runstat != CRST_BREAK || pCtrl->mvData.fillLimit > 0) // 非刹车过程 或 在刹车过程中且允许填充数据
|
||
{
|
||
if (pCtrl->mvData.fillLimit > 0)
|
||
{
|
||
pCtrl->mvData.fillLimit--;
|
||
}
|
||
|
||
// 写各个轴的插补数据
|
||
for (i = 0; i < AXIS_NUM; i++)
|
||
{
|
||
if (pCtrl->mvData.mvmtdat.segmnetNum[i] != 0) // 有效段数
|
||
{
|
||
idata.datBufLen = pCtrl->mvData.mvmtdat.segmnetNum[i]; // 有效段个数
|
||
// 计算该段插补需要的实轴数据
|
||
//--------------------------
|
||
idata.axisId = g_axisIDList[i]; // 实轴选择
|
||
idata.vaxisId = vAxisId; // 使用虚轴
|
||
if (bufsel == 0) // 缓冲区选择
|
||
{
|
||
idata.bufSel = BUF_SEL1; // 缓冲区1
|
||
}
|
||
else
|
||
{
|
||
idata.bufSel = BUF_SEL2; // 缓冲区2
|
||
}
|
||
idata.startEscNum = pCtrl->mvData.mvmtdat.startEsc[i]; // 起始跳过脉冲数
|
||
idata.partVxNum = pCtrl->mvData.mvmtdat.vppSegment[i]; // 每段对应的虚轴脉冲数
|
||
//---------------- 20220531 修改每段脉冲数限制
|
||
if ((idata.partVxNum * SUPPORT_SEGMENT) < pCtrl->mvData.mvmtdat.interLong)
|
||
{
|
||
idata.partVxNum = (pCtrl->mvData.mvmtdat.interLong+SUPPORT_SEGMENT-1) / SUPPORT_SEGMENT;
|
||
}
|
||
//----------------- 20220531 修改每段脉冲数限制
|
||
idata.dirAttr = AXIS_DIR_USEBUF; // 使用段内方向
|
||
|
||
// 填充多段数据
|
||
for (j = 0; j < pCtrl->mvData.mvmtdat.segmnetNum[i]; j++)
|
||
{
|
||
idata.datBuff[j] = pCtrl->mvData.mvmtdat.movement[i][j]; // 数据段
|
||
}
|
||
|
||
idata.datBuff[j] = 0;
|
||
idata.datBufLen += 1;
|
||
|
||
idata.sineOut = pCtrl->mvData.mvmtdat.sineOut[i]; // 正弦输出模式
|
||
|
||
//printf("HEX:%d.datBuff[0]=0x%x\r\n", i+1, idata.datBuff[0]);
|
||
//printf("DEC:%d.datBuff[0]=%d\r\n", i+1, idata.datBuff[0]);
|
||
|
||
rslt = WriteInterData(&idata);
|
||
if (rslt < 0)
|
||
{
|
||
return rslt;
|
||
}
|
||
//printf("axis %d wt, c=%d\r\n", i+1, idata.datBuff[0]);
|
||
}
|
||
else
|
||
{
|
||
if (bufsel == 0)
|
||
{
|
||
CleanAxisBuff(g_axisIDList[i], BUF_SEL1);
|
||
}
|
||
else
|
||
{
|
||
CleanAxisBuff(g_axisIDList[i], BUF_SEL2);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 启动运动, 虚轴命令
|
||
vAxisCmd.vaxisId = vAxisId; // 使用虚轴
|
||
if (bufsel == 0)
|
||
{
|
||
bufsta |= 0x01;
|
||
vAxisCmd.bufSel = BUF_SEL1;
|
||
// curtab1num = pCtrl->mvData.interLong;
|
||
// printf("write buff1\r\n");
|
||
}
|
||
else
|
||
{
|
||
bufsta |= 0x02;
|
||
vAxisCmd.bufSel = BUF_SEL2;
|
||
// curtab2num = pCtrl->mvData.interLong;
|
||
// printf("write buff2\r\n");
|
||
}
|
||
|
||
// printf("write add vtable\r\n");
|
||
vAxisCmd.startPPS = pCtrl->mvData.lowPPS; // 启动速度
|
||
vAxisCmd.runPPS = pCtrl->mvData.runPPS; // 运行速度
|
||
vAxisCmd.jumpNum = 0; // 跳过脉冲数
|
||
vAxisCmd.calcGap = pCtrl->mvData.calcGap; // 计算时间
|
||
vAxisCmd.outNum = pCtrl->mvData.mvmtdat.interLong; // 插补轴的脉冲个数
|
||
rslt = StartVAxisRun(&vAxisCmd); // 写运行命令
|
||
if (rslt < 0)
|
||
{
|
||
break;
|
||
}
|
||
bufsel = ~bufsel; // 切换buf
|
||
pCtrl->mvData.fillCount++; // 填充数据计数增加
|
||
// pCtrl->mvData.runIdx++; // 运行计数器
|
||
pCtrl->mvData.mvmtStatus = 0; // 设置数据执行标志
|
||
}
|
||
}
|
||
|
||
// 等待运动结束
|
||
rslt = 0;
|
||
err = 0;
|
||
|
||
{
|
||
// 得到虚轴运行状态
|
||
// 为了得到一个正确的运行状态
|
||
tmp16 = GetRunStatus();
|
||
vrsta = GetRunStatus();
|
||
while(tmp16 != vrsta)
|
||
{
|
||
// printf("5. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
|
||
tmp16 = vrsta;
|
||
vrsta = GetRunStatus();
|
||
}
|
||
tmp16 = GetRunStatus();
|
||
vrsta = GetRunStatus();
|
||
while(tmp16 != vrsta)
|
||
{
|
||
// printf("6. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
|
||
tmp16 = vrsta;
|
||
vrsta = GetRunStatus();
|
||
}
|
||
|
||
if ((vrsta & (vAxisBuf1Finish|vAxisBuf2Finish)) != 0) // 虚轴缓冲区1或2完成
|
||
{
|
||
if (pCtrl->mvData.runstat != CRST_NORMAL) // 处于进入停止状态或刹车状态时，执行完一个表后，重新计算加速度
|
||
{
|
||
s32 inmvmt;
|
||
// 调整减速速度
|
||
u32 curpps = GetVAxisPPS(vAxisId); // 当前速度
|
||
|
||
inmvmt = decCount;
|
||
// inmvmt -= 1;
|
||
if (inmvmt > 0)
|
||
{
|
||
inmvmt *= pCtrl->mvData.mvmtdat.interLong;
|
||
|
||
// 重新计算加速度
|
||
decPPSG = CalcGapTime(inmvmt, curpps, pCtrl->mvData.lowPPS);
|
||
|
||
// printf("pCtrl->mvData.runstat=%d, curpps=%d, mvmt=%d, lopps=%d, recalc decPPSG=%d\r\n", pCtrl->mvData.runstat, curpps, inmvmt, pCtrl->mvData.lowPPS, decPPSG);
|
||
|
||
vAxisCmd.vaxisId = vAxisId; // 使用虚轴
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = decPPSG;
|
||
vAxisCmd.runPPS = pCtrl->mvData.lowPPS;
|
||
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
|
||
rslt = StartVAxisRun(&vAxisCmd); // 减速
|
||
if (rslt < 0)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
if ((vrsta & vAxisBuf1Finish) != 0) // 虚轴缓冲区1完成标志
|
||
{
|
||
SetVAxisConfig(vAxisCleanOv1); // 清除表1完成标志
|
||
bufsta &= 0xFE; // 释放buf1
|
||
pCtrl->mvData.execCount++; // 已执行数据增加
|
||
pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);
|
||
|
||
pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount; // 正在执行数据的索引（另外一个表的数据） = 数据起始+跳过数据+已经执行数据
|
||
// printf("table 1 ok, datidx=%d, endpps=%d\r\n", iddx, pCtrl->mvData.lastEndPPS);
|
||
// printf("table 1 ok, exeingidx=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, pCtrl->mvData.lastEndPPS);
|
||
|
||
if (pCtrl->mvData.runstat == CRST_BREAK) // 进入减速后，第一次切换表开始减速
|
||
{
|
||
if (decCount != 0)
|
||
{
|
||
decCount--; // 减速执行圈数减少
|
||
}
|
||
// printf("in break, table 1 ok, exeingIdx=%d, decCount=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, decCount, pCtrl->mvData.lastEndPPS);
|
||
}
|
||
}
|
||
|
||
if ((vrsta & vAxisBuf2Finish) != 0) // 虚轴缓冲区2完成标志
|
||
{
|
||
SetVAxisConfig(vAxisCleanOv2); // 清除表2完成标志
|
||
bufsta &= 0xFD; // 释放buf2
|
||
pCtrl->mvData.execCount++; // 已执行数据增加
|
||
pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);
|
||
|
||
pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount; // 正在执行数据的索引（另外一个表的数据） = 数据起始+跳过数据+已经执行数据
|
||
// printf("table 2 ok, datidx=%d, endpps=%d\r\n", iddx, pCtrl->mvData.lastEndPPS);
|
||
// printf("table 2 ok, exeingidx=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, pCtrl->mvData.lastEndPPS);
|
||
|
||
if (pCtrl->mvData.runstat == CRST_BREAK) // 进入减速后，第一次切换表开始减速
|
||
{
|
||
if (decCount != 0)
|
||
{
|
||
decCount--; // 减速执行圈数减少
|
||
}
|
||
// printf("in break, table 2 ok, exeingIdx=%d, decCount=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, decCount, pCtrl->mvData.lastEndPPS);
|
||
}
|
||
}
|
||
}
|
||
|
||
if ((vrsta & AXIS_ALARM_FLAG) != 0) // 实轴报警标志
|
||
{
|
||
err = GetAlarmValue(); // 得到轴报警信息
|
||
SetVAxisConfig(VCTR_AXIS_ALM_INT); // 清除状态
|
||
if (err != 0)
|
||
{
|
||
printf("2. axis alarm value=0x%x\r\n", err);
|
||
}
|
||
rslt = 1; // 驱动器报警停车
|
||
break;
|
||
}
|
||
else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0) // 两个缓冲区的数据都执行完成，没有有效数据
|
||
{
|
||
if (pCtrl->mvData.waitNoData == 0)
|
||
{
|
||
rslt = 0; // 无有效数据，结束
|
||
// printf("no more data in two buff in ContinuousInterpolation, err=0x%x\r\n", pCtrl->mvData.errInfo);
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
// printf("wait when no data\r\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
if (pCtrl->GetEmergencyStop != NULL) // 急停条件函数
|
||
{
|
||
err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
pCtrl->mvData.errInfo = err;
|
||
printf("2. emergency stop 3, err=0x%x\r\n", err);
|
||
rslt = 2; // 紧急停止条件触发停车，立刻停止脉冲发送
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetQuickStop != NULL) // 快速停止条件函数
|
||
{
|
||
err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
if (err != 0)
|
||
{
|
||
// printf("quick stop, enter break status, rslt=0x%x\r\n", err);
|
||
pCtrl->mvData.errInfo = err;
|
||
|
||
// 设置当前速度为运行速度，保持速度不变
|
||
decPPSG = pCtrl->qstopPPSG;
|
||
|
||
vAxisCmd.vaxisId = vAxisId; // 使用虚轴
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = decPPSG;
|
||
vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
|
||
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
|
||
rslt = StartVAxisRun(&vAxisCmd);
|
||
if (rslt < 0)
|
||
{
|
||
break;
|
||
}
|
||
// 计算以当前运行速度减速到停止速度需要的位移
|
||
decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);
|
||
if (pCtrl->mvData.mvmtdat.interLong != 0)
|
||
{
|
||
decCount += pCtrl->mvData.mvmtdat.interLong-1;
|
||
decCount /= pCtrl->mvData.mvmtdat.interLong; // 计算得到减速需要的整圈数
|
||
}
|
||
else
|
||
{
|
||
printf("1. Err: interLong==0\r\n");
|
||
}
|
||
|
||
//printf("1. runpps=%d, lowPPS=%d, calcGap=%d, decCount=%d\r\n", vAxisCmd.runPPS, vAxisCmd.startPPS, vAxisCmd.calcGap, decCount);
|
||
|
||
pCtrl->mvData.fillLimit = decCount+pCtrl->mvData.execCount-pCtrl->mvData.fillCount; // 需要继续填充的数据计数
|
||
|
||
/* 20220406 ljs 填充限制+1
|
||
* 降速所需圈数decCount,求出的这个值是正确的
|
||
* pCtrl->mvData.execCount是执行完成计数值
|
||
* pCtrl->mvData.fillCount是填充计数值
|
||
* 代码执行到此处时,一定有一个表正在执行,且另外一个表已经被填充
|
||
* 正在执行的表是不能降速的,只有未执行的表是可以降速的
|
||
* 故上述填充限制计算得出的值会少填充一针数据
|
||
* 例如,此时正在执行第113针,已经填充了第114针,那么第113针不考虑降速,从第114针开始降速
|
||
* 若填充计数值为65,此时执行完成计数值为63(因为第113针尚未执行完成,故差值为2)
|
||
* 若求出需要4圈降速,那么第114,115,116,117针需要降速,且可以降速到目标速度
|
||
* 根据上述计算方式求出的填充限制值为2,则可继续填充执行第115,116针,则第117针虽然可以填充,
|
||
* 但因为此时填充限制值为0,故虽然填充了,但不执行
|
||
* 所以实际只有3针用于降速
|
||
* 这就是为什么降速不到目标速度的原因
|
||
*/
|
||
pCtrl->mvData.fillLimit += 1;
|
||
|
||
//printf("1. fill limit=%d, decCount=%d,fillCount=%d,exeCount=%d\r\n", pCtrl->mvData.fillLimit, decCount,pCtrl->mvData.fillCount,pCtrl->mvData.execCount);
|
||
if (pCtrl->mvData.fillLimit < 0)
|
||
{
|
||
// printf("1. no need to fill data\r\n");
|
||
pCtrl->mvData.fillLimit = 0;
|
||
}
|
||
pCtrl->mvData.runstat = CRST_QSTOP; // 设置停车状态
|
||
}
|
||
}
|
||
|
||
if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetNormalStop != NULL) // 一般停止
|
||
{
|
||
err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
|
||
|
||
if (err != 0)
|
||
{
|
||
// printf("normal stop, enter break status, rslt=0x%x\r\n", err);
|
||
pCtrl->mvData.errInfo = err;
|
||
|
||
// 设置当前速度为运行速度，保持速度不变
|
||
decPPSG = pCtrl->nstopPPSG;
|
||
|
||
vAxisCmd.vaxisId = vAxisId; // 使用虚轴
|
||
vAxisCmd.bufSel = BUF_SEL3;
|
||
vAxisCmd.calcGap = decPPSG;
|
||
vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
|
||
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
|
||
rslt = StartVAxisRun(&vAxisCmd);
|
||
if (rslt < 0)
|
||
{
|
||
break;
|
||
}
|
||
// 计算以当前运行速度减速到停止速度需要的位移
|
||
decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);
|
||
|
||
if (pCtrl->mvData.mvmtdat.interLong != 0)
|
||
{
|
||
decCount += pCtrl->mvData.mvmtdat.interLong-1;
|
||
decCount /= pCtrl->mvData.mvmtdat.interLong; // 计算得到减速需要的整圈数
|
||
}
|
||
else
|
||
{
|
||
printf("2. Err: interLong==0\r\n");
|
||
}
|
||
//printf("2. runpps=%d, lowPPS=%d, calcGap=%d, decCount=%d\r\n", vAxisCmd.runPPS, vAxisCmd.startPPS, vAxisCmd.calcGap, decCount);
|
||
|
||
pCtrl->mvData.fillLimit = decCount+pCtrl->mvData.execCount-pCtrl->mvData.fillCount; // 需要继续填充的数据计数
|
||
|
||
// 20220406 ljs 填充限制+1
|
||
pCtrl->mvData.fillLimit += 1;
|
||
|
||
//printf("2. fill limit=%d, decCount=%d,fillCount=%d,exeCount=%d\r\n", pCtrl->mvData.fillLimit, decCount,pCtrl->mvData.fillCount,pCtrl->mvData.execCount);
|
||
if (pCtrl->mvData.fillLimit < 0)
|
||
{
|
||
// printf("1. no need to fill data\r\n");
|
||
pCtrl->mvData.fillLimit = 0;
|
||
}
|
||
pCtrl->mvData.runstat = CRST_NSTOP; // 设置停车状态
|
||
}
|
||
}
|
||
|
||
}while(1);
|
||
|
||
StopVAxisRun(vAxisId);
|
||
|
||
if (pCtrl->WorkAfterRun != NULL)
|
||
{
|
||
pCtrl->WorkAfterRun(pCtrl->workPara1, pCtrl->workPara2);
|
||
}
|
||
|
||
|
||
// 更新坐标
|
||
if (GetMotosCounter(position) == 0)
|
||
{
|
||
RefreshMotosPos(position); // 更新各个轴的位置
|
||
if (pCtrl->RefreshPosition != NULL)
|
||
{
|
||
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = 5;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|