#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