yangmeng/optical
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
main
optical/EMBOS/Users/EmbFunc/movectrl/movectrl.c

1884 lines
44 KiB
C
Raw Permalink Normal View History

调试完成
2025-09-04 01:45:08 +00:00
#define _IN_MOVECTRL_C
#include "movectrl.h"
//----------------------------------------------------------------------------------------------------
#include "delay.h"
#include "trigger.h"
#include "inout.h"
#include "shell.h"
#include "error.h"
#include "workctrl.h"
#ifndef IsStopButton
#define IsStopButton GetInputNullOff
#endif
//----------------------------------------------------------------------------------------------------
__weak void NormalRefPosition(u32 para, s32 * posilist)
{
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD>ֹͣ
int GetCommonEms(void)
{
int err = ERR_NONE;
TasksWhenDelay();
return err;
}
// <20><><EFBFBD><EFBFBD>ֹͣ
int GetCommonQStop(void)
{
int err = ERR_NONE;
return err;
}
// <20><>ֹͨͣ
int GetCommonNStop(void)
{
int err = ERR_NONE;
#if (0)
if (err == ERR_NONE &&
IsConsoleCancel() != 0 && // <20>ն<EFBFBD><D5B6><EFBFBD><EFBFBD><EFBFBD>ֹͣ
1 )
{
printf("get console cancel\r\n");
err = STA_NORMAL_STOP;
}
#endif
#if (1)
if (err == ERR_NONE &&
IsStopButton() == SENSOR_ON && // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>ť<EFBFBD><C5A5><EFBFBD><EFBFBD>
1 )
{
printf("get stop button down\r\n");
err = STA_NORMAL_STOP;
}
#endif
return err;
}
//----------------------------------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>⣨<EFBFBD><E2A3A8><EFBFBD>㣩
#if (1)
#define ADJ_RUN_MVMT(cm) ((cm)*3)
#define ADJ_FAN_MVMT(cm) ((cm)*36/360)
#define CANC_MVMT(cm) ((cm)*288/360)
#define ADJ_WAVE_MVMT(cm) ((cm)*10/360)
#define ADJ_WVC_MVMT(cm) ((cm)*1/360)
// <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
int RSensorEmsScan(u32 para1, u32 para2)
{
int rslt = GetCommonEms();
return rslt;
}
// <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int RSensorQStopScan(u32 para1, u32 para2)
{
int rslt;
RSenserCheck * pchk;
rslt = GetCommonQStop();
if (rslt != ERR_NONE)
{
return rslt;
}
pchk = (RSenserCheck *)para1;
if (pchk == NULL)
{
return STA_NORMAL_STOP;
}
if (pchk->zeroflag == 0) // <20><><EFBFBD><EFBFBD>û<EFBFBD>о<EFBFBD><D0BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
pchk->cansflag = 0;
if (pchk->sensorscan != NULL)
{
if (pchk->sensorscan() == pchk->val) // <20><><EFBFBD>⵽<EFBFBD><E2B5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
// printf("1. first in sensor area when run\r\n");
pchk->zeroflag = 1; // <20><><EFBFBD>þ<EFBFBD><C3BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
pchk->canscount = g_motosPara.motosPos[pchk->axisIdx];
}
}
}
else
{
if (pchk->cansflag == 0) // <20><><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if (pchk->sensorscan != NULL)
{
if (pchk->sensorscan() != pchk->val) // <20><><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if (abs(g_motosPara.motosPos[pchk->axisIdx] - pchk->canscount) > pchk->cansmvmt)
{
// printf("1. first out zero area when run\r\n");
pchk->cansflag = 1;
}
}
}
}
else
{
if (pchk->sensorscan != NULL)
{
if (pchk->sensorscan() == pchk->val) // <20><><EFBFBD>⴫<EFBFBD><E2B4AB><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
// printf("1. in sensor area when run\r\n");
rslt = STA_NORMAL_STOP;
}
}
}
}
return rslt;
}
// <20><>ֹͨͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int RSensorNStopScan(u32 para1, u32 para2)
{
int rslt;
rslt = GetCommonNStop();
if (rslt != ERR_NONE)
{
return rslt;
}
return rslt;
}
//------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int RoundRunToSensor(RSenserCheck * pck)
{
return RoundRunToSensorBase(pck, 0);
}
// <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե
int RoundRunToSensorEdge(RSenserCheck * pck)
{
return RoundRunToSensorBase(pck, 1);
}
// <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int RoundRunToSensorCenter(RSenserCheck * pck)
{
return RoundRunToSensorBase(pck, 2);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// flag,<2C><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ĸ<EFBFBD>λ<EFBFBD><CEBB> 0,<2C>޶<EFBFBD><DEB6><EFBFBD> 1,ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Եλ<D4B5><CEBB> 2,ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
int RoundRunToSensorBase(RSenserCheck * pck, int flag)
{
int rslt, err, i;
InterMoveCtrl lmctrl;
u16 axisIdx;
u32 interLong;
if (pck == NULL)
{
return -1;
}
axisIdx = pck->axisIdx;
assert_param(axisIdx < AXIS_NUM);
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
lmctrl.vAxisId = pck->vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.newConfig = pck->newCfg; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pck->cansmvmt = CANC_MVMT(pck->circleNum);
if (1 &&
pck->newCfg == 0 &&
pck->syncflag != 0 &&
1 )
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pck->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = ADJ_RUN_MVMT(pck->circleNum);
}
}
}
else
{
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pck->axisCfg; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pck->axisOtp; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ(CW/CCW<43><57>PULSE/DIR)
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // ʵ<><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>)
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // ʵ<><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>)
lmctrl.mvmtPara.movement[axisIdx] = ADJ_RUN_MVMT(pck->circleNum); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
interLong = abs(ADJ_RUN_MVMT(pck->circleNum));
lmctrl.mvmtPara.extraRepeat = 0; // <20><><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
lmctrl.mvmtPara.interLong = interLong; // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.mvmtPara.pulsePerSegment = abs(pck->circleNum); // ÿ<><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
lmctrl.spdPara.startPPS = pck->rspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pck->rspd.stspSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pck->rspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pck->rspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pck->rspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pck->rspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.stopMode = STOP_MODE_SPD; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
// <20>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.Delay = DelayRef; // <20><>ʱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.RefreshPosition = NormalRefPosition; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>»ص<C2BB><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.refPosPara = 0; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>²<EFBFBD><C2B2><EFBFBD>
lmctrl.WorkBeforeRun = NULL; // <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.WorkAfterRun = NULL; // <20><><EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.workPara1 = 0;
lmctrl.workPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetSlowDown = NULL; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⽵<EFBFBD><E2BDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>ٶȽ<D9B6><C8BD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD>ٶȻָ<C8BB>
lmctrl.GetNormalStop = RSensorNStopScan; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = RSensorQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
lmctrl.GetEmergencyStop = RSensorEmsScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壨<EFBFBD><E5A3A8><EFBFBD>轵<EFBFBD>٣<EFBFBD>
lmctrl.condPara1 = (u32)(pck);;
lmctrl.condPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
if (rslt == 3) // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
{
DelayRef(100); // <20>ȴ<EFBFBD><C8B4><EFBFBD>е<EFBFBD>ȶ<EFBFBD>
// <20><><EFBFBD>¼<EFBFBD><C2BC>鴫<EFBFBD><E9B4AB><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
if (pck->sensorscan != NULL)
{
if (pck->sensorscan() != pck->val) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
printf("not at sensor\r\n");
if (
pck->newCfg == 0 &&
pck->syncflag != 0 )
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pck->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
}
}
}
else
{
lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
}
lmctrl.motosConfig.newConfig = 0;
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
}
}
}
err = GetMoveResultError(rslt, lmctrl.errInfo);
#if (1)
if (flag != 0)
{
// <20>ڶ<EFBFBD><DAB6>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
pck->sensorscan() == pck->val && // <20>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
1 )
{
// printf("move to edge\r\n");
int gapcount, pcount;
// ȡ<><C8A1>
if (pck->val == SENSOR_ON)
{
pck->val = SENSOR_OFF;
}
else
{
pck->val = SENSOR_ON;
}
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>,ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.startPPS = pck->rspd.stspSpd/2; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pck->rspd.stspSpd/2; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pck->rspd.runSpd/2; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pck->rspd.runSpd/2; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pck->rspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pck->rspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
if (
pck->newCfg == 0 &&
pck->syncflag != 0 )
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pck->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
}
}
}
else
{
lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
}
pck->cansflag = 0;
pck->cansmvmt = ADJ_WVC_MVMT(pck->circleNum);
lmctrl.motosConfig.newConfig = 0;
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>, <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>Ե
err = GetMoveResultError(rslt, lmctrl.errInfo);
#if (1)
if (flag == 2)
{
if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
pck->sensorscan() == pck->val &&
1 )
{
pcount = GetMotoCounter(axisIdx); // <20><>ȡλ<C8A1><CEBB>
// <20><><EFBFBD><EFBFBD>
if (
pck->newCfg == 0 &&
pck->syncflag != 0)
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pck->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum);
}
}
}
else
{
lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum);
}
pck->cansflag = 0;
pck->cansmvmt = ADJ_WVC_MVMT(pck->circleNum);
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>, <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>Ե
err = GetMoveResultError(rslt, lmctrl.errInfo);
if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
pck->sensorscan() == pck->val &&
1 )
{
gapcount = abs(GetMotoCounter(axisIdx) - pcount); // <20><>ȡλ<C8A1><CEBB>
gapcount /= 2;
if (gapcount != 0)
{
if (
pck->newCfg == 0 &&
pck->syncflag != 0 )
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pck->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = gapcount*(-1);
}
}
}
else
{
lmctrl.mvmtPara.movement[axisIdx] = gapcount*(-1);
}
lmctrl.GetNormalStop = NULL; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = NULL; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
lmctrl.stopMode = STOP_MODE_TAB; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
lmctrl.spdPara.startPPS = pck->rspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pck->rspd.runSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pck->rspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
}
}
}
}
#endif
}
}
#endif
return err;
}
//------------------------------------------------------------------
// <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
int RMotoRunEmsScan(u32 para1, u32 para2)
{
RMotoRun * pmr;
int rslt = GetCommonEms();
pmr = (RMotoRun *)para1;
if (pmr->RRunProc != NULL)
{
pmr->RRunProc();
}
return rslt;
}
// <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int RMotoRunQStopScan(u32 para1, u32 para2)
{
int rslt;
rslt = GetCommonQStop();
if (rslt != ERR_NONE)
{
return rslt;
}
return rslt;
}
// <20><>ֹͨͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int RMotoRunNStopScan(u32 para1, u32 para2)
{
int rslt;
rslt = GetCommonNStop();
if (rslt != ERR_NONE)
{
return rslt;
}
return rslt;
}
// <20><>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
int RoundRunAsOffset(RMotoRun * pmr)
{
int rslt, err, i;
InterMoveCtrl lmctrl;
int axisIdx;
int movmt;
if (pmr == NULL)
{
return ERR_MV_PARA;
}
movmt = 0;
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
lmctrl.vAxisId = pmr->vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.newConfig = pmr->newCfg; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (i = 0; i < AXIS_NUM; i++)
{
axisIdx = pmr->config[i].axisIdx;
if (axisIdx >= 0)
{
assert_param(axisIdx < AXIS_NUM);
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmr->config[i].axisCfg; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmr->config[i].poutType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ(CW/CCW<43><57>PULSE/DIR)
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // ʵ<><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>)
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // ʵ<><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>)
lmctrl.mvmtPara.movement[axisIdx] = pmr->config[i].movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
if (pmr->config[i].movement != 0)
{
movmt = pmr->config[i].movement;
}
}
}
// printf("round run as offset, movmt=%d\r\n", movmt);
if (movmt == 0) // <20><><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>
{
return ERR_NONE;
}
lmctrl.mvmtPara.extraRepeat = pmr->mrepeat; // <20><><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
lmctrl.mvmtPara.interLong = pmr->interLong; // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.mvmtPara.pulsePerSegment = pmr->segmentNum; // ÿ<><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
lmctrl.spdPara.startPPS = pmr->rspd.stspSpd;
lmctrl.spdPara.stopPPS = pmr->rspd.stspSpd;
lmctrl.spdPara.slowPPS = pmr->rspd.stspSpd;
lmctrl.spdPara.runPPS = pmr->rspd.runSpd;
lmctrl.spdPara.addPPSG = pmr->rspd.spdAdd;
lmctrl.spdPara.decPPSG = pmr->rspd.spdAdd;
lmctrl.spdPara.brkPPSG = pmr->rspd.brkAdd; // ֹͣ<CDA3><D6B9><EFBFBD>ٶ<EFBFBD>
lmctrl.stopMode = STOP_MODE_TAB; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
// <20>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.Delay = DelayRef; // <20><>ʱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.RefreshPosition = NormalRefPosition; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>»ص<C2BB><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.refPosPara = 0; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>²<EFBFBD><C2B2><EFBFBD>
lmctrl.WorkBeforeRun = NULL; // <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.WorkAfterRun = NULL; // <20><><EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.workPara1 = 0;
lmctrl.workPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetSlowDown = NULL; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⽵<EFBFBD><E2BDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>ٶȽ<D9B6><C8BD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD>ٶȻָ<C8BB>
if (pmr->escStop != 0)
{
// printf("stop condition is null\r\n");
lmctrl.GetNormalStop = NULL;
lmctrl.GetQuickStop = NULL;
}
else
{
// printf("stop condition is r move stop\r\n");
lmctrl.GetNormalStop = RMotoRunNStopScan; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = RMotoRunQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
}
lmctrl.GetEmergencyStop = RMotoRunEmsScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壨<EFBFBD><E5A3A8><EFBFBD>轵<EFBFBD>٣<EFBFBD>
lmctrl.condPara1 = (u32)(pmr);;
lmctrl.condPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
return err;
}
#endif
//----------------------------------------------------------------------------------------------------
#if (1)
// <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
int MoveToSensorEmsScan(u32 para1, u32 para2)
{
int rslt;
MTSCtrl * pmts;
pmts = (MTSCtrl*)(para1);
rslt = GetCommonEms();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
if (pmts->emcstopscan != NULL)
{
rslt = pmts->emcstopscan();
}
if (rslt != ERR_NONE)
{
return rslt;
}
if (pmts->movement > 0)
{
if (pmts->limitpsensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (pmts->limitpsensor() == SENSOR_ON)
{
rslt = ERR_LMT_POSITIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else if (pmts->movement < 0)
{
if (pmts->limitnsensor != NULL) // <20><><EFBFBD>ⷴ<EFBFBD><E2B7B4><EFBFBD><EFBFBD>λ
{
if (pmts->limitnsensor() == SENSOR_ON)
{
rslt = ERR_LMT_NEGATIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else
{
rslt = ERR_MTZ_ERROR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
return rslt;
}
// <20><>ֹͨͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveToSensorNStopScan(u32 para1, u32 para2)
{
int rslt;
MTSCtrl * pmts;
pmts = (MTSCtrl*)(para1);
rslt = GetCommonNStop();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
return rslt;
}
// <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveToSensorQStopScan(u32 para1, u32 para2)
{
int rslt;
MTSCtrl * pmts;
pmts = (MTSCtrl*)(para1);
rslt = GetCommonQStop();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
if (rslt == ERR_NONE)
{
if (pmts->zerosensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (pmts->zerosensor() == pmts->zeroval)
{
rslt = STA_MTZ_SUCCESS; // <20><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>
}
}
}
return rslt;
}
// <20><><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>
int MoveToSensorSlowScan(u32 para1, u32 para2)
{
int rslt;
MTSCtrl * pmts;
pmts = (MTSCtrl*)(para1);
rslt = 0;
if (pmts != NULL)
{
if (pmts->slowsensor != NULL) // <20><><EFBFBD>⽵<EFBFBD><E2BDB5>
{
if (pmts->slowsensor() == SENSOR_ON)
{
pmts->slowsta = 1;
}
}
}
rslt = pmts->slowsta; // <20><><EFBFBD><EFBFBD>״̬
return rslt;
}
//------------------------------------------------
// ֱ<><D6B1><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int LineRunToSensorCtrl(MTSCtrl * pmtsc)
{
int rslt, err, poismv;
InterMoveCtrl lmctrl;
u16 axisIdx;
int i;
if (pmtsc == NULL)
{
return -1;
}
axisIdx = pmtsc->axisIdx;
assert_param(axisIdx < AXIS_NUM);
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
lmctrl.vAxisId = pmtsc->vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.newConfig = pmtsc->newCfg; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (
pmtsc->newCfg == 0 &&
pmtsc->syncflag != 0)
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pmtsc->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = pmtsc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
}
}
else
{
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsc->axisCfg; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsc->poutType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ(CW/CCW<43><57>PULSE/DIR)
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // ʵ<><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>)
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // ʵ<><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>)
lmctrl.mvmtPara.movement[axisIdx] = pmtsc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
lmctrl.mvmtPara.extraRepeat = 0; // <20><><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
lmctrl.mvmtPara.interLong = abs(pmtsc->movement); // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT; // ÿ<><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
lmctrl.spdPara.startPPS = pmtsc->lspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pmtsc->lspd.stspSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pmtsc->lspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pmtsc->lspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pmtsc->lspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pmtsc->lspd.spdAdd/2; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pmtsc->lspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.stopMode = STOP_MODE_SPD; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
// <20>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.Delay = DelayRef; // <20><>ʱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.RefreshPosition = NormalRefPosition; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>»ص<C2BB><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.refPosPara = 0; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>²<EFBFBD><C2B2><EFBFBD>
lmctrl.WorkBeforeRun = NULL; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.WorkAfterRun = NULL; // <20><><EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.workPara1 = 0;
lmctrl.workPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetSlowDown = MoveToSensorSlowScan; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⽵<EFBFBD><E2BDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>ٶȽ<D9B6><C8BD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD>ٶȻָ<C8BB>
lmctrl.GetNormalStop = MoveToSensorNStopScan; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = MoveToSensorQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
lmctrl.GetEmergencyStop = MoveToSensorEmsScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壨<EFBFBD><E5A3A8><EFBFBD>轵<EFBFBD>٣<EFBFBD>
lmctrl.condPara1 = (u32)(pmtsc);
lmctrl.condPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err = ERR_NONE;
poismv = 0;
#if (1)
if (pmtsc->syncflag == 0)
{
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>
if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE) ) // <20><><EFBFBD><EFBFBD>λ
{
poismv = pmtsc->mvposi; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
}
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (err == STA_MTZ_SUCCESS) // <20><><EFBFBD><EFBFBD>λλ<CEBB><CEBB>
{
poismv = pmtsc->mvposi; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
}
else if (err == ERR_NONE)
{
rslt = MoveToSensorSlowScan(lmctrl.condPara1, lmctrl.condPara2); // <20>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (rslt != ERR_NONE)
{
poismv = pmtsc->mvposi; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
}
}
else
{
}
}
#endif
do
{
// <20>˶<EFBFBD>
if (poismv != 0)
{
if (pmtsc->moveasoffset != NULL)
{
err = pmtsc->moveasoffset(poismv, &(pmtsc->lspd)); // <20>˶<EFBFBD>һ<EFBFBD>ξ<EFBFBD><CEBE><EFBFBD>
}
if (err == ERR_NONE)
{
// <20><><EFBFBD>¼<EFBFBD><C2BC><EFBFBD>
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>
if (err == ERR_NONE)
{
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (err == ERR_NONE)
{
pmtsc->slowsta = 0;
}
}
}
}
if (err == ERR_NONE)
{
s32 temp, val;
temp = GetMotoCounter(axisIdx); // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
if (poismv == 0 && (rslt == 3 || rslt == 2))
{
if (rslt == 3)
{
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (err == STA_MTZ_SUCCESS && pmtsc->mvposi != 0)
{
val = GetMotoCounter(axisIdx);
if (abs(val-temp) < abs(pmtsc->mvposi/2)) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>̫С
{
poismv = pmtsc->mvposi; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
continue; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>һ<EFBFBD>ι<EFBFBD><CEB9><EFBFBD>
}
}
}
else if (rslt == 2)
{
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>
if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE) ) // <20><><EFBFBD><EFBFBD>λ
{
poismv = pmtsc->mvposi; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
continue; // <20><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>һ<EFBFBD>ι<EFBFBD><CEB9><EFBFBD>
}
}
}
err = GetMoveResultError(rslt, lmctrl.errInfo);
}
break;
}while(1);
return err;
}
//------------------------------------------------
#endif
//----------------------------------------------------------------------------------------------------
#if (1)
// <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
int MoveBHEmsScan(u32 para1, u32 para2)
{
int rslt;
MVByHandCtrl * pmbhc;
pmbhc = (MVByHandCtrl*)(para1);
rslt = GetCommonEms();
if (rslt != ERR_NONE || pmbhc == NULL)
{
return rslt;
}
if (pmbhc->movement > 0)
{
if (pmbhc->limitpsensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (pmbhc->limitpsensor(pmbhc->para) == SENSOR_ON)
{
rslt = ERR_LMT_POSITIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else if (pmbhc->movement < 0)
{
if (pmbhc->limitnsensor != NULL) // <20><><EFBFBD>ⷴ<EFBFBD><E2B7B4><EFBFBD><EFBFBD>λ
{
if (pmbhc->limitnsensor(pmbhc->para) == SENSOR_ON)
{
rslt = ERR_LMT_NEGATIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else
{
rslt = ERR_MTZ_ERROR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
return rslt;
}
// <20><>ֹͨͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveBHNStopScan(u32 para1, u32 para2)
{
int rslt;
MVByHandCtrl * pmbhc;
pmbhc = (MVByHandCtrl*)(para1);
rslt = GetCommonNStop();
if (rslt != ERR_NONE || pmbhc == NULL)
{
return rslt;
}
if (rslt == ERR_NONE && pmbhc->stopscan != NULL)
{
rslt = pmbhc->stopscan(pmbhc->para);
}
return rslt;
}
// <20><><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>
int MoveBHNSlowScan(u32 para1, u32 para2)
{
int rslt;
MVByHandCtrl * pmbhc;
pmbhc = (MVByHandCtrl*)(para1);
rslt = ERR_NONE;
if (pmbhc->slowscan != NULL)
{
rslt = pmbhc->slowscan(para2);
}
return rslt;
}
// <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveBHQStopScan(u32 para1, u32 para2)
{
int rslt;
MVByHandCtrl * pmbhc;
pmbhc = (MVByHandCtrl*)(para1);
rslt = GetCommonQStop();
if (rslt != ERR_NONE || pmbhc == NULL)
{
return rslt;
}
return rslt;
}
// <20>ֶ<EFBFBD><D6B6>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>
int MoveByHandCtrl(MVByHandCtrl * pmbhc)
{
int rslt, err, i;
u16 axisIdx;
InterMoveCtrl lmctrl;
if (pmbhc == NULL)
{
return ERR_NONE;
}
if (pmbhc->movement == 0)
{
return ERR_NONE;
}
// <20>Ⱦ<EFBFBD><C8BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (pmbhc->precondition != NULL)
{
err = pmbhc->precondition(pmbhc->para);
if (err != ERR_NONE)
{
return err;
}
}
axisIdx = pmbhc->axisIdx;
assert_param(axisIdx < AXIS_NUM);
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
lmctrl.vAxisId = pmbhc->vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.newConfig = pmbhc->newCfg; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (
pmbhc->newCfg == 0 &&
pmbhc->syncflag != 0)
{
for (i = 0; i < AXIS_NUM; i++)
{
if (pmbhc->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = pmbhc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
}
}
else
{
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmbhc->axisCfg; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmbhc->poutType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ(CW/CCW<43><57>PULSE/DIR)
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // ʵ<><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>)
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // ʵ<><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>)
lmctrl.mvmtPara.movement[axisIdx] = pmbhc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
lmctrl.mvmtPara.extraRepeat = 0; // <20><><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
lmctrl.mvmtPara.interLong = abs(pmbhc->movement); // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.mvmtPara.pulsePerSegment = pmbhc->ppSegment; // ÿ<><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
lmctrl.spdPara.startPPS = pmbhc->hspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pmbhc->hspd.stspSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pmbhc->hspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pmbhc->hspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pmbhc->hspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pmbhc->hspd.spdAdd/2; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pmbhc->hspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.stopMode = STOP_MODE_SPD; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
// <20>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.Delay = DelayRef; // <20><>ʱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.RefreshPosition = NormalRefPosition; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>»ص<C2BB><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.refPosPara = 0; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>²<EFBFBD><C2B2><EFBFBD>
lmctrl.WorkBeforeRun = NULL; // <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.WorkAfterRun = NULL; // <20><><EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.workPara1 = 0;
lmctrl.workPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetSlowDown = MoveBHNSlowScan; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⽵<EFBFBD><E2BDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>ٶȽ<D9B6><C8BD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD>ٶȻָ<C8BB>
lmctrl.GetNormalStop = MoveBHNStopScan; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = MoveBHQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
lmctrl.GetEmergencyStop = MoveBHEmsScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壨<EFBFBD><E5A3A8><EFBFBD>轵<EFBFBD>٣<EFBFBD>
lmctrl.condPara1 = (u32)(pmbhc);;
lmctrl.condPara2 = (u32)(pmbhc->movement<0?-1:1); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
return err;
}
#endif
//------------------------------------------------
//-------------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>첽<EFBFBD>˶<EFBFBD>
// ʹ<><CAB9>STM32<33><32><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>&<26><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int SeparateExerStart(SepMoveCtrl * pCtrl) // <20><><EFBFBD><EFBFBD><EFBFBD>첽<EFBFBD>˶<EFBFBD> <20><><EFBFBD><EFBFBD>
{
int rslt;
MotoCtrl lmctrl;
u16 axisIdx;
u16 axisID;
int i;
if (pCtrl == NULL)
{
return -1;
}
axisIdx = pCtrl->axisIdx;
assert_param(axisIdx < AXIS_NUM);
memset(&lmctrl, 0, sizeof(MotoCtrl));
lmctrl.movement = pCtrl->movement; // λ<><CEBB><EFBFBD><EFBFBD>
lmctrl.startPPS = pCtrl->startPPS; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.runPPS = pCtrl->runPPS; // <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
lmctrl.addPPSG = pCtrl->addPPSG; // <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
lmctrl.brkPPSG = pCtrl->brkPPSG; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.funTestTime = pCtrl->funTestTime; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
for (i = 0; i < 6; i++)
{
lmctrl.syncList[i] = pCtrl->syncList[i]; // ͬ<><CDAC><EFBFBD>ƶ<EFBFBD>ѡ<EFBFBD><D1A1>
}
lmctrl.GetNormalStop = pCtrl->GetNormalStop; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetQuickStop = pCtrl->GetQuickStop; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.condPara1 = pCtrl->condPara1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.condPara2 = pCtrl->condPara2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.blockRunflag = pCtrl->blockRunflag; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־
lmctrl.ExecWhenRun = pCtrl->ExecWhenRun; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>еĺ<D0B5><C4BA><EFBFBD>
lmctrl.ExecWhenStart = pCtrl->ExecWhenStart; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰִ<C7B0>еĺ<D0B5><C4BA><EFBFBD>
lmctrl.ExecWhenStop = pCtrl->ExecWhenStop; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9>ִ<EFBFBD>еĺ<D0B5><C4BA><EFBFBD>
axisID = axisIdx + 1;
if (pCtrl->newCfg != 0)
{// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((pCtrl->axisCfg & CONFIG_EN) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if ((pCtrl->axisCfg & POA_EN) != 0) // A<><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, POUTA_EN);
}
else
{
SetAxisConfig(axisID, POUTA_DIS);
}
if ((pCtrl->axisCfg & POB_EN) != 0) // B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, POUTB_EN);
}
else
{
SetAxisConfig(axisID, POUTB_DIS);
}
if ((pCtrl->axisCfg & SON_EN) != 0) // <20>ŷ<EFBFBD>ON<4F><4E><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, SERVO_ON);
}
else
{
SetAxisConfig(axisID, SERVO_OFF);
}
if ((pCtrl->axisCfg & ALM_LEVH) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽΪ<C6BD><CEAA>
{
SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
}
else
{
SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
}
if ((pCtrl->axisCfg & ALM_EN) != 0) // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisAlarmConfig(axisID, ALM_STOP_EN);
}
else
{
SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
}
if ((pCtrl->axisCfg & POUT_EN) != 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SetAxisConfig(axisID, pCtrl->poutType); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ <20><><EFBFBD>巽ʽ<E5B7BD><CABD><EFBFBD><EFBFBD>
}
else
{
SetAxisConfig(axisID, POUT_DISABLE); // <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
#if (STM32_MOTOS_NUM >= 1)
if (((axisID == 1) || (lmctrl.syncList[1-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(1, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(1, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto1Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
#if (STM32_MOTOS_NUM >= 2)
if (((axisID == 2) || (lmctrl.syncList[2-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(2, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(2, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto2Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
#if (STM32_MOTOS_NUM >= 3)
if (((axisID == 3) || (lmctrl.syncList[3-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(3, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(3, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto3Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
#if (STM32_MOTOS_NUM >= 4)
if (((axisID == 4) || (lmctrl.syncList[4-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(4, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(4, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto4Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
#if (STM32_MOTOS_NUM >= 5)
if (((axisID == 5) || (lmctrl.syncList[5-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(5, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(5, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto5Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
#if (STM32_MOTOS_NUM >= 6)
if (((axisID == 6) || (lmctrl.syncList[6-1] == 1)) && (pCtrl->useOutPort == 0))
{
SetAxisConfig(6, POUT_ENABLE_BUSIO); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetAxisConfig(6, DATI_BUS_IO); // <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>ģʽ
SetSTM32Moto6Mode(MODE_STEPDIR); // <20><><EFBFBD><EFBFBD>ģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
}
#endif
switch (axisIdx)
{
#if (STM32_MOTOS_NUM >= 1)
case 0:
rslt = STM32Moto1Start(&lmctrl);
break;
#endif
#if (STM32_MOTOS_NUM >= 2)
case 1:
rslt = STM32Moto2Start(&lmctrl);
break;
#endif
#if (STM32_MOTOS_NUM >= 3)
case 2:
rslt = STM32Moto3Start(&lmctrl);
break;
#endif
#if (STM32_MOTOS_NUM >= 4)
case 3:
rslt = STM32Moto4Start(&lmctrl);
break;
#endif
#if (STM32_MOTOS_NUM >= 5)
case 4:
rslt = STM32Moto5Start(&lmctrl);
break;
#endif
#if (STM32_MOTOS_NUM >= 6)
case 5:
rslt = STM32Moto6Start(&lmctrl);
break;
#endif
default:
rslt = ERR_MV_PARA; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
return rslt;
}
//------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>첽<EFBFBD>˶<EFBFBD> ֹͣ
int SeparateExerStop(u16 axisIdx)
{
int rslt;
assert_param(axisIdx < AXIS_NUM);
rslt = ERR_NONE;
switch (axisIdx)
{
case 0:
#if (STM32_MOTOS_NUM >= 1)
STM32Moto1Stop();
break;
#endif
#if (STM32_MOTOS_NUM >= 2)
case 1:
STM32Moto2Stop();
break;
#endif
#if (STM32_MOTOS_NUM >= 3)
case 2:
STM32Moto3Stop();
break;
#endif
#if (STM32_MOTOS_NUM >= 4)
case 3:
STM32Moto4Stop();
break;
#endif
#if (STM32_MOTOS_NUM >= 5)
case 4:
STM32Moto5Stop();
break;
#endif
#if (STM32_MOTOS_NUM >= 6)
case 5:
STM32Moto6Stop();
break;
#endif
default:
rslt = ERR_MV_PARA; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
return rslt;
}
//------------------------------------------------
int GetSeparateExerState(u16 axisIdx) // <20><><EFBFBD><EFBFBD><EFBFBD>첽<EFBFBD>˶<EFBFBD> <20>õ<EFBFBD><C3B5><EFBFBD>ǰ״̬
{
int rslt,err;
assert_param(axisIdx < AXIS_NUM);
rslt = ERR_NONE;
switch (axisIdx)
{
#if (STM32_MOTOS_NUM >= 1)
case 0:
rslt = GetSTM32Moto1State();
break;
#endif
#if (STM32_MOTOS_NUM >= 2)
case 1:
rslt = GetSTM32Moto2State();
break;
#endif
#if (STM32_MOTOS_NUM >= 3)
case 2:
rslt = GetSTM32Moto3State();
break;
#endif
#if (STM32_MOTOS_NUM >= 4)
case 3:
rslt = GetSTM32Moto4State();
break;
#endif
#if (STM32_MOTOS_NUM >= 5)
case 4:
rslt = GetSTM32Moto5State();
break;
#endif
#if (STM32_MOTOS_NUM >= 6)
case 5:
rslt = GetSTM32Moto6State();
break;
#endif
default:
rslt = ERR_MV_PARA; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
if (rslt == STM32MOTO_STOP)
{
err = STA_PROCESS_FINISH; // ִ<><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
{
//err = STA_PROCESS_RUNNING; // ִ<>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD>
err = rslt; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
}
return err;
}
//------------------------------------------------
//-------------------------------------------------------------------------------
// ת<><D7AA>Ϊ -180--180<38><30>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ChangeRotToNP180(s32 * pval)
{
while (*pval > (ROT_PULSE_PER_CIRCLE/2))
{
*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
}
while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE/2) * -1))
{
*pval += (s32)(ROT_PULSE_PER_CIRCLE);
}
}
// ת<><D7AA>Ϊ -360--360<36><30>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ChangeRotToNP360(s32 * pval)
{
while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE) * -1))
{
*pval += (s32)(ROT_PULSE_PER_CIRCLE);
}
while(*pval > ((s32)(ROT_PULSE_PER_CIRCLE)))
{
*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
}
}
// ת<><D7AA>Ϊ 0--360<36><30>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ChangeRotTo0To360(s32 * pval)
{
while (*pval < 0)
{
*pval += (s32)(ROT_PULSE_PER_CIRCLE);
}
while(*pval > ((s32)(ROT_PULSE_PER_CIRCLE)))
{
*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
}
}
// ת<><D7AA>Ϊ -360--0<><30>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ChangeRotToN360To0(s32 * pval)
{
while (*pval > 0)
{
*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
}
while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE) * -1))
{
*pval += (s32)(ROT_PULSE_PER_CIRCLE);
}
}
//
int GetMoveResultError(int rslt, int errinfo)
{
int err;
switch (rslt)
{
case -1: // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err = ERR_MV_PARA;
break;
case -2: // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
err = ERR_FILL_DATA;
break;
case -3: // FPGA<47><41>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
err = ERR_FPGA_RESET; // FPGA<47><41>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
break;
case -4: // FPGA<47>汾<EFBFBD><E6B1BE><EFBFBD><EFBFBD>
err = ERR_FPGA_ERR; // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE>汾<EFBFBD><E6B1BE><EFBFBD><EFBFBD>
break;
case 1: // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err = ERR_DRIVER;
break;
case 0: // <20>˶<EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case 2: // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
case 3: // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
case 4: // <20><>ͨͣ<CDA8><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
err = errinfo;
break;
case 5:
err = ERR_FPGA_RESET;
break;
default: // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err = ERR_NONE;
break;
}
return err;
}
//---------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ת<EFBFBD><D7AA>
// <20><><EFBFBD><EFBFBD>pdir == 0, ת<><D7AA>Ϊ -180--180<38><30>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,
// <20><><EFBFBD><EFBFBD>pdir != 0 <20><>ôת<C3B4><D7AA>Ϊ 0-- 360֮<30><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ChangeMsToNP180(s32 * pval, int pdir)
{
while (*pval < ((s32)(MS_PULSE_PER_CIRCLE/2) * -1))
{
*pval += (s32)(MS_PULSE_PER_CIRCLE);
}
while (*pval > ((s32)(MS_PULSE_PER_CIRCLE/2)))
{
*pval -= (s32)(MS_PULSE_PER_CIRCLE);
}
if (pdir != 0) // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
{
if (*pval < 0)
{
*pval += MS_PULSE_PER_CIRCLE;
}
}
}
void ChangeMsToNP360(s32 * pval)
{
*pval %= MS_PULSE_PER_CIRCLE;
while (*pval < ((s32)(MS_PULSE_PER_CIRCLE) * -1))
{
*pval += (s32)(MS_PULSE_PER_CIRCLE);
}
while(*pval > ((s32)(MS_PULSE_PER_CIRCLE)))
{
*pval -= (s32)(MS_PULSE_PER_CIRCLE);
}
}
void ChangeMsTo0To360(s32 * pval)
{
*pval %= MS_PULSE_PER_CIRCLE;
while (*pval < 0)
{
*pval += (s32)(MS_PULSE_PER_CIRCLE);
}
while(*pval > ((s32)(MS_PULSE_PER_CIRCLE)))
{
*pval -= (s32)(MS_PULSE_PER_CIRCLE);
}
}
//--------------------------------------------------------------------------------
// <20><>ֹͨͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveToSensorAsMidPosNStopScan(u32 para1, u32 para2)
{
int rslt;
MTSAMPCtrl * pmts;
pmts = (MTSAMPCtrl*)(para1);
rslt = GetCommonNStop();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
return rslt;
}
// <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveToSensorAsMidPosQStopScan(u32 para1, u32 para2)
{
int rslt;
MTSAMPCtrl * pmts;
pmts = (MTSAMPCtrl*)(para1);
rslt = GetCommonQStop();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
if (rslt == ERR_NONE)
{
if (pmts->zerosensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (pmts->zerosensor() == pmts->zeroval)
{
rslt = STA_MTZ_SUCCESS; // <20><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>
}
}
}
return rslt;
}
// <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
int MoveToSensorAsMidPosEmsScan(u32 para1, u32 para2)
{
int rslt;
MTSAMPCtrl * pmts;
pmts = (MTSAMPCtrl*)(para1);
rslt = GetCommonEms();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
if (pmts->emcstopscan != NULL)
{
rslt = pmts->emcstopscan();
}
if (rslt != ERR_NONE)
{
return rslt;
}
if (pmts->movement > 0)
{
if (pmts->limitpsensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (pmts->limitpsensor() == SENSOR_ON)
{
rslt = ERR_LMT_POSITIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else if (pmts->movement < 0)
{
if (pmts->limitnsensor != NULL) // <20><><EFBFBD>ⷴ<EFBFBD><E2B7B4><EFBFBD><EFBFBD>λ
{
if (pmts->limitnsensor() == SENSOR_ON)
{
rslt = ERR_LMT_NEGATIVE; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
}
}
}
else
{
rslt = ERR_MTZ_ERROR; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
return rslt;
}
// <20>ҵ<EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
int MoveToMidPosQStopScan(u32 para1, u32 para2)
{
static int isckzero = 0;
int rslt = 0;
MTSAMPCtrl * pmts;
pmts = (MTSAMPCtrl*)(para1);
rslt = GetCommonQStop();
if (rslt != ERR_NONE || pmts == NULL)
{
return rslt;
}
if (rslt == ERR_NONE)
{
if (pmts->zerosensor != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
if (isckzero == 1)
{
if (pmts->zerosensor() != pmts->zeroval)
{
isckzero = 0;
rslt = 1;
}
}
else
{
if (pmts->zerosensor() == pmts->zeroval)
{
isckzero = 1;
}
}
}
}
return rslt;
}
// ֱ<><D6B1><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>,<2C>ر<EFBFBD>˵<EFBFBD><CBB5>,<2C><><EFBFBD>ֹ<EFBFBD><D6B9>㷽ʽ<E3B7BD><CABD><EFBFBD><EFBFBD>ʹ<EFBFBD>ô<EFBFBD><C3B4>л<EFBFBD><D0BB>ڵĵ<DAB5>Ƭ,<2C><><EFBFBD><EFBFBD><EFBFBD>ǻ<EFBFBD><C7BB>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
// return -1:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int LineRunToSensorAsMidPosCtrl(MTSAMPCtrl * pmtsampc)
{
int rslt, err;
InterMoveCtrl lmctrl;
u16 axisIdx;
int mvnum = 0; // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>һ<EFBFBD><D2BB>,<2C><><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>û<EFBFBD><C3BB><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
if (pmtsampc == NULL)
{
return -1;
}
axisIdx = pmtsampc->axisIdx;
assert_param(axisIdx < AXIS_NUM);
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
lmctrl.vAxisId = pmtsampc->vAxisId; // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.newConfig = pmtsampc->newCfg; // <20><>Ҫ<EFBFBD><D2AA><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err = ERR_NONE;
#if (1)
//if (pmtsampc->syncflag == 0)
{
err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>
if ((pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE) ) // <20><><EFBFBD><EFBFBD>λ
{
mvnum++;
pmtsampc->movement = pmtsampc->movement * (-1); // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
}
err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (err == STA_MTZ_SUCCESS) // <20><><EFBFBD><EFBFBD>λλ<CEBB><CEBB>
{
pmtsampc->movement = 0; // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6>ҵ<EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
}
}
#endif
if (pmtsampc->newCfg == 0 && pmtsampc->syncflag != 0)
{
for (int i = 0; i < AXIS_NUM; i++)
{
if (pmtsampc->syncList[i] != 0)
{
lmctrl.mvmtPara.movement[i] = pmtsampc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
}
}
else
{
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsampc->axisCfg; // ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsampc->poutType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ(CW/CCW<43><57>PULSE/DIR)
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // ʵ<><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>)
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // ʵ<><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>)
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD>λp
}
lmctrl.mvmtPara.extraRepeat = 0; // <20><><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement); // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT; // ÿ<><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶȿ<D9B6><C8BF>Ʋ<EFBFBD><C6B2><EFBFBD>
lmctrl.spdPara.startPPS = pmtsampc->lspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pmtsampc->lspd.stspSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pmtsampc->lspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pmtsampc->lspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pmtsampc->lspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pmtsampc->lspd.spdAdd/2; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pmtsampc->lspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.stopMode = STOP_MODE_SPD; // ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD>ʽ
// <20>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.Delay = DelayRef; // <20><>ʱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.RefreshPosition = NormalRefPosition; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>»ص<C2BB><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.refPosPara = 0; // <20><><EFBFBD><EFBFBD>ˢ<EFBFBD>²<EFBFBD><C2B2><EFBFBD>
lmctrl.WorkBeforeRun = NULL; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.WorkAfterRun = NULL; // <20><><EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
lmctrl.workPara1 = 0;
lmctrl.workPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lmctrl.GetSlowDown = NULL; // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⽵<EFBFBD><E2BDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>ٶȽ<D9B6><C8BD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD>ٶȻָ<C8BB>
lmctrl.GetNormalStop = MoveToSensorAsMidPosNStopScan; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC>ٶȣ<D9B6>
lmctrl.GetQuickStop = MoveToSensorAsMidPosQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
lmctrl.GetEmergencyStop = MoveToSensorAsMidPosEmsScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壨<EFBFBD><E5A3A8><EFBFBD>轵<EFBFBD>٣<EFBFBD>
lmctrl.condPara1 = (u32)(pmtsampc);;
lmctrl.condPara2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
while(mvnum < 2 && pmtsampc->movement != 0)
{
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
if (rslt == 3)
{
err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
break;
}
else if (rslt == 2)
{
err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2); // <20><><EFBFBD>⼱ͣ<E2BCB1><CDA3><EFBFBD><EFBFBD>
if ( (pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE) || // <20><><EFBFBD><EFBFBD>λ
0 )
{
mvnum++;
pmtsampc->movement = pmtsampc->movement * (-1); // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
}
else
{
break;
}
}
else
{
break;
}
}
if (mvnum >= 2) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>δ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
err = ERR_MTZ_ERROR;
return err;
}
if (err == STA_MTZ_SUCCESS) // <20><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>,<2C><>ʼ<EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
s32 pos1, pos2; // <20><>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>λ<EFBFBD>ü<EFBFBD>¼
pmtsampc->movement = pmtsampc->bafflewidth + 1000;
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement); // <20>岹<EFBFBD><E5B2B9><EFBFBD><EFBFBD>λ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>movement<6E><74><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ֵ
lmctrl.spdPara.startPPS = pmtsampc->mposspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.stopPPS = pmtsampc->mposspd.stspSpd; // ֹͣ<CDA3>ٶ<EFBFBD>
lmctrl.spdPara.runPPS = pmtsampc->mposspd.runSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.slowPPS = pmtsampc->mposspd.stspSpd; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.spdPara.addPPSG = pmtsampc->mposspd.spdAdd; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.decPPSG = pmtsampc->mposspd.spdAdd/2; // <20><><EFBFBD>ټ<EFBFBD><D9BC>ٶ<EFBFBD>
lmctrl.spdPara.brkPPSG = pmtsampc->mposspd.brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
lmctrl.GetQuickStop = MoveToMidPosQStopScan; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>
err = ERR_NONE;
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
if (err == 1)
{
pos1 = GetMotoCounter(axisIdx);
pmtsampc->movement = pmtsampc->movement * (-1);
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
err = ERR_NONE;
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
if (err == 1)
{
int dir;
pos2 = GetMotoCounter(axisIdx);
if (pmtsampc->movement > 0)
{
dir = -1; // <20><>ǰһ<C7B0><D2BB><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>෴
}
else
{
dir = 1;
}
pmtsampc->movement = abs(pos1-pos2)/2; // 20220317 BUG<55>޸<EFBFBD>,<2C><><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֻ<EFBFBD>ᵥ<EFBFBD><E1B5A5><EFBFBD>˶<EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD>
pmtsampc->movement *= dir;
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement);
lmctrl.GetQuickStop = NULL; // 20210812 <20><><EFBFBD><EFBFBD>Щ<EFBFBD><D0A9>Ƭ<EFBFBD><C6AC><EFBFBD>л<EFBFBD><D0BB><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD><EFBFBD><EFBFBD>ʱ,ʵ<>ʴ<EFBFBD><CAB4>ֹ<EFBFBD><D6B9>㷽ʽ<E3B7BD><CABD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD><EFBFBD><EFBFBD>)
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>»<EFBFBD><C2BB><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ʴ˴<CAB4><CBB4>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ʱ<EFBFBD><CAB1><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
err = ERR_NONE;
rslt = InterpolationMotion(&lmctrl); // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
err = GetMoveResultError(rslt, lmctrl.errInfo);
if (err == ERR_NONE)
{
err = STA_MTZ_SUCCESS;
}
}
}
}
return err;
}
Reference in New Issue Copy Permalink