yangmeng/optical
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
9e0e6c5fb6
optical/EMBOS/Users/EmbBase/motos.c

1820 lines
44 KiB
C
Raw Normal View History

调试完成
2025-09-04 01:45:08 +00:00
#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,
};
//-------------------------------------------------------------------------------
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)); // <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
int GetMotosCounter(s32 * position)
{
int i;
if (position != NULL)
{
for (i = 0; i < AXIS_NUM; i++)
{
position[i] = GetAxisCounter(g_axisIDList[i]);
}
}
return 0;
}
// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>λ<EFBFBD><CEBB>
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);
}
}
}
// ˢ<><CBA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void RefreshMotosPos(s32 * poslist)
{
// ȫ<><C8AB>ˢ<EFBFBD><CBA2>
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);
}
}
// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void SetMotosConfig(MotosConfig * pConfig)
{
int i;
u16 axisID;
if (pConfig == NULL)
{
return;
}
if (pConfig->newConfig == 0)
{
return;
}
// ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
StopVAxisRun(VAXIS_ID_ALL);
for (i = 0; i < AXIS_NUM; i++)
{
axisID = g_axisIDList[i];
if ((pConfig->axisConfig[i].axisConfig & CONFIG_EN) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if ((pConfig->axisConfig[i].axisConfig & POA_EN) != 0) // A<><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, POUTA_EN);
}
else
{
SetAxisConfig(axisID, POUTA_DIS);
}
if ((pConfig->axisConfig[i].axisConfig & POB_EN) != 0) // B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, POUTB_EN);
}
else
{
SetAxisConfig(axisID, POUTB_DIS);
}
if ((pConfig->axisConfig[i].axisConfig & SON_EN) != 0) // <20>ŷ<EFBFBD>ON<4F><4E><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, SERVO_ON);
}
else
{
SetAxisConfig(axisID, SERVO_OFF);
}
if ((pConfig->axisConfig[i].axisConfig & ALM_LEVH) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽΪ<C6BD><CEAA>
{
SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
}
else
{
SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
}
if ((pConfig->axisConfig[i].axisConfig & ALM_EN) != 0) // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisAlarmConfig(axisID, ALM_STOP_EN);
}
else
{
SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
}
if ((pConfig->axisConfig[i].axisConfig & POUT_EN) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, pConfig->axisConfig[i].poutType); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ <20><><EFBFBD>巽ʽ<E5B7BD><CABD><EFBFBD><EFBFBD>
SetAxisConfig(axisID, pConfig->axisConfig[i].spdSource); // ʵ<><CAB5><EFBFBD>ٶȿ<D9B6><C8BF><EFBFBD>ģʽ(<28><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
SetAxisConfig(axisID, pConfig->axisConfig[i].datSource); // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD>ݻ<EFBFBD>ȡģʽ(Ӳ<><D3B2><EFBFBD>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>涯<EFBFBD><E6B6AF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3>)
}
else
{
SetAxisConfig(axisID, POUT_DISABLE); // <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
SetVAxisConfig(VCTR_AXIS_ALM_INT); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
}
//-------------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>Ӽ<EFBFBD><D3BC><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
// <20><><EFBFBD><EFBFBD>: pps1 <20>ٶ<EFBFBD>1
// pps2 <20>ٶ<EFBFBD>2
// calcTime <20>ٶȱ仯ʱ<E4BBAF>䣬<EFBFBD><E4A3AC>λΪ1/72us
// <20><><EFBFBD><EFBFBD>:
// λ<><CEBB>ֵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
u32 CalcDisplacement(u32 pps1, u32 pps2, u32 calcTime)
{
// <20>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
// <20><>ʽ: d=(v2+v1)*(v2-v1)/(2a)
// v1 = pps1
// v2 = pps2
// a = 1ppg/(calcTime/FPGA_CLK)
// <09><><EFBFBD><EFBFBD> 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);
}
// <20><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ijλ<C4B3><CEBB><EFBFBD><EFBFBD><EFBFBD>ٶȱ仯<C8B1><E4BBAF>Ҫ<EFBFBD><D2AA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λΪ1/72us<75><73>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// disp λ<><CEBB>
// pps1 <20>ٶ<EFBFBD>1
// pps2 <20>ٶ<EFBFBD>2
//
// <20><><EFBFBD><EFBFBD>:
// λ<><CEBB>ֵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
u32 CalcGapTime(u32 disp, u32 pps1, u32 pps2)
{
double temp;
// <20>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
// <20><>ʽ: a=(v2+v1)*(v2-v1)/(2*d)
// v1 = pps1
// v2 = pps2
// d = disp
// a = 1ppg/(calcTime/FPGA_CLK)
// <09><><EFBFBD><EFBFBD> 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);
}
// <20><><EFBFBD><EFBFBD>ijλ<C4B3>ƺ<EFBFBD>ij<EFBFBD><C4B3><EFBFBD>ٶ<EFBFBD><D9B6>£<EFBFBD><C2A3><EFBFBD><EFBFBD>յ<EFBFBD><D5B5>ٶ<EFBFBD>ֵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// disp λ<><CEBB>
// pps1 <20>ٶ<EFBFBD>1
// addgap ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//
// <20><><EFBFBD><EFBFBD>:
// λ<><CEBB>ֵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
u32 CalcEndSpd(u32 disp, u32 pps1, s32 addgap)
{
double temp;
// <20>ٶȱ仯<C8B1><E4BBAF><EFBFBD>̶<EFBFBD>Ϊ1ppg
// <20><>ʽ: a=(v2+v1)*(v2-v1)/(2*d)
// v1 = pps1
// d = disp
// a = 1ppg/(addgap/FPGA_CLK)
// <09><><EFBFBD><EFBFBD> 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);
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// <20><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߲岹<DFB2>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>(Ӳ<><D3B2>ʵ<EFBFBD><CAB5>)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>6<EFBFBD><36>ֱ<EFBFBD>߲岹<DFB2><E5B2B9>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>岹<EFBFBD><E5B2B9><EFBFBD>ƽṹָ<E1B9B9><D6B8>
// <20><><EFBFBD><EFBFBD>: 0<><30><EFBFBD>˶<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɡ<EFBFBD>
// -1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// -2<><32>ִ<EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD>
// -3 fpga <20><>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
// -4 FPGA <20>汾<EFBFBD><E6B1BE><EFBFBD><EFBFBD>
// =1, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =2, <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =3, <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =4, <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
#if (1)
int InterpolationMotion(InterMoveCtrl * pCtrl)
{
int i, rslt;
u32 totalnum, circlenum, circlesize, lastsize; // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3><EFBFBD>
u32 runcircle, execcircle, runrepeat, exerepeat; // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
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]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
int runsta, bufsta, bufsel;
int slowSta;
int brksta, inbrk; // ɲ<><C9B2>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>־
int nstpsta, innstp; // ɲ<><C9B2>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>־
int stopfill; // ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD>־
u16 tmp16, vrsta, err;
double calc, len;
u16 outNum;
u32 startpps, runpps, slowpps, stoppps; // <20>ٶ<EFBFBD>
u32 addppsg, decppsg, brkppsg, tslppsg; // <20><><EFBFBD>ٶ<EFBFBD>
u32 num, addnum, decnum, jumpnum; // <20>Ӽ<EFBFBD><D3BC><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
u32 curtab1num, curtab2num;
VAxisCmdStr vAxisCmd;
InterData idata;
num = 0;
// ------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD>ж<EFBFBD>
if (pCtrl == NULL)
{
printf("para error in MotosRun\r\n");
rslt = -1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return rslt;
}
if (pCtrl->vAxisId != VAXIS_ID1)
{
printf("para vAxisId error in MotosRun\r\n");
rslt = -1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return rslt;
}
for (i = 0; i < AXIS_NUM; i++) // ͳһʵ<D2BB><CAB5><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>Դ
{
pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
}
// ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SetMotosConfig(&(pCtrl->motosConfig));
// <20>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
if (pCtrl->Delay != NULL && pCtrl->motosConfig.newConfig != 0)
{
pCtrl->Delay(100);
}
// ------------------------------------------------------------------------
if (pCtrl->GetEmergencyStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD>巢<EFBFBD><E5B7A2>
return rslt;
}
}
if (pCtrl->GetQuickStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
return rslt;
}
}
if (pCtrl->GetNormalStop != NULL) // һ<><D2BB>ֹͣ
{
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; // <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
return rslt;
}
}
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
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");
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
totalnum = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
circlenum = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
circlesize = 0; // ÿ<><C3BF>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>
lastsize = 0; // <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>
runrepeat = pCtrl->mvmtPara.extraRepeat;
exerepeat = pCtrl->mvmtPara.extraRepeat;
// <20>õ<EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD>Ϊ<EFBFBD><EFBFBD><EEB3A4>
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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĺϳɳ<CFB3><C9B3><EFBFBD>
{
calc = movement[i];
calc *= calc;
len += calc;
}
else
{
}
*/
count[i] = 0;
}
totalnum = pCtrl->mvmtPara.interLong;
pulsePerSegment = pCtrl->mvmtPara.pulsePerSegment;
/*
if (pCtrl->interLong == LAS_COMPOSITE) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĺϳɳ<CFB3><C9B3><EFBFBD>
{
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) // С<><D0A1>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λͼ<CEBB><CDBC><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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) // <20><><EFBFBD><EFBFBD>ֹͣģʽ<C4A3>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
{
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;
}
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
addnum = CalcDisplacement(startpps, runpps, addppsg); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
decnum = CalcDisplacement(stoppps, runpps, decppsg); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 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 )
{
// <20><><EFBFBD>¼<EFBFBD><C2BC><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
calc = addnum;
calc /= (addnum + decnum);
calc *= totalnum; // <20><><EFBFBD>ձ<EFBFBD><D5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD>
calc /= pulsePerSegment; // <20><><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
addnum = (u32)calc;
if (addnum != 0)
{
addnum *= pulsePerSegment; // ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
{
addnum = totalnum / 2;
}
len = addnum*2;
len *= FPGA_CLK;
len /= addppsg;
len -= startpps;
calc = startpps;
calc *= startpps;
len += calc;
calc = sqrt(len); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
runpps = (u32)(calc+0.5);
decnum = CalcDisplacement(stoppps, runpps, decppsg); // <20><><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// printf("recalc addnum=%d, runpps=%d, decnum=%d\r\n", addnum, runpps, decnum);
}
// <20><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD>
runsta = 0;
bufsel = 0;
bufsta = 0;
slowSta = 0;
brksta = 0;
inbrk = 0;
nstpsta = 0;
innstp = 0;
stopfill = 0; // ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD>־
runcircle = circlenum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
execcircle = circlenum; // ִ<>м<EFBFBD><D0BC><EFBFBD>
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); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
memset(&idata, 0, sizeof(InterData));
while(execcircle > 0 || exerepeat > 0) // ִ<><D6B4>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
{
if ( stopfill == 0 && // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
(bufsta & 0x03) != 0x03 && // <20><>buff<66><66><EFBFBD><EFBFBD>
(runcircle != 0 || runrepeat != 0) && // <20><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
1 )
{
if (runcircle == 1) // <20><><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD>
{
num = lastsize;
if (runrepeat == 0)
{
runsta = 1; // <20><><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD>
}
}
else
{
num = circlesize;
if (runrepeat == 0 && (runsta == 0 && decnum > lastsize)) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һȦ<D2BB><C8A6>λ<EFBFBD><CEBB>
{
if ((runcircle-1) <= (decnum-lastsize+circlesize-1)/circlesize)
{
runsta = 1; // <20><><EFBFBD>ٱ<EFBFBD>־
}
}
}
runcount += num;
if (runsta == 1) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵ<EFBFBD>
{
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++) // д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ岹<C4B2><E5B2B9><EFBFBD><EFBFBD>
{
if (movement[i] != 0)
{
if (bufsel == 0)
{
idata.bufSel = BUF_SEL1;
}
else
{
idata.bufSel = BUF_SEL2;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>öβ岹<CEB2><E5B2B9>Ҫ<EFBFBD><D2AA>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//--------------------------
idata.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
if (dir[i] >= 0) // <20><><EFBFBD><EFBFBD>
{
idata.dirAttr = DIR_POSPOS; // <20><><EFBFBD><EFBFBD>
}
else
{
idata.dirAttr = DIR_NEGNEG; // <20><><EFBFBD><EFBFBD>
}
idata.datBufLen = 1+1; // <20><>Ч<EFBFBD>θ<EFBFBD><CEB8><EFBFBD>: 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; // <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
idata.datBuff[1] = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д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;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.runPPS = runpps; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.jumpNum = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.calcGap = addppsg; // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
}
else
{
// printf("write dec vtable, jump=%d\r\n", jumpnum);
vAxisCmd.startPPS = stoppps; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.runPPS = stoppps; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.jumpNum = jumpnum; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.calcGap = decppsg; // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
}
vAxisCmd.outNum = num; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
StartVAxisRun(&vAxisCmd); // д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
// printf("At%d, StartVAxisRun, vrsta=0x%x, vrsta1=0x%x\r\n", Get100UsSoftTimer(), GetRunStatus(), GetRunStatus1());
runcircle--; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С
if (runcircle == 0)
{
if (runrepeat != 0)
{
runrepeat--;
runcircle = circlenum;
}
}
bufsel = ~bufsel; // <20>л<EFBFBD>buf
}
// <20>ȴ<EFBFBD><C8B4>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
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; // <20><><EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD>ٶ<EFBFBD>
//----------
{
int count = 100;
do
{
// <20>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
tmp16 = GetRunStatus();
vrsta = GetRunStatus();
// printf("1. run sta =0x%x\r\n", vrsta);
while(tmp16 != vrsta) // <20>ȶ<EFBFBD><C8B6><EFBFBD>ֵ
{
// 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) ) // <20><><EFBFBD>ɱ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬʱ<CDAC><CAB1>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>ʱ<EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD>Ƕ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
{
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 <20><>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
break;
}
if ((vrsta & vAxisBuf1Finish) != 0) // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɱ<EFBFBD>־
{
SetVAxisConfig(vAxisCleanOv1); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ɱ<EFBFBD>־
bufsta &= 0xFE; // <20>ͷ<EFBFBD>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) // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɱ<EFBFBD>־
{
SetVAxisConfig(vAxisCleanOv2); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ɱ<EFBFBD>־
bufsta &= 0xFD; // <20>ͷ<EFBFBD>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) // ʵ<>ᱨ<EFBFBD><E1B1A8><EFBFBD><EFBFBD>־
{
err = GetAlarmValue(); // <20>õ<EFBFBD><C3B5>ᱨ<EFBFBD><E1B1A8><EFBFBD><EFBFBD>Ϣ
SetVAxisConfig(VCTR_AXIS_ALM_INT); // <20><><EFBFBD><EFBFBD>״̬
StopVAxisRun(vAxisId);
if (err != 0)
{
printf("1. axis alarm value=0x%x\r\n", err);
}
rslt = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
break;
}
else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɣ<EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
{
#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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else // if (nstpsta == 1)
{
rslt = 4; // <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
else
{
// printf("stop when spd low, execcircle=%d,exerepeat=%d\r\n", execcircle, exerepeat);
if (execcircle == 0 && exerepeat == 0)
{
/*
* 20221124
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><EFBFBD>
*/
rslt = 0;
}
else
{
rslt = -2; // <20><><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ݣ<EFBFBD><DDA3>˳<EFBFBD>
}
}
break;
}
}
}
// ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (pCtrl->GetEmergencyStop != NULL) // <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD>巢<EFBFBD><E5B7A2>
break;
}
}
if (inbrk != 0 || (brksta != 1 && pCtrl->GetQuickStop != NULL)) // <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>ǰ<EFBFBD>ٶȽ<D9B6><C8BD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
if (pCtrl->stopMode == STOP_MODE_TAB)
{
u32 curpps, curmvmt, decCount, resdcount;
curpps = GetVAxisPPS(vAxisId); // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
decCount = CalcDisplacement(curpps, stoppps, brkppsg); // <20><><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC>ٵ<EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD><D9B6><EFBFBD>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
curmvmt = GetVAxisCounter(vAxisId); // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><D0BD><EFBFBD>
// <20><><EFBFBD><EFBFBD>״̬
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)) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
inbrk = 1; // <20><><EFBFBD>ý<EFBFBD><C3BD>뽵<EFBFBD><EBBDB5>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺͼ<DDBA><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵ<EFBFBD>
}
else
{
// <20>жϵ<D0B6>ǰ<EFBFBD><C7B0><EFBFBD>еı<D0B5><C4B1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>
if ((vrsta & vAxisBuf1Running) != 0) // <20><>1<EFBFBD><31><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
{
if (decCount < (curtab1num-curmvmt))
{
resdcount = (curtab1num-curmvmt);
SetVAxisConfig(vAxisDisBuf2); // ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
else if ((vrsta & vAxisBuf2Running) != 0) // <20><>2<EFBFBD><32><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
{
if (decCount < (curtab2num-curmvmt))
{
resdcount = (curtab2num-curmvmt);
SetVAxisConfig(vAxisDisBuf1); // ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
if (resdcount == 0)
{
resdcount = (curtab1num+curtab2num-curmvmt);
}
brkppsg = CalcGapTime(decCount*num, curpps, stoppps); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
inbrk = 0; // <20><><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD>
stopfill = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
brksta = 1; // <20><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>״̬
vAxisCmd.vaxisId = vAxisId;
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = brkppsg;
vAxisCmd.runPPS = stoppps;
vAxisCmd.startPPS = stoppps; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD>٣<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
StartVAxisRun(&vAxisCmd);
}
}
if (innstp != 0 || nstpsta != 1) // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>ǰ<EFBFBD>ٶȽ<D9B6><C8BD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
if (pCtrl->stopMode == STOP_MODE_TAB)
{
u32 curpps, curmvmt, decCount, resdcount;
curpps = GetVAxisPPS(vAxisId); // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
decCount = CalcDisplacement(curpps, stoppps, decppsg); // <20><><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC>ٵ<EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD><D9B6><EFBFBD>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
curmvmt = GetVAxisCounter(vAxisId); // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><D0BD><EFBFBD>
// <20><><EFBFBD><EFBFBD>״̬
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)) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
innstp = 1; // <20><><EFBFBD>ý<EFBFBD><C3BD>뽵<EFBFBD><EBBDB5>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺͼ<DDBA><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵ<EFBFBD>
}
else
{
// <20>жϵ<D0B6>ǰ<EFBFBD><C7B0><EFBFBD>еı<D0B5><C4B1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>
if ((vrsta & vAxisBuf1Running) != 0) // <20><>1<EFBFBD><31><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
{
if (decCount < (curtab1num-curmvmt))
{
resdcount = (curtab1num-curmvmt);
SetVAxisConfig(vAxisDisBuf2); // ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
else if ((vrsta & vAxisBuf2Running) != 0) // <20><>2<EFBFBD><32><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
{
if (decCount < (curtab2num-curmvmt))
{
resdcount = (curtab2num-curmvmt);
SetVAxisConfig(vAxisDisBuf1); // ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
if (resdcount == 0)
{
resdcount = (curtab1num+curtab2num-curmvmt);
}
decppsg = CalcGapTime(decCount*num, curpps, stoppps); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
innstp = 0; // <20><><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD>
stopfill = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
nstpsta = 1; // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>״̬
vAxisCmd.vaxisId = vAxisId;
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = decppsg;
vAxisCmd.runPPS = stoppps;
vAxisCmd.startPPS = stoppps; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD>٣<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
StartVAxisRun(&vAxisCmd);
}
}
if (pCtrl->GetSlowDown != NULL) // <20><><EFBFBD><EFBFBD>
{
tmp16 = pCtrl->GetSlowDown(pCtrl->condPara1, pCtrl->condPara2);
if (tmp16 != 0)
{
if (slowSta == 0) // <20>ǽ<EFBFBD><C7BD><EFBFBD>״̬
{
slowSta = 1; // <20><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>״̬
// printf("in slow down sta, slowpps=%d, tslppsg=%d\r\n", slowpps, tslppsg);
vAxisCmd.vaxisId = vAxisId; //ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = tslppsg;
vAxisCmd.runPPS = slowpps;
vAxisCmd.startPPS = slowpps;
StartVAxisRun(&vAxisCmd); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
else
{
if (slowSta == 1) // <20>ڽ<EFBFBD><DABD><EFBFBD>״̬
{
slowSta = 0;
// printf("out slow down sta\r\n");
StopSpdCtrl(vAxisId); // <20>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}
}
}
// ɲ<><C9B2>
if ((brksta == 1 || nstpsta == 1) && pCtrl->stopMode == STOP_MODE_SPD) // ֹͣɲ<D6B9><C9B2>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵ<D9B6><C8B5><EFBFBD>ֹͣƵ<D6B9><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>ģʽ
{
val32 = GetVAxisPPS(vAxisId); // <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
if (val32 <= stoppps) // Ƶ<><C6B5>С<EFBFBD><D0A1>ֹͣƵ<D6B9>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD><EFBFBD>ֹͣ
{
if (brksta == 1)
{
rslt = 3; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD>ٶȵ<D9B6><C8B5><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
}
else // if (nstpsta == 1)
{
rslt = 4; // <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD>ٶȵ<D9B6><C8B5><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
}
break;
}
else
{
// printf(">stoppps\r\n");
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GetMotosCounter(position) == 0)
{
RefreshMotosPos(position); // <20><><EFBFBD>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
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);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GetMotosCounter(position) == 0)
{
RefreshMotosPos(position); // <20><><EFBFBD>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
if (pCtrl->RefreshPosition != NULL)
{
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
}
}
else
{
rslt = 5;
}
if (rslt == 0)
{
pCtrl->errInfo = 0;
}
return rslt;
}
#endif
//------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------
// <20><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߲岹<DFB2>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>6<EFBFBD><36><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD> ֧<><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>岹<EFBFBD><E5B2B9><EFBFBD>ƽṹָ<E1B9B9><D6B8>
// <20><><EFBFBD><EFBFBD>: 0<><30><EFBFBD>˶<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɡ<EFBFBD>
// -1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// -2<><32>ִ<EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD>
// -3 FPGA <20><>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
// -4 FPGA <20>汾<EFBFBD><E6B1BE><EFBFBD><EFBFBD>
// =1, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =2, <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =3, <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =4, <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// =5,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
#if (1)
int ContinuousInterpolation(ContinuousInterMoveCtrl * pCtrl)
{
int i, j, rslt, err;
int bufsta, bufsel; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1>
u16 vrsta, tmp16; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
u16 vAxisId;
u32 decPPSG, decCount;
u16 vAxisCleanOv1, vAxisCleanOv2;
u16 vAxisBuf1Finish, vAxisBuf2Finish; // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ɱ<EFBFBD>־,<2C><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ɱ<EFBFBD>־
u16 vAxisBuf1Running, vAxisBuf2Running; // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD>ִ<EFBFBD>б<EFBFBD>־,<2C><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>ִ<EFBFBD>б<EFBFBD>־
// u16 vAxisDisBuf1, vAxisDisBuf2;
s32 position[AXIS_NUM];
VAxisCmdStr vAxisCmd;
InterData idata;
// ------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD>ж<EFBFBD>
if (pCtrl == NULL)
{
printf("para error in ContinuousInterpolation\r\n");
rslt = -1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return rslt;
}
if (pCtrl->vAxisId != VAXIS_ID1)
{
printf("para vAxisId error in ContinuousInterpolation\r\n");
rslt = -1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return rslt;
}
for (i = 0; i < AXIS_NUM; i++) // ͳһʵ<D2BB><CAB5><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>Դ
{
pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
}
// ------------------------------------------------------------------------
if (pCtrl->GetEmergencyStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD>巢<EFBFBD><E5B7A2>
return rslt;
}
}
if (pCtrl->GetQuickStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
return rslt;
}
}
if (pCtrl->GetNormalStop != NULL) // һ<><D2BB>ֹͣ
{
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; // <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
return rslt;
}
}
// ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SetMotosConfig(&(pCtrl->motosConfig));
// <20>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>
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); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
memset(&idata, 0, sizeof(InterData));
do
{
pCtrl->mvData.bufsta = bufsta;
// ˢ<><CBA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (pCtrl->RefreshData != NULL)
{
rslt = pCtrl->RefreshData(pCtrl->refDatPara, (u32)(&(pCtrl->mvData)));
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GetMotosCounter(position) == 0)
{
RefreshMotosPos(position); // <20><><EFBFBD>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
if (pCtrl->RefreshPosition != NULL)
{
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
}
}
else
{
rslt = 5;
break;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((bufsta & 0x03) != 0x03 && pCtrl->mvData.mvmtStatus != 0) // <20><>Buff<66>п<EFBFBD><D0BF>У<EFBFBD><D0A3><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
{
if (pCtrl->mvData.runstat != CRST_BREAK || pCtrl->mvData.fillLimit > 0) // <20><>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if (pCtrl->mvData.fillLimit > 0)
{
pCtrl->mvData.fillLimit--;
}
// д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ岹<C4B2><E5B2B9><EFBFBD><EFBFBD>
for (i = 0; i < AXIS_NUM; i++)
{
if (pCtrl->mvData.mvmtdat.segmnetNum[i] != 0) // <20><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
{
idata.datBufLen = pCtrl->mvData.mvmtdat.segmnetNum[i]; // <20><>Ч<EFBFBD>θ<EFBFBD><CEB8><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD>öβ岹<CEB2><E5B2B9>Ҫ<EFBFBD><D2AA>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//--------------------------
idata.axisId = g_axisIDList[i]; // ʵ<><CAB5>ѡ<EFBFBD><D1A1>
idata.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (bufsel == 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1>
{
idata.bufSel = BUF_SEL1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
}
else
{
idata.bufSel = BUF_SEL2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2
}
idata.startEscNum = pCtrl->mvData.mvmtdat.startEsc[i]; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
idata.partVxNum = pCtrl->mvData.mvmtdat.vppSegment[i]; // ÿ<>ζ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//---------------- 20220531 <20>޸<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((idata.partVxNum * SUPPORT_SEGMENT) < pCtrl->mvData.mvmtdat.interLong)
{
idata.partVxNum = (pCtrl->mvData.mvmtdat.interLong+SUPPORT_SEGMENT-1) / SUPPORT_SEGMENT;
}
//----------------- 20220531 <20>޸<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
idata.dirAttr = AXIS_DIR_USEBUF; // ʹ<>ö<EFBFBD><C3B6>ڷ<EFBFBD><DAB7><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (j = 0; j < pCtrl->mvData.mvmtdat.segmnetNum[i]; j++)
{
idata.datBuff[j] = pCtrl->mvData.mvmtdat.movement[i][j]; // <20><><EFBFBD>ݶ<EFBFBD>
}
idata.datBuff[j] = 0;
idata.datBufLen += 1;
idata.sineOut = pCtrl->mvData.mvmtdat.sineOut[i]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
//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);
}
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.runPPS = pCtrl->mvData.runPPS; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
vAxisCmd.jumpNum = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.calcGap = pCtrl->mvData.calcGap; // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
vAxisCmd.outNum = pCtrl->mvData.mvmtdat.interLong; // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rslt = StartVAxisRun(&vAxisCmd); // д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (rslt < 0)
{
break;
}
bufsel = ~bufsel; // <20>л<EFBFBD>buf
pCtrl->mvData.fillCount++; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// pCtrl->mvData.runIdx++; // <20><><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>
pCtrl->mvData.mvmtStatus = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>б<EFBFBD>־
}
}
// <20>ȴ<EFBFBD><C8B4>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
rslt = 0;
err = 0;
{
// <20>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
// Ϊ<>˵õ<CBB5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
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) // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>1<EFBFBD><31>2<EFBFBD><32><EFBFBD><EFBFBD>
{
if (pCtrl->mvData.runstat != CRST_NORMAL) // <20><><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD>ֹͣ״̬<D7B4><CCAC>ɲ<EFBFBD><C9B2>״̬ʱ<CCAC><CAB1>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
{
s32 inmvmt;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
u32 curpps = GetVAxisPPS(vAxisId); // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
inmvmt = decCount;
// inmvmt -= 1;
if (inmvmt > 0)
{
inmvmt *= pCtrl->mvData.mvmtdat.interLong;
// <20><><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
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; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = decPPSG;
vAxisCmd.runPPS = pCtrl->mvData.lowPPS;
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
rslt = StartVAxisRun(&vAxisCmd); // <20><><EFBFBD><EFBFBD>
if (rslt < 0)
{
break;
}
}
}
if ((vrsta & vAxisBuf1Finish) != 0) // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ɱ<EFBFBD>־
{
SetVAxisConfig(vAxisCleanOv1); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ɱ<EFBFBD>־
bufsta &= 0xFE; // <20>ͷ<EFBFBD>buf1
pCtrl->mvData.execCount++; // <20><>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);
pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD> = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ+<2B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B>Ѿ<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٺ󣬵<D9BA>һ<EFBFBD><D2BB><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
{
if (decCount != 0)
{
decCount--; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
// 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) // <20><><EFBFBD>Ỻ<EFBFBD><E1BBBA><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ɱ<EFBFBD>־
{
SetVAxisConfig(vAxisCleanOv2); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ɱ<EFBFBD>־
bufsta &= 0xFD; // <20>ͷ<EFBFBD>buf2
pCtrl->mvData.execCount++; // <20><>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);
pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD> = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ+<2B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B>Ѿ<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٺ󣬵<D9BA>һ<EFBFBD><D2BB><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
{
if (decCount != 0)
{
decCount--; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
// 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) // ʵ<>ᱨ<EFBFBD><E1B1A8><EFBFBD><EFBFBD>־
{
err = GetAlarmValue(); // <20>õ<EFBFBD><C3B5>ᱨ<EFBFBD><E1B1A8><EFBFBD><EFBFBD>Ϣ
SetVAxisConfig(VCTR_AXIS_ALM_INT); // <20><><EFBFBD><EFBFBD>״̬
if (err != 0)
{
printf("2. axis alarm value=0x%x\r\n", err);
}
rslt = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
break;
}
else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>ɣ<EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
{
if (pCtrl->mvData.waitNoData == 0)
{
rslt = 0; // <20><><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD>
// 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) // <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD>巢<EFBFBD><E5B7A2>
break;
}
}
if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetQuickStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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;
// <20><><EFBFBD>õ<EFBFBD>ǰ<EFBFBD>ٶ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
decPPSG = pCtrl->qstopPPSG;
vAxisCmd.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = decPPSG;
vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
rslt = StartVAxisRun(&vAxisCmd);
if (rslt < 0)
{
break;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC>ٵ<EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD><D9B6><EFBFBD>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);
if (pCtrl->mvData.mvmtdat.interLong != 0)
{
decCount += pCtrl->mvData.mvmtdat.interLong-1;
decCount /= pCtrl->mvData.mvmtdat.interLong; // <20><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
}
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; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD>
/* 20220406 ljs <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+1
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><EFBFBD>decCount,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><EFBFBD>
* pCtrl->mvData.execCount<EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><EFBFBD><EFBFBD>ֵ
* pCtrl->mvData.fillCount<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>е<EFBFBD><EFBFBD>˴<EFBFBD>ʱ,һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>еı<EFBFBD><EFBFBD>Dz<EFBFBD><EFBFBD>ܽ<EFBFBD><EFBFBD>ٵ<EFBFBD>,ֻ<EFBFBD><EFBFBD>δִ<EFBFBD>еı<EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD>Խ<EFBFBD><EFBFBD>ٵ<EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>е<EFBFBD>113<EFBFBD><EFBFBD>,<EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD>114<EFBFBD><EFBFBD>,<EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD>113<EFBFBD>벻<EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD>,<EFBFBD>ӵ<EFBFBD>114<EFBFBD>뿪ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ65,<EFBFBD><EFBFBD>ʱִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><EFBFBD><EFBFBD>ֵΪ63(<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>113<EFBFBD><EFBFBD><EFBFBD><EFBFBD>δִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD>ʲ<EFBFBD>ֵΪ2)
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ4Ȧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD>114,115,116,117<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD>ҿ<EFBFBD><EFBFBD>Խ<EFBFBD><EFBFBD>ٵ<EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷽ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ2,<EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>е<EFBFBD>115,116<EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>117<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ0,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊʲô<EFBFBD><EFBFBD><EFBFBD>ٲ<EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶȵ<EFBFBD>ԭ<EFBFBD><EFBFBD>
*/
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; // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>״̬
}
}
if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetNormalStop != NULL) // һ<><D2BB>ֹͣ
{
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;
// <20><><EFBFBD>õ<EFBFBD>ǰ<EFBFBD>ٶ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
decPPSG = pCtrl->nstopPPSG;
vAxisCmd.vaxisId = vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vAxisCmd.bufSel = BUF_SEL3;
vAxisCmd.calcGap = decPPSG;
vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
rslt = StartVAxisRun(&vAxisCmd);
if (rslt < 0)
{
break;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC>ٵ<EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD><D9B6><EFBFBD>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>
decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);
if (pCtrl->mvData.mvmtdat.interLong != 0)
{
decCount += pCtrl->mvData.mvmtdat.interLong-1;
decCount /= pCtrl->mvData.mvmtdat.interLong; // <20><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
}
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; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD>
// 20220406 ljs <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+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; // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>״̬
}
}
}while(1);
StopVAxisRun(vAxisId);
if (pCtrl->WorkAfterRun != NULL)
{
pCtrl->WorkAfterRun(pCtrl->workPara1, pCtrl->workPara2);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GetMotosCounter(position) == 0)
{
RefreshMotosPos(position); // <20><><EFBFBD>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
if (pCtrl->RefreshPosition != NULL)
{
pCtrl->RefreshPosition(pCtrl->refPosPara, position);
}
}
else
{
rslt = 5;
}
return rslt;
}
#endif
Reference in New Issue Copy Permalink