487 lines
11 KiB
C
487 lines
11 KiB
C
|
|
|
|
#include "drivermotion.h"
|
|
|
|
#if (DEV_MOTIONS_NUM > 0)
|
|
|
|
//----------------------------------------------------------------
|
|
|
|
// 积分计算的运动控制的计算
|
|
|
|
#ifndef CALC_TIMES_PER_SEC
|
|
#define CALC_TIMES_PER_SEC (10000) // 每秒钟计算次数
|
|
#endif
|
|
|
|
#ifndef USE_FLOAT_CALC
|
|
#define USE_FLOAT_CALC 1 // 0, 整数运算, 1, 浮点数运算
|
|
#endif
|
|
|
|
#if (USE_FLOAT_CALC == 1)
|
|
#define CALC_TYPE float
|
|
#define CALC_MUTI 1.0f
|
|
#else
|
|
|
|
#define CALC_TYPE s32
|
|
#define CALC_MUTI 10000
|
|
|
|
#endif
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
|
|
|
|
|
|
typedef struct
|
|
{
|
|
// 输入参数
|
|
CALC_TYPE startSpd; // 启动停止速度
|
|
CALC_TYPE runSpd; // 运动速度
|
|
CALC_TYPE runAdd; // 启停加速度
|
|
CALC_TYPE brkAdd; // 刹车加速度
|
|
|
|
u32 funTestTime; // 运行过程中外部函数检测间隔时间 0:不检测外部函数 (单位:0.1ms)
|
|
|
|
// 函数指针
|
|
int (*GetNormalStop)(u32,u32); // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按启停加速度降速）
|
|
int (*GetQuickStop)(u32,u32); // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度降速）
|
|
u32 condPara1, condPara2; // 条件函数参数
|
|
|
|
int blockRunflag; // 阻塞运行标志, =1阻塞运行,同步运动 =0,非阻塞,异步运动
|
|
void (*ExecWhenRun)(void); // 电机运行过程中执行的函数
|
|
void (*ExecWhenStart)(void); // 电机启动时执行的函数
|
|
void (*ExecWhenStop)(void); // 电机停止时执行的函数
|
|
void (*DelayWhenBlock)(u32); // 阻塞时的Delay函数
|
|
|
|
//---------------------
|
|
|
|
// 状态参数
|
|
int runState; // 运行状态
|
|
s32 realpos; // 绝对坐标
|
|
int startrunflag; // 启动运行标志
|
|
int stopflag; // 停止标志
|
|
|
|
//---------------------
|
|
|
|
// 计算控制变量
|
|
int rundir; // 运行方向
|
|
u32 totalNums; // 运动总脉冲数
|
|
u32 pulseCount; // 当前脉冲计数
|
|
u32 decPos; // 减速点位置
|
|
|
|
u32 funTestCount; // 运行过程中外部函数检测间隔时间计数器
|
|
|
|
CALC_TYPE currentSpd; // 当前速度
|
|
CALC_TYPE currentAcc; // 当前加速度
|
|
CALC_TYPE currentPos; // 当前位置
|
|
//---------------------
|
|
|
|
} DriverMotionCtrl;
|
|
|
|
DriverMotionCtrl g_drvMotionsCtrlList[DEV_MOTIONS_NUM];
|
|
|
|
//----------------------------------------------------------------
|
|
// 积分计算过程
|
|
|
|
void IntegrationMotionCalc(DriverMotionCtrl * pCtrl)
|
|
{
|
|
if (pCtrl == NULL ||
|
|
0 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (pCtrl->runState <= DRV_MOTION_STOP ||
|
|
pCtrl->startrunflag <= 0) // 停止状态
|
|
{
|
|
return; // 不执行
|
|
}
|
|
|
|
if (pCtrl->stopflag != 0)
|
|
{
|
|
if (pCtrl->stopflag == 1)
|
|
{
|
|
pCtrl->currentAcc = pCtrl->runAdd; // 当前加速度
|
|
pCtrl->runState = DRV_MOTION_NSTOP;
|
|
}
|
|
else
|
|
{
|
|
pCtrl->currentAcc = pCtrl->brkAdd; // 当前加速度
|
|
pCtrl->runState = DRV_MOTION_ESTOP;
|
|
}
|
|
}
|
|
|
|
if (pCtrl->funTestTime > 0) // 条件检测
|
|
{
|
|
pCtrl->funTestCount++; // 计数增加
|
|
if ((pCtrl->funTestCount >= pCtrl->funTestTime) && // 到了测试时间
|
|
1 )
|
|
{
|
|
pCtrl->funTestCount = 0;
|
|
if (pCtrl->GetNormalStop != NULL) // 一般停止条件
|
|
{
|
|
if ((pCtrl->runState == DRV_MOTION_ADD) || // 加速
|
|
(pCtrl->runState == DRV_MOTION_RUN) || // 匀速
|
|
(pCtrl->runState == DRV_MOTION_DEC) || // 计数降速
|
|
0 )
|
|
{
|
|
if (pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
|
|
{
|
|
pCtrl->currentAcc = pCtrl->runAdd; // 当前加速度
|
|
pCtrl->runState = DRV_MOTION_NSTOP; // 普通条件停车
|
|
}
|
|
}
|
|
}
|
|
if (pCtrl->GetQuickStop != NULL) // 快速停止条件
|
|
{
|
|
if ((pCtrl->runState == DRV_MOTION_ADD) || // 加速
|
|
(pCtrl->runState == DRV_MOTION_RUN) || // 匀速
|
|
(pCtrl->runState == DRV_MOTION_DEC) || // 计数降速
|
|
(pCtrl->runState == DRV_MOTION_ESTOP) || // 条件停车
|
|
0 )
|
|
{
|
|
if (pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
|
|
{
|
|
pCtrl->currentAcc = pCtrl->brkAdd; // 当前加速度
|
|
pCtrl->runState = DRV_MOTION_ESTOP; // 急停条件停车
|
|
}
|
|
}
|
|
}
|
|
if (pCtrl->ExecWhenRun != NULL) // 电机运行过程中执行的函数
|
|
{
|
|
pCtrl->ExecWhenRun();
|
|
}
|
|
}
|
|
}
|
|
|
|
// 加速中
|
|
if (pCtrl->runState == DRV_MOTION_ADD)
|
|
{
|
|
pCtrl->currentSpd += pCtrl->currentAcc; // 改变当前速度
|
|
if (pCtrl->currentSpd >= pCtrl->runSpd) // 当前速度大于运行速度,进入匀速状态
|
|
{
|
|
pCtrl->runState = DRV_MOTION_RUN;
|
|
pCtrl->currentSpd = pCtrl->runSpd;
|
|
pCtrl->decPos = (pCtrl->totalNums - pCtrl->pulseCount); // 减速点位移
|
|
}
|
|
}
|
|
|
|
// 减速中
|
|
if (pCtrl->runState == DRV_MOTION_DEC) // 计数降速
|
|
{
|
|
pCtrl->currentSpd -= pCtrl->currentAcc; // 当前速度
|
|
if (pCtrl->currentSpd <= pCtrl->startSpd)
|
|
{
|
|
pCtrl->currentSpd = pCtrl->startSpd;
|
|
}
|
|
}
|
|
|
|
// 条件停车中
|
|
if ((pCtrl->runState == DRV_MOTION_NSTOP) || // 条件停车
|
|
(pCtrl->runState == DRV_MOTION_ESTOP) || // 条件刹车
|
|
0 )
|
|
{
|
|
pCtrl->currentSpd -= pCtrl->currentAcc; // 当前速度
|
|
if (pCtrl->currentSpd <= pCtrl->startSpd) // 当前速度小于启动速度,进入停止状态
|
|
{
|
|
pCtrl->currentSpd = pCtrl->startSpd;
|
|
pCtrl->runState = DRV_MOTION_STOPING;
|
|
}
|
|
}
|
|
|
|
// 位置计算
|
|
{
|
|
pCtrl->currentPos += pCtrl->currentSpd * pCtrl->rundir; // 速度积分
|
|
if (pCtrl->currentPos > CALC_MUTI || pCtrl->currentPos < -CALC_MUTI) // 位置移出
|
|
{
|
|
int ofst = (int)(pCtrl->currentPos/CALC_MUTI); // 计算位置偏差
|
|
pCtrl->currentPos -= ofst*CALC_MUTI;
|
|
pCtrl->realpos += ofst; // 位置改变
|
|
pCtrl->pulseCount += abs(ofst); // 计数值增加
|
|
}
|
|
}
|
|
|
|
// 停止处理
|
|
{
|
|
if ((pCtrl->pulseCount >= pCtrl->totalNums) || // 发送脉冲完成
|
|
(pCtrl->runState <= DRV_MOTION_STOP) || // 停止信号
|
|
(pCtrl->runState == DRV_MOTION_STOPING) || // 停止中
|
|
0 )
|
|
{
|
|
if (pCtrl->runState > DRV_MOTION_STOP) // 运行状态
|
|
{
|
|
pCtrl->runState = DRV_MOTION_STOP; // 设置为关闭状态
|
|
pCtrl->startrunflag = 0; // 关闭运行标志
|
|
if (pCtrl->ExecWhenStop != NULL) // 执行关闭时的函数
|
|
{
|
|
pCtrl->ExecWhenStop();
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( (pCtrl->runState == DRV_MOTION_ADD) && // 加速状态中
|
|
(pCtrl->pulseCount >= (pCtrl->totalNums>>1)) && // 大于位移的一半
|
|
1 )
|
|
{
|
|
pCtrl->runState = DRV_MOTION_RUN;
|
|
pCtrl->currentAcc = pCtrl->runAdd; // 当前加速度
|
|
pCtrl->decPos = pCtrl->pulseCount;
|
|
#if (0)
|
|
#if (USE_FLOAT_CALC == 0)
|
|
printf("enter dec, spd=%d\r\n", pCtrl->currentSpd);
|
|
#else
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
if ( (pCtrl->runState == DRV_MOTION_RUN) && // 匀速中
|
|
(pCtrl->pulseCount >= pCtrl->decPos) && // 大于降速点
|
|
1 )
|
|
{
|
|
pCtrl->runState = DRV_MOTION_DEC;
|
|
pCtrl->currentAcc = pCtrl->runAdd; // 当前加速度
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
CALC_TYPE pps_to_ppg(s32 pps)
|
|
{
|
|
return pps*CALC_MUTI / CALC_TIMES_PER_SEC;
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
// 启动运动控制（停止状态时可用）
|
|
int DrvMotionStart(int driverIdx, DrvMotionPara * pPara)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
printf("DrvMotionStart fail, driverIdx error\r\n");
|
|
return -1;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
if (pPara == NULL)
|
|
{
|
|
printf("DrvMotionStart fail, pPara is NULL\r\n");
|
|
return -1;
|
|
}
|
|
|
|
if (pCtrl->runState <= DRV_MOTION_STOP) // 停车/报警状态允许启动电机
|
|
{
|
|
pCtrl->startrunflag = 0; // 关闭运行标志
|
|
|
|
if (pPara->movement == 0) // 电机运行完毕
|
|
{
|
|
pCtrl->runState = DRV_MOTION_STOP;
|
|
printf("driverIdx=%d, movement=0\r\n", driverIdx);
|
|
pCtrl->rundir = 0;
|
|
return 0;
|
|
}
|
|
|
|
if (pPara->startPPS < 10)
|
|
{
|
|
pPara->startPPS = 10;
|
|
}
|
|
if (pPara->runPPS < 10)
|
|
{
|
|
pPara->runPPS = 10;
|
|
}
|
|
if (pPara->startPPS > pPara->runPPS)
|
|
{
|
|
pPara->startPPS = pPara->runPPS;
|
|
}
|
|
if (pPara->addPPSS < 1)
|
|
{
|
|
pPara->addPPSS = 1;
|
|
}
|
|
|
|
// 输入速度参数计算
|
|
pCtrl->startSpd = pps_to_ppg(pPara->startPPS); // 启动停止速度
|
|
pCtrl->runSpd = pps_to_ppg(pPara->runPPS); // 运动速度
|
|
pCtrl->runAdd = pps_to_ppg(pPara->addPPSS); // 启停加速度
|
|
pCtrl->brkAdd = pps_to_ppg(pPara->brkPPSS); // 刹车加速度
|
|
|
|
pCtrl->currentSpd = pCtrl->startSpd; // 当前速度
|
|
pCtrl->currentAcc = pCtrl-> runAdd; // 当前加速度
|
|
pCtrl->currentPos = 1; // 当前位置, 先输出一个脉冲
|
|
pCtrl->runState = DRV_MOTION_ADD; // 运行状态, 加速中
|
|
|
|
pCtrl->funTestTime = pPara->funTestTime;
|
|
pCtrl->funTestCount = 0; // 运行过程中外部函数检测间隔时间计数器
|
|
|
|
// 函数指针
|
|
pCtrl->GetNormalStop = pPara->GetNormalStop; // 一般停止条件函数
|
|
pCtrl->GetQuickStop = pPara->GetQuickStop; // 快速停止条件函数
|
|
pCtrl->condPara1 = pPara->condPara1; // 条件函数参数
|
|
pCtrl->condPara2 = pPara->condPara2; // 条件函数参数
|
|
|
|
pCtrl->blockRunflag = pPara->blockRunflag; // 阻塞运行标志
|
|
pCtrl->ExecWhenStart = pPara->ExecWhenStart; // 启动时执行的函数
|
|
pCtrl->ExecWhenRun = pPara->ExecWhenRun; // 运行过程中执行的函数
|
|
pCtrl->ExecWhenStop = pPara->ExecWhenStop; // 停止时执行的函数
|
|
pCtrl->DelayWhenBlock = pPara->DelayWhenBlock; // 阻塞时执行的函数
|
|
|
|
if (pCtrl->ExecWhenStart != NULL) // 启动前执行
|
|
{
|
|
pCtrl->ExecWhenStart();
|
|
}
|
|
//---------------------
|
|
// 位移配置
|
|
pCtrl->rundir = (pPara->movement > 0) ? 1:-1; // 方向
|
|
pCtrl->totalNums = abs(pPara->movement); // 总位移
|
|
pCtrl->pulseCount = 0; // 脉冲计数
|
|
pCtrl->decPos = pCtrl->totalNums; // 减速位置
|
|
|
|
// 输出配置
|
|
pCtrl->stopflag = 0;
|
|
pCtrl->startrunflag = 1; // 设置启动运行标志
|
|
|
|
return DrvMotionRun(driverIdx);
|
|
}
|
|
else
|
|
{
|
|
printf("DrvMotionStart failed, drv%d is running\r\n", driverIdx);
|
|
return -2; // 电机正在运行
|
|
}
|
|
}
|
|
|
|
// 停止运动控制（非阻塞运动时用到）
|
|
void DrvMotionStop(int driverIdx)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
pCtrl->stopflag = 1;
|
|
}
|
|
|
|
// 停止运动控制（非阻塞运动时用到）
|
|
void DrvMotionBreak(int driverIdx)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
pCtrl->stopflag = 2;
|
|
}
|
|
|
|
|
|
int DrvMotionRun(int driverIdx)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
// 运行循环
|
|
do
|
|
{
|
|
if (pCtrl->runState <= DRV_MOTION_STOP)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (pCtrl->blockRunflag == 0) // 阻塞运行标志
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (pCtrl->DelayWhenBlock != NULL)
|
|
{
|
|
pCtrl->DelayWhenBlock(0);
|
|
}
|
|
|
|
}while(1);
|
|
|
|
return pCtrl->runState;
|
|
}
|
|
|
|
// 得到运动状态（非阻塞运动时用到）
|
|
int GetDrvMotionState(int driverIdx)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
return pCtrl->runState;
|
|
}
|
|
|
|
// 得到当前运动位置（非阻塞运动时用到）
|
|
s32 GetDrvMotionRealPos(int driverIdx)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
return pCtrl->realpos;
|
|
}
|
|
|
|
// 设置当前运动位置（停止状态时可用）
|
|
// 禁止应用层使用此代码,若需重置电机坐标,需使用ResetDriverPos
|
|
void SetDrvMotionRealPos(int driverIdx, s32 pos)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
|
|
{
|
|
return ;
|
|
}
|
|
|
|
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
|
|
|
|
pCtrl->realpos = pos;
|
|
// printf("set drv%d realpos to %d\r\n", driverIdx+1, pos);
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
|
|
// 初始化
|
|
void InitDrvMotions(void)
|
|
{
|
|
memset(&g_drvMotionsCtrlList, 0, sizeof(DriverMotionCtrl)*DEV_MOTIONS_NUM);
|
|
|
|
// 测试命令
|
|
}
|
|
|
|
// 计算过程
|
|
void DrvMotionCalcProc(void)
|
|
{
|
|
DriverMotionCtrl * pCtrl;
|
|
int i;
|
|
for (i = 0; i < DEV_MOTIONS_NUM; i++)
|
|
{
|
|
pCtrl = &(g_drvMotionsCtrlList[i]);
|
|
IntegrationMotionCalc(pCtrl);
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|