1884 lines
44 KiB
C
1884 lines
44 KiB
C
|
||
#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)
|
||
{
|
||
}
|
||
|
||
//----------------------------------------------------------------------------------------------------
|
||
//----------------------------------------------------------------------------------------------------
|
||
|
||
// 紧急停止
|
||
int GetCommonEms(void)
|
||
{
|
||
int err = ERR_NONE;
|
||
|
||
TasksWhenDelay();
|
||
|
||
return err;
|
||
}
|
||
|
||
// 快速停止
|
||
int GetCommonQStop(void)
|
||
{
|
||
int err = ERR_NONE;
|
||
|
||
return err;
|
||
}
|
||
|
||
// 普通停止
|
||
int GetCommonNStop(void)
|
||
{
|
||
int err = ERR_NONE;
|
||
|
||
#if (0)
|
||
if (err == ERR_NONE &&
|
||
IsConsoleCancel() != 0 && // 终端输入停止
|
||
1 )
|
||
{
|
||
printf("get console cancel\r\n");
|
||
err = STA_NORMAL_STOP;
|
||
}
|
||
#endif
|
||
|
||
#if (1)
|
||
if (err == ERR_NONE &&
|
||
IsStopButton() == SENSOR_ON && // 检测暂停按钮按下
|
||
1 )
|
||
{
|
||
printf("get stop button down\r\n");
|
||
err = STA_NORMAL_STOP;
|
||
}
|
||
#endif
|
||
|
||
return err;
|
||
}
|
||
|
||
|
||
//----------------------------------------------------------------------------------------------------
|
||
|
||
// 周期性运动的零位检测（归零）
|
||
#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)
|
||
|
||
|
||
// 急停控制
|
||
int RSensorEmsScan(u32 para1, u32 para2)
|
||
{
|
||
int rslt = GetCommonEms();
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 快速停止控制
|
||
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) // 如果没有经过传感器位置
|
||
{
|
||
pchk->cansflag = 0;
|
||
if (pchk->sensorscan != NULL)
|
||
{
|
||
if (pchk->sensorscan() == pchk->val) // 检测到传感器位置
|
||
{
|
||
// printf("1. first in sensor area when run\r\n");
|
||
pchk->zeroflag = 1; // 设置经过传感器标志
|
||
pchk->canscount = g_motosPara.motosPos[pchk->axisIdx];
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (pchk->cansflag == 0) // 如果没有跳过传感器位置
|
||
{
|
||
if (pchk->sensorscan != NULL)
|
||
{
|
||
if (pchk->sensorscan() != pchk->val) // 检测非传感器位置
|
||
{
|
||
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) // 检测传感器位置
|
||
{
|
||
// printf("1. in sensor area when run\r\n");
|
||
rslt = STA_NORMAL_STOP;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 普通停止控制
|
||
int RSensorNStopScan(u32 para1, u32 para2)
|
||
{
|
||
int rslt;
|
||
|
||
rslt = GetCommonNStop();
|
||
if (rslt != ERR_NONE)
|
||
{
|
||
return rslt;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
//------------------------------------------------------------------
|
||
|
||
// 控制流程,运动到传感器
|
||
int RoundRunToSensor(RSenserCheck * pck)
|
||
{
|
||
return RoundRunToSensorBase(pck, 0);
|
||
}
|
||
|
||
// 运动到传感器边缘
|
||
int RoundRunToSensorEdge(RSenserCheck * pck)
|
||
{
|
||
return RoundRunToSensorBase(pck, 1);
|
||
}
|
||
|
||
// 运动到传感器中心
|
||
int RoundRunToSensorCenter(RSenserCheck * pck)
|
||
{
|
||
return RoundRunToSensorBase(pck, 2);
|
||
}
|
||
|
||
// 控制流程
|
||
// flag,找零成功后转动到哪个位置 0,无动作 1,转动到边缘位置 2,转动到中心位置
|
||
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; // 使用虚轴
|
||
lmctrl.motosConfig.newConfig = pck->newCfg; // 需要更新电机配置
|
||
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; // 实轴输出配置
|
||
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pck->axisOtp; // 脉冲输出模式(CW/CCW，PULSE/DIR)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
|
||
lmctrl.mvmtPara.movement[axisIdx] = ADJ_RUN_MVMT(pck->circleNum); // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
|
||
interLong = abs(ADJ_RUN_MVMT(pck->circleNum));
|
||
lmctrl.mvmtPara.extraRepeat = 0; // 延续重复次数
|
||
lmctrl.mvmtPara.interLong = interLong; // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
lmctrl.mvmtPara.pulsePerSegment = abs(pck->circleNum); // 每段脉冲数
|
||
|
||
// 速度控制参数
|
||
lmctrl.spdPara.startPPS = pck->rspd.stspSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pck->rspd.stspSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pck->rspd.runSpd; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pck->rspd.runSpd; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pck->rspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pck->rspd.spdAdd; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd; // 刹车加速度
|
||
|
||
lmctrl.stopMode = STOP_MODE_SPD; // 停止运动方式
|
||
|
||
// 回调函数及参数
|
||
lmctrl.Delay = DelayRef; // 延时回调函数
|
||
lmctrl.RefreshPosition = NormalRefPosition; // 坐标刷新回调函数
|
||
lmctrl.refPosPara = 0; // 坐标刷新参数
|
||
lmctrl.WorkBeforeRun = NULL; // 运行前工作回调函数
|
||
lmctrl.WorkAfterRun = NULL; // 运行后工作回调函数
|
||
lmctrl.workPara1 = 0;
|
||
lmctrl.workPara2 = 0; // 工作参数
|
||
lmctrl.GetSlowDown = NULL; // 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
|
||
lmctrl.GetNormalStop = RSensorNStopScan; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = RSensorQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
lmctrl.GetEmergencyStop = RSensorEmsScan; // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
|
||
lmctrl.condPara1 = (u32)(pck);;
|
||
lmctrl.condPara2 = 0; // 条件函数参数
|
||
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
|
||
if (rslt == 3) // 条件停车
|
||
{
|
||
DelayRef(100); // 等待机械稳定
|
||
|
||
// 重新检查传感器位置
|
||
if (pck->sensorscan != NULL)
|
||
{
|
||
if (pck->sensorscan() != pck->val) // 如果传感器不在位置，那么反向慢速转动到位置
|
||
{
|
||
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); // 运动控制
|
||
}
|
||
}
|
||
}
|
||
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
#if (1)
|
||
if (flag != 0)
|
||
{
|
||
// 摆动找到中心位置
|
||
if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
|
||
pck->sensorscan() == pck->val && // 在传感器位置
|
||
1 )
|
||
{
|
||
// printf("move to edge\r\n");
|
||
int gapcount, pcount;
|
||
|
||
// 取反
|
||
if (pck->val == SENSOR_ON)
|
||
{
|
||
pck->val = SENSOR_OFF;
|
||
}
|
||
else
|
||
{
|
||
pck->val = SENSOR_ON;
|
||
}
|
||
|
||
// 速度控制参数,使用慢速速度
|
||
lmctrl.spdPara.startPPS = pck->rspd.stspSpd/2; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pck->rspd.stspSpd/2; // 停止速度
|
||
lmctrl.spdPara.runPPS = pck->rspd.runSpd/2; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pck->rspd.runSpd/2; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pck->rspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pck->rspd.spdAdd; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd; // 刹车加速度
|
||
|
||
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); // 运动控制, 运动到边缘
|
||
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); // 读取位置
|
||
|
||
// 正向
|
||
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); // 运动控制, 运动到另外的边缘
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
|
||
pck->sensorscan() == pck->val &&
|
||
1 )
|
||
{
|
||
gapcount = abs(GetMotoCounter(axisIdx) - pcount); // 读取位置
|
||
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; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = NULL; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
lmctrl.stopMode = STOP_MODE_TAB; // 停止运动方式
|
||
|
||
lmctrl.spdPara.startPPS = pck->rspd.runSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pck->rspd.runSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pck->rspd.runSpd; // 运行速度
|
||
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
}
|
||
#endif
|
||
|
||
return err;
|
||
}
|
||
|
||
//------------------------------------------------------------------
|
||
|
||
// 急停控制
|
||
int RMotoRunEmsScan(u32 para1, u32 para2)
|
||
{
|
||
RMotoRun * pmr;
|
||
int rslt = GetCommonEms();
|
||
pmr = (RMotoRun *)para1;
|
||
|
||
if (pmr->RRunProc != NULL)
|
||
{
|
||
pmr->RRunProc();
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 快速停止控制
|
||
int RMotoRunQStopScan(u32 para1, u32 para2)
|
||
{
|
||
int rslt;
|
||
|
||
rslt = GetCommonQStop();
|
||
if (rslt != ERR_NONE)
|
||
{
|
||
return rslt;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 普通停止控制
|
||
int RMotoRunNStopScan(u32 para1, u32 para2)
|
||
{
|
||
int rslt;
|
||
|
||
rslt = GetCommonNStop();
|
||
if (rslt != ERR_NONE)
|
||
{
|
||
return rslt;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 按偏移量运动
|
||
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; // 使用虚轴
|
||
lmctrl.motosConfig.newConfig = pmr->newCfg; // 需要更新电机配置
|
||
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; // 实轴输出配置
|
||
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmr->config[i].poutType; // 脉冲输出模式(CW/CCW，PULSE/DIR)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmr->config[i].movement; // 各个轴的运动量，0，无运动；其他，运动长度，单位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) // 无运动量
|
||
{
|
||
return ERR_NONE;
|
||
}
|
||
|
||
lmctrl.mvmtPara.extraRepeat = pmr->mrepeat; // 延续重复次数
|
||
lmctrl.mvmtPara.interLong = pmr->interLong; // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
lmctrl.mvmtPara.pulsePerSegment = pmr->segmentNum; // 每段脉冲数
|
||
|
||
// 速度控制参数
|
||
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; // 停止加速度
|
||
|
||
lmctrl.stopMode = STOP_MODE_TAB; // 停止运动方式
|
||
|
||
// 回调函数及参数
|
||
lmctrl.Delay = DelayRef; // 延时回调函数
|
||
lmctrl.RefreshPosition = NormalRefPosition; // 坐标刷新回调函数
|
||
lmctrl.refPosPara = 0; // 坐标刷新参数
|
||
lmctrl.WorkBeforeRun = NULL; // 运行前工作回调函数
|
||
lmctrl.WorkAfterRun = NULL; // 运行后工作回调函数
|
||
lmctrl.workPara1 = 0;
|
||
lmctrl.workPara2 = 0; // 工作参数
|
||
lmctrl.GetSlowDown = NULL; // 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
|
||
|
||
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; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = RMotoRunQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
}
|
||
|
||
lmctrl.GetEmergencyStop = RMotoRunEmsScan; // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
|
||
lmctrl.condPara1 = (u32)(pmr);;
|
||
lmctrl.condPara2 = 0; // 条件函数参数
|
||
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
return err;
|
||
}
|
||
|
||
#endif
|
||
|
||
//----------------------------------------------------------------------------------------------------
|
||
|
||
#if (1)
|
||
|
||
// 急停检测
|
||
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) // 检测正向限位
|
||
{
|
||
if (pmts->limitpsensor() == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_POSITIVE; // 正向限位
|
||
}
|
||
}
|
||
}
|
||
else if (pmts->movement < 0)
|
||
{
|
||
if (pmts->limitnsensor != NULL) // 检测反向限位
|
||
{
|
||
if (pmts->limitnsensor() == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_NEGATIVE; // 反向限位
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = ERR_MTZ_ERROR; // 归零错误
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 普通停止检测
|
||
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;
|
||
}
|
||
|
||
// 快速停止检测
|
||
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) // 检测零位
|
||
{
|
||
if (pmts->zerosensor() == pmts->zeroval)
|
||
{
|
||
rslt = STA_MTZ_SUCCESS; // 归零成功
|
||
}
|
||
}
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 降速检测
|
||
int MoveToSensorSlowScan(u32 para1, u32 para2)
|
||
{
|
||
int rslt;
|
||
MTSCtrl * pmts;
|
||
|
||
pmts = (MTSCtrl*)(para1);
|
||
rslt = 0;
|
||
|
||
if (pmts != NULL)
|
||
{
|
||
if (pmts->slowsensor != NULL) // 检测降速
|
||
{
|
||
if (pmts->slowsensor() == SENSOR_ON)
|
||
{
|
||
pmts->slowsta = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
rslt = pmts->slowsta; // 降速状态
|
||
return rslt;
|
||
}
|
||
|
||
//------------------------------------------------
|
||
// 直线运动到传感器
|
||
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; // 使用虚轴
|
||
lmctrl.motosConfig.newConfig = pmtsc->newCfg; // 需要更新电机配置
|
||
|
||
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; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsc->axisCfg; // 实轴输出配置
|
||
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsc->poutType; // 脉冲输出模式(CW/CCW，PULSE/DIR)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmtsc->movement; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
|
||
lmctrl.mvmtPara.extraRepeat = 0; // 延续重复次数
|
||
lmctrl.mvmtPara.interLong = abs(pmtsc->movement); // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT; // 每段脉冲数
|
||
|
||
// 速度控制参数
|
||
lmctrl.spdPara.startPPS = pmtsc->lspd.stspSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pmtsc->lspd.stspSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pmtsc->lspd.runSpd; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pmtsc->lspd.stspSpd; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pmtsc->lspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pmtsc->lspd.spdAdd/2; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pmtsc->lspd.brkAdd; // 刹车加速度
|
||
|
||
lmctrl.stopMode = STOP_MODE_SPD; // 停止运动方式
|
||
|
||
// 回调函数及参数
|
||
lmctrl.Delay = DelayRef; // 延时回调函数
|
||
lmctrl.RefreshPosition = NormalRefPosition; // 坐标刷新回调函数
|
||
lmctrl.refPosPara = 0; // 坐标刷新参数
|
||
lmctrl.WorkBeforeRun = NULL; // 运行工作回调函数
|
||
lmctrl.WorkAfterRun = NULL; // 运行后工作回调函数
|
||
lmctrl.workPara1 = 0;
|
||
lmctrl.workPara2 = 0; // 工作参数
|
||
lmctrl.GetSlowDown = MoveToSensorSlowScan; // 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
|
||
lmctrl.GetNormalStop = MoveToSensorNStopScan; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = MoveToSensorQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
lmctrl.GetEmergencyStop = MoveToSensorEmsScan; // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
|
||
lmctrl.condPara1 = (u32)(pmtsc);
|
||
lmctrl.condPara2 = 0; // 条件函数参数
|
||
|
||
// 检测条件
|
||
err = ERR_NONE;
|
||
poismv = 0;
|
||
|
||
#if (1)
|
||
if (pmtsc->syncflag == 0)
|
||
{
|
||
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // 检测急停条件
|
||
if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
|
||
(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE) ) // 在限位
|
||
{
|
||
poismv = pmtsc->mvposi; // 反向运动
|
||
}
|
||
|
||
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // 检测停止条件
|
||
if (err == STA_MTZ_SUCCESS) // 在零位位置
|
||
{
|
||
poismv = pmtsc->mvposi; // 反向运动
|
||
}
|
||
else if (err == ERR_NONE)
|
||
{
|
||
rslt = MoveToSensorSlowScan(lmctrl.condPara1, lmctrl.condPara2); // 在降速区域
|
||
if (rslt != ERR_NONE)
|
||
{
|
||
poismv = pmtsc->mvposi; // 反向运动
|
||
}
|
||
}
|
||
else
|
||
{
|
||
}
|
||
}
|
||
#endif
|
||
|
||
do
|
||
{
|
||
// 运动
|
||
if (poismv != 0)
|
||
{
|
||
if (pmtsc->moveasoffset != NULL)
|
||
{
|
||
err = pmtsc->moveasoffset(poismv, &(pmtsc->lspd)); // 运动一段距离
|
||
}
|
||
|
||
if (err == ERR_NONE)
|
||
{
|
||
// 重新检测
|
||
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // 检测急停条件
|
||
if (err == ERR_NONE)
|
||
{
|
||
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // 检测停止条件
|
||
if (err == ERR_NONE)
|
||
{
|
||
pmtsc->slowsta = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (err == ERR_NONE)
|
||
{
|
||
s32 temp, val;
|
||
temp = GetMotoCounter(axisIdx); // 电机位置
|
||
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
|
||
if (poismv == 0 && (rslt == 3 || rslt == 2))
|
||
{
|
||
if (rslt == 3)
|
||
{
|
||
err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2); // 检测停止条件
|
||
if (err == STA_MTZ_SUCCESS && pmtsc->mvposi != 0)
|
||
{
|
||
val = GetMotoCounter(axisIdx);
|
||
if (abs(val-temp) < abs(pmtsc->mvposi/2)) // 如果归零行走位移太小
|
||
{
|
||
poismv = pmtsc->mvposi; // 反向运动
|
||
continue; // 重新执行一次归零
|
||
}
|
||
}
|
||
}
|
||
else if (rslt == 2)
|
||
{
|
||
err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2); // 检测急停条件
|
||
if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
|
||
(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE) ) // 在限位
|
||
{
|
||
poismv = pmtsc->mvposi; // 反向运动
|
||
continue; // 重新执行一次归零
|
||
}
|
||
}
|
||
}
|
||
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
}
|
||
|
||
break;
|
||
}while(1);
|
||
|
||
return err;
|
||
}
|
||
|
||
//------------------------------------------------
|
||
|
||
#endif
|
||
|
||
//----------------------------------------------------------------------------------------------------
|
||
|
||
#if (1)
|
||
|
||
// 急停检测
|
||
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) // 检测正向限位
|
||
{
|
||
if (pmbhc->limitpsensor(pmbhc->para) == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_POSITIVE; // 正向限位
|
||
}
|
||
}
|
||
}
|
||
else if (pmbhc->movement < 0)
|
||
{
|
||
if (pmbhc->limitnsensor != NULL) // 检测反向限位
|
||
{
|
||
if (pmbhc->limitnsensor(pmbhc->para) == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_NEGATIVE; // 反向限位
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = ERR_MTZ_ERROR; // 归零错误
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 普通停止检测
|
||
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;
|
||
}
|
||
|
||
// 慢速检测
|
||
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;
|
||
}
|
||
|
||
// 快速停止检测
|
||
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;
|
||
}
|
||
|
||
// 手动移动控制
|
||
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;
|
||
}
|
||
|
||
// 先决条件和准备工作
|
||
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; // 使用虚轴
|
||
|
||
lmctrl.motosConfig.newConfig = pmbhc->newCfg; // 需要更新电机配置
|
||
|
||
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; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmbhc->axisCfg; // 实轴输出配置
|
||
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmbhc->poutType; // 脉冲输出模式(CW/CCW，PULSE/DIR)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmbhc->movement; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
|
||
lmctrl.mvmtPara.extraRepeat = 0; // 延续重复次数
|
||
lmctrl.mvmtPara.interLong = abs(pmbhc->movement); // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
lmctrl.mvmtPara.pulsePerSegment = pmbhc->ppSegment; // 每段脉冲数
|
||
|
||
// 速度控制参数
|
||
lmctrl.spdPara.startPPS = pmbhc->hspd.stspSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pmbhc->hspd.stspSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pmbhc->hspd.runSpd; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pmbhc->hspd.stspSpd; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pmbhc->hspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pmbhc->hspd.spdAdd/2; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pmbhc->hspd.brkAdd; // 刹车加速度
|
||
|
||
lmctrl.stopMode = STOP_MODE_SPD; // 停止运动方式
|
||
|
||
// 回调函数及参数
|
||
lmctrl.Delay = DelayRef; // 延时回调函数
|
||
lmctrl.RefreshPosition = NormalRefPosition; // 坐标刷新回调函数
|
||
lmctrl.refPosPara = 0; // 坐标刷新参数
|
||
lmctrl.WorkBeforeRun = NULL; // 运行前工作回调函数
|
||
lmctrl.WorkAfterRun = NULL; // 运行后工作回调函数
|
||
lmctrl.workPara1 = 0;
|
||
lmctrl.workPara2 = 0; // 工作参数
|
||
lmctrl.GetSlowDown = MoveBHNSlowScan; // 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
|
||
lmctrl.GetNormalStop = MoveBHNStopScan; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = MoveBHQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
lmctrl.GetEmergencyStop = MoveBHEmsScan; // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
|
||
lmctrl.condPara1 = (u32)(pmbhc);;
|
||
lmctrl.condPara2 = (u32)(pmbhc->movement<0?-1:1); // 条件函数参数
|
||
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
return err;
|
||
}
|
||
|
||
#endif
|
||
|
||
//------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
// 单轴异步运动
|
||
// 使用STM32计算速度&发送脉冲
|
||
int SeparateExerStart(SepMoveCtrl * pCtrl) // 单轴异步运动 启动
|
||
{
|
||
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; // 位移量
|
||
|
||
lmctrl.startPPS = pCtrl->startPPS; // 启动速度
|
||
lmctrl.runPPS = pCtrl->runPPS; // 运动速度
|
||
lmctrl.addPPSG = pCtrl->addPPSG; // 启停加速度
|
||
lmctrl.brkPPSG = pCtrl->brkPPSG; // 刹车加速度
|
||
|
||
lmctrl.funTestTime = pCtrl->funTestTime; // 运行过程中外部函数检测间隔时间
|
||
for (i = 0; i < 6; i++)
|
||
{
|
||
lmctrl.syncList[i] = pCtrl->syncList[i]; // 同步移动选择
|
||
}
|
||
|
||
lmctrl.GetNormalStop = pCtrl->GetNormalStop; // 一般停止条件函数
|
||
lmctrl.GetQuickStop = pCtrl->GetQuickStop; // 快速停止条件函数
|
||
lmctrl.condPara1 = pCtrl->condPara1; // 条件函数参数
|
||
lmctrl.condPara2 = pCtrl->condPara2; // 条件函数参数
|
||
|
||
lmctrl.blockRunflag = pCtrl->blockRunflag; // 阻塞运行标志
|
||
lmctrl.ExecWhenRun = pCtrl->ExecWhenRun; // 电机运行过程中执行的函数
|
||
lmctrl.ExecWhenStart = pCtrl->ExecWhenStart; // 电机启动前执行的函数
|
||
lmctrl.ExecWhenStop = pCtrl->ExecWhenStop; // 电机停止后执行的函数
|
||
|
||
axisID = axisIdx + 1;
|
||
if (pCtrl->newCfg != 0)
|
||
{// 重新配置
|
||
if ((pCtrl->axisCfg & CONFIG_EN) != 0) // 配置允许
|
||
{
|
||
if ((pCtrl->axisCfg & POA_EN) != 0) // A允许输出
|
||
{
|
||
SetAxisConfig(axisID, POUTA_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUTA_DIS);
|
||
}
|
||
|
||
if ((pCtrl->axisCfg & POB_EN) != 0) // B允许输出
|
||
{
|
||
SetAxisConfig(axisID, POUTB_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUTB_DIS);
|
||
}
|
||
|
||
if ((pCtrl->axisCfg & SON_EN) != 0) // 伺服ON允许
|
||
{
|
||
SetAxisConfig(axisID, SERVO_ON);
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, SERVO_OFF);
|
||
}
|
||
|
||
if ((pCtrl->axisCfg & ALM_LEVH) != 0) // 报警电平为高
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
|
||
}
|
||
else
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
|
||
}
|
||
|
||
if ((pCtrl->axisCfg & ALM_EN) != 0) // 报警停车允许
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_STOP_EN);
|
||
}
|
||
else
|
||
{
|
||
SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
|
||
}
|
||
|
||
if ((pCtrl->axisCfg & POUT_EN) != 0) // 输出允许
|
||
{
|
||
SetAxisConfig(axisID, pCtrl->poutType); // 脉冲输出模式 脉冲方式输出
|
||
}
|
||
else
|
||
{
|
||
SetAxisConfig(axisID, POUT_DISABLE); // 禁止脉冲输出
|
||
}
|
||
}
|
||
}
|
||
#if (STM32_MOTOS_NUM >= 1)
|
||
if (((axisID == 1) || (lmctrl.syncList[1-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(1, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(1, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto1Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#endif
|
||
#if (STM32_MOTOS_NUM >= 2)
|
||
if (((axisID == 2) || (lmctrl.syncList[2-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(2, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(2, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto2Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#endif
|
||
#if (STM32_MOTOS_NUM >= 3)
|
||
if (((axisID == 3) || (lmctrl.syncList[3-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(3, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(3, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto3Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#endif
|
||
#if (STM32_MOTOS_NUM >= 4)
|
||
if (((axisID == 4) || (lmctrl.syncList[4-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(4, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(4, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto4Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#endif
|
||
#if (STM32_MOTOS_NUM >= 5)
|
||
if (((axisID == 5) || (lmctrl.syncList[5-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(5, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(5, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto5Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#endif
|
||
#if (STM32_MOTOS_NUM >= 6)
|
||
if (((axisID == 6) || (lmctrl.syncList[6-1] == 1)) && (pCtrl->useOutPort == 0))
|
||
{
|
||
SetAxisConfig(6, POUT_ENABLE_BUSIO); // 脉冲输出允许:输入为内部总线直接控制模式
|
||
SetAxisConfig(6, DATI_BUS_IO); // 内部总线直接控制模式
|
||
SetSTM32Moto6Mode(MODE_STEPDIR); // 脉冲模式为方向/脉冲形式
|
||
}
|
||
#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; // 参数错误
|
||
break;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
//------------------------------------------------
|
||
// 单轴异步运动 停止
|
||
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; // 参数错误
|
||
break;
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
//------------------------------------------------
|
||
int GetSeparateExerState(u16 axisIdx) // 单轴异步运动 得到当前状态
|
||
{
|
||
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; // 参数错误
|
||
break;
|
||
}
|
||
|
||
if (rslt == STM32MOTO_STOP)
|
||
{
|
||
err = STA_PROCESS_FINISH; // 执行完成
|
||
}
|
||
else
|
||
{
|
||
//err = STA_PROCESS_RUNNING; // 执行过程中
|
||
err = rslt; // 返回运行状态
|
||
}
|
||
|
||
return err;
|
||
}
|
||
|
||
//------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
// 转化为 -180--180度之间的数
|
||
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);
|
||
}
|
||
}
|
||
|
||
// 转化为 -360--360度之间的数
|
||
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);
|
||
}
|
||
}
|
||
|
||
// 转化为 0--360度之间的数
|
||
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);
|
||
}
|
||
}
|
||
|
||
// 转化为 -360--0度之间的数
|
||
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: // 参数错误
|
||
err = ERR_MV_PARA;
|
||
break;
|
||
case -2: // 填充数据错误
|
||
err = ERR_FILL_DATA;
|
||
break;
|
||
case -3: // FPGA读写错误
|
||
err = ERR_FPGA_RESET; // FPGA复位错误
|
||
break;
|
||
case -4: // FPGA版本错误
|
||
err = ERR_FPGA_ERR; // 运动控制芯片版本错误
|
||
break;
|
||
case 1: // 驱动器报警
|
||
err = ERR_DRIVER;
|
||
break;
|
||
case 0: // 运动执行完成
|
||
case 2: // 紧急停止条件触发停车
|
||
case 3: // 快速停止条件触发停车
|
||
case 4: // 普通停车条件触发停车
|
||
err = errinfo;
|
||
break;
|
||
case 5:
|
||
err = ERR_FPGA_RESET;
|
||
break;
|
||
default: // 正常结束或其他
|
||
err = ERR_NONE;
|
||
break;
|
||
}
|
||
return err;
|
||
}
|
||
|
||
//---------------------------------------------------
|
||
|
||
// 主轴角度转化
|
||
// 如果pdir == 0, 转化为 -180--180度之间的数,
|
||
// 如果pdir != 0 那么转化为 0-- 360之间的数
|
||
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) // 正向转动
|
||
{
|
||
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);
|
||
}
|
||
}
|
||
|
||
//--------------------------------------------------------------------------------
|
||
// 普通停止检测
|
||
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;
|
||
}
|
||
|
||
// 快速停止检测
|
||
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) // 检测零位
|
||
{
|
||
if (pmts->zerosensor() == pmts->zeroval)
|
||
{
|
||
rslt = STA_MTZ_SUCCESS; // 归零成功
|
||
}
|
||
}
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 急停检测
|
||
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) // 检测正向限位
|
||
{
|
||
if (pmts->limitpsensor() == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_POSITIVE; // 正向限位
|
||
}
|
||
}
|
||
}
|
||
else if (pmts->movement < 0)
|
||
{
|
||
if (pmts->limitnsensor != NULL) // 检测反向限位
|
||
{
|
||
if (pmts->limitnsensor() == SENSOR_ON)
|
||
{
|
||
rslt = ERR_LMT_NEGATIVE; // 反向限位
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
rslt = ERR_MTZ_ERROR; // 归零错误
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 找挡片中心位置停止检测
|
||
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) // 检测零位
|
||
{
|
||
if (isckzero == 1)
|
||
{
|
||
if (pmts->zerosensor() != pmts->zeroval)
|
||
{
|
||
isckzero = 0;
|
||
rslt = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (pmts->zerosensor() == pmts->zeroval)
|
||
{
|
||
isckzero = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return rslt;
|
||
}
|
||
|
||
// 直线运动到零位传感器且挡片在中心位置,特别说明,此种归零方式不能使用带有豁口的挡片,尤其是豁口不在中心位置
|
||
// return -1:参数错误
|
||
int LineRunToSensorAsMidPosCtrl(MTSAMPCtrl * pmtsampc)
|
||
{
|
||
int rslt, err;
|
||
InterMoveCtrl lmctrl;
|
||
u16 axisIdx;
|
||
int mvnum = 0; // 运动次数,最多运动两次,即正反限位各一次,若运动到正反限位还没有找到零位传感器,则归零失败
|
||
|
||
if (pmtsampc == NULL)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
axisIdx = pmtsampc->axisIdx;
|
||
assert_param(axisIdx < AXIS_NUM);
|
||
|
||
memset(&lmctrl, 0, sizeof(InterMoveCtrl));
|
||
|
||
lmctrl.vAxisId = pmtsampc->vAxisId; // 使用虚轴
|
||
lmctrl.motosConfig.newConfig = pmtsampc->newCfg; // 需要更新电机配置
|
||
|
||
err = ERR_NONE;
|
||
|
||
#if (1)
|
||
//if (pmtsampc->syncflag == 0)
|
||
{
|
||
err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2); // 检测急停条件
|
||
if ((pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
|
||
(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE) ) // 在限位
|
||
{
|
||
mvnum++;
|
||
pmtsampc->movement = pmtsampc->movement * (-1); // 在限位则向反向运动找零
|
||
}
|
||
|
||
err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2); // 检测停止条件
|
||
if (err == STA_MTZ_SUCCESS) // 在零位位置
|
||
{
|
||
pmtsampc->movement = 0; // 在零位则跳过找零位传感器过程,直接慢速移动找挡片中心位置
|
||
}
|
||
}
|
||
#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; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsampc->axisCfg; // 实轴输出配置
|
||
lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsampc->poutType; // 脉冲输出模式(CW/CCW，PULSE/DIR)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId); // 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
|
||
lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION; // 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
|
||
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement; // 各个轴的运动量，0，无运动；其他，运动长度，单位p
|
||
}
|
||
|
||
lmctrl.mvmtPara.extraRepeat = 0; // 延续重复次数
|
||
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement); // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT; // 每段脉冲数
|
||
|
||
// 速度控制参数
|
||
lmctrl.spdPara.startPPS = pmtsampc->lspd.stspSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pmtsampc->lspd.stspSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pmtsampc->lspd.runSpd; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pmtsampc->lspd.stspSpd; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pmtsampc->lspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pmtsampc->lspd.spdAdd/2; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pmtsampc->lspd.brkAdd; // 刹车加速度
|
||
|
||
lmctrl.stopMode = STOP_MODE_SPD; // 停止运动方式
|
||
|
||
// 回调函数及参数
|
||
lmctrl.Delay = DelayRef; // 延时回调函数
|
||
lmctrl.RefreshPosition = NormalRefPosition; // 坐标刷新回调函数
|
||
lmctrl.refPosPara = 0; // 坐标刷新参数
|
||
lmctrl.WorkBeforeRun = NULL; // 运行工作回调函数
|
||
lmctrl.WorkAfterRun = NULL; // 运行后工作回调函数
|
||
lmctrl.workPara1 = 0;
|
||
lmctrl.workPara2 = 0; // 工作参数
|
||
lmctrl.GetSlowDown = NULL; // 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
|
||
lmctrl.GetNormalStop = MoveToSensorAsMidPosNStopScan; // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
|
||
lmctrl.GetQuickStop = MoveToSensorAsMidPosQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
lmctrl.GetEmergencyStop = MoveToSensorAsMidPosEmsScan; // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
|
||
lmctrl.condPara1 = (u32)(pmtsampc);;
|
||
lmctrl.condPara2 = 0; // 条件函数参数
|
||
|
||
// 运动到零位传感器
|
||
while(mvnum < 2 && pmtsampc->movement != 0)
|
||
{
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
|
||
if (rslt == 3)
|
||
{
|
||
err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2); // 检测停止条件
|
||
break;
|
||
}
|
||
else if (rslt == 2)
|
||
{
|
||
err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2); // 检测急停条件
|
||
if ( (pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
|
||
(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE) || // 在限位
|
||
0 )
|
||
{
|
||
mvnum++;
|
||
pmtsampc->movement = pmtsampc->movement * (-1); // 反向运动
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (mvnum >= 2) // 正负限位都运动还未找到零位,归零错误
|
||
{
|
||
err = ERR_MTZ_ERROR;
|
||
return err;
|
||
}
|
||
|
||
if (err == STA_MTZ_SUCCESS) // 找零成功,开始移动到挡片中心位置
|
||
{
|
||
s32 pos1, pos2; // 挡片两侧位置记录
|
||
|
||
pmtsampc->movement = pmtsampc->bafflewidth + 1000;
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
|
||
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement); // 插补长轴位移，必须大于或等于movement分量中的最大值
|
||
|
||
lmctrl.spdPara.startPPS = pmtsampc->mposspd.stspSpd; // 启动速度
|
||
lmctrl.spdPara.stopPPS = pmtsampc->mposspd.stspSpd; // 停止速度
|
||
lmctrl.spdPara.runPPS = pmtsampc->mposspd.runSpd; // 运行速度
|
||
lmctrl.spdPara.slowPPS = pmtsampc->mposspd.stspSpd; // 降速速度
|
||
lmctrl.spdPara.addPPSG = pmtsampc->mposspd.spdAdd; // 升速加速度
|
||
lmctrl.spdPara.decPPSG = pmtsampc->mposspd.spdAdd/2; // 降速加速度
|
||
lmctrl.spdPara.brkPPSG = pmtsampc->mposspd.brkAdd; // 刹车加速度
|
||
|
||
lmctrl.GetQuickStop = MoveToMidPosQStopScan; // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
|
||
|
||
err = ERR_NONE;
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
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); // 运动控制
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
if (err == 1)
|
||
{
|
||
int dir;
|
||
pos2 = GetMotoCounter(axisIdx);
|
||
if (pmtsampc->movement > 0)
|
||
{
|
||
dir = -1; // 跟前一次运动方向相反
|
||
}
|
||
else
|
||
{
|
||
dir = 1;
|
||
}
|
||
pmtsampc->movement = abs(pos1-pos2)/2; // 20220317 BUG修复,单纯取绝对值电机只会单向运动,不会根据实际运动方向自切换
|
||
pmtsampc->movement *= dir;
|
||
lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
|
||
lmctrl.mvmtPara.interLong = abs(pmtsampc->movement);
|
||
lmctrl.GetQuickStop = NULL; // 20210812 因有些挡片带有豁口(带降速传感器时,实际此种归零方式完全不需要降速传感器)
|
||
// 当找零运动到豁口时,个别情况下会存在传感器响应不正常的情况
|
||
// 故此处移动到挡片中心位置时不再检测此停止条件
|
||
|
||
err = ERR_NONE;
|
||
rslt = InterpolationMotion(&lmctrl); // 运动控制
|
||
err = GetMoveResultError(rslt, lmctrl.errInfo);
|
||
|
||
if (err == ERR_NONE)
|
||
{
|
||
err = STA_MTZ_SUCCESS;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return err;
|
||
}
|
||
|
||
|