yangmeng/optical
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
9e0e6c5fb6
optical/NxFuncs/stm32motos/stm32motos.c
YM\yangmeng f024a6318b 调试完成
2025-09-04 09:45:08 +08:00

1056 lines
28 KiB
C
Raw Blame History

#define _IN_STM32MOTOS_C
#include "stm32motos.h"
#include "inout.h"
#include "shell.h"
//----------------------------------------------------------------------
void TestStm32Motos(char * para1, char * para2); //终端调试接口
//---------------------------------------------------------------------------------------------
typedef struct
{
// 配置参数
u32 pulseMode; // 脉冲类型 0:cw/ccw !=0:pulse/dir
u32 startPPS; // 启动速度
u32 runPPS; // 运动速度
u32 addPPSG; // 启停加速度
u32 brkPPSG; // 刹车加速度
// 运动中参数
s32 realpos; // 电机绝对坐标
int runState; // 运行状态
int dir; // 运行方向
s32 currentSpd; // 当前速度
u32 currentAcc; // 当前加速度
s32 totalAcc; // 累加加速度
u32 currentPos; // 当前位置
u32 totalPos; // 总位移
u32 decPos; // 减速点位移
int pulseState; // 脉冲状态 0:当前输出为0, !=当前输出为1
int startrunflag; // 启动运行标志
// 函数指针
u32 funTestTime; // 运行过程中外部函数检测间隔时间 单位:0.1ms
u32 funTestCount; // 运行过程中外部函数检测间隔时间计数器
int syncList[6]; // 同步移动选择
int (*GetNormalStop)(u32,u32); // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按启停加速度降速）
int (*GetQuickStop)(u32,u32); // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度降速）
u32 condPara1, condPara2; // 条件函数参数
int blockRunflag; // 阻塞运行标志
void (*ExecWhenRun)(void); // 电机运行过程中执行的函数
void (*ExecWhenStart)(void); // 电机启动时执行的函数
void (*ExecWhenStop)(void); // 电机停止时执行的函数
}TimerCtrl;
//---------------------------------------------------------------------------------------------
#ifndef SPEED_PSC
#define SPEED_PSC 72
#endif
#define SPEED_TIME 100 // 0.1ms
//---------------------------------------------------------------------------------------------
// TIM1: 异步控制速度控制(0.1毫秒中断)
#ifndef SPEED_TIM
#define SPEED_TIM TIM1
#endif
#ifndef MOTO1_TIM
#define MOTO1_TIM TIM2
#endif
#ifndef MOTO2_TIM
#define MOTO2_TIM TIM3
#endif
#ifndef MOTO3_TIM
#define MOTO3_TIM TIM4
#endif
#ifndef MOTO4_TIM
#define MOTO4_TIM TIM5
#endif
#ifndef MOTO5_TIM
#define MOTO5_TIM TIM6
#endif
#ifndef MOTO6_TIM
#define MOTO6_TIM TIM7
#endif
//---------------------------------------------------------------------------------------------
// 电机 定时器时钟，对应管脚，中断配置
#ifndef MOOT_TIM_PSC
#define MOOT_TIM_PSC 120 // 预分频
#endif
#define MOTO_FIN ((SYSCLK/MOOT_TIM_PSC)) // 时钟频率 = ((RTE_SYSCLK / MOOT_TIM_PSC)/(@25k时校准系数))
#define SPD_MIN 10 // 最慢脉冲频率 单位: 脉冲/秒 = (MOTO_FIN / 65535)
#define SPD_MAX 50000 // 最快脉冲频率 单位: 脉冲/秒 = (MOTO_FIN / 10)
#define ADD_MIN 1 // 最小加速度
#define ADD_MAX 100000 // 最大加速度 单位: pps/s
//---------------------------------------------------------------------------------------------
OutFunc SoftPwmProc = SetOutputNull;
OutFunc Moto1PulseOn = SetOutputNull;
OutFunc Moto1PulseOff = SetOutputNull;
OutFunc Moto1SignOn = SetOutputNull;
OutFunc Moto1SignOff = SetOutputNull;
OutFunc Moto2PulseOn = SetOutputNull;
OutFunc Moto2PulseOff = SetOutputNull;
OutFunc Moto2SignOn = SetOutputNull;
OutFunc Moto2SignOff = SetOutputNull;
OutFunc Moto3PulseOn = SetOutputNull;
OutFunc Moto3PulseOff = SetOutputNull;
OutFunc Moto3SignOn = SetOutputNull;
OutFunc Moto3SignOff = SetOutputNull;
OutFunc Moto4PulseOn = SetOutputNull;
OutFunc Moto4PulseOff = SetOutputNull;
OutFunc Moto4SignOn = SetOutputNull;
OutFunc Moto4SignOff = SetOutputNull;
OutFunc Moto5PulseOn = SetOutputNull;
OutFunc Moto5PulseOff = SetOutputNull;
OutFunc Moto5SignOn = SetOutputNull;
OutFunc Moto5SignOff = SetOutputNull;
OutFunc Moto6PulseOn = SetOutputNull;
OutFunc Moto6PulseOff = SetOutputNull;
OutFunc Moto6SignOn = SetOutputNull;
OutFunc Moto6SignOff = SetOutputNull;
//---------------------------------------------------------------------------------------------
// 脉冲 开
void SetMotoPulseOn(int curAux, int syncList[6])
{
if ((curAux == 1) || (syncList[1-1] == 1))
{
Moto1PulseOn();
}
if ((curAux == 2) || (syncList[2-1] == 1))
{
Moto2PulseOn();
}
if ((curAux == 3) || (syncList[3-1] == 1))
{
Moto3PulseOn();
}
if ((curAux == 4) || (syncList[4-1] == 1))
{
Moto4PulseOn();
}
if ((curAux == 5) || (syncList[5-1] == 1))
{
Moto5PulseOn();
}
if ((curAux == 6) || (syncList[6-1] == 1))
{
Moto6PulseOn();
}
}
// 脉冲 关
void SetMotoPulseOff(int curAux, int syncList[6])
{
if ((curAux == 1) || (syncList[1-1] == 1))
{
Moto1PulseOff();
}
if ((curAux == 2) || (syncList[2-1] == 1))
{
Moto2PulseOff();
}
if ((curAux == 3) || (syncList[3-1] == 1))
{
Moto3PulseOff();
}
if ((curAux == 4) || (syncList[4-1] == 1))
{
Moto4PulseOff();
}
if ((curAux == 5) || (syncList[5-1] == 1))
{
Moto5PulseOff();
}
if ((curAux == 6) || (syncList[6-1] == 1))
{
Moto6PulseOff();
}
}
// 方向 开
void SetMotoSignOn(int curAux, int syncList[6])
{
if ((curAux == 1) || (syncList[1-1] == 1))
{
Moto1SignOn();
}
if ((curAux == 2) || (syncList[2-1] == 1))
{
Moto2SignOn();
}
if ((curAux == 3) || (syncList[3-1] == 1))
{
Moto3SignOn();
}
if ((curAux == 4) || (syncList[4-1] == 1))
{
Moto4SignOn();
}
if ((curAux == 5) || (syncList[5-1] == 1))
{
Moto5SignOn();
}
if ((curAux == 6) || (syncList[6-1] == 1))
{
Moto6SignOn();
}
}
// 方向 关
void SetMotoSignOff(int curAux, int syncList[6])
{
if ((curAux == 1) || (syncList[1-1] == 1))
{
Moto1SignOff();
}
if ((curAux == 2) || (syncList[2-1] == 1))
{
Moto2SignOff();
}
if ((curAux == 3) || (syncList[3-1] == 1))
{
Moto3SignOff();
}
if ((curAux == 4) || (syncList[4-1] == 1))
{
Moto4SignOff();
}
if ((curAux == 5) || (syncList[5-1] == 1))
{
Moto5SignOff();
}
if ((curAux == 6) || (syncList[6-1] == 1))
{
Moto6SignOff();
}
}
//---------------------------------------
// 电机控制宏定义
#define USE_MOTO(x) \
TimerCtrl g_moto##x##TimerCtrl; \
\
/*---------------------------------------*/ \
void InitStm32Moto##x##OutFunc(OutFunc stepOn, OutFunc StepOff, OutFunc dirOn, OutFunc dirOff) \
{ \
Moto##x##PulseOn = stepOn; \
Moto##x##PulseOff = StepOff; \
Moto##x##SignOn = dirOn; \
Moto##x##SignOff = dirOff; \
}\
\
void InitSTM32Moto##x##Ctrl(void) \
{ \
LL_TIM_ClearFlag_UPDATE(MOTO##x##_TIM); /* 清除溢出中断标志 */ \
LL_TIM_DisableIT_UPDATE(MOTO##x##_TIM); /* 关闭中断 */ \
LL_TIM_DisableCounter(MOTO##x##_TIM); /* 关闭定时器 */ \
\
memset(&g_moto##x##TimerCtrl, 0, sizeof(TimerCtrl)); \
g_moto##x##TimerCtrl.pulseMode = MODE_STEPDIR; /* 脉冲模式 */ \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 中断处理 */ \
void STM32Moto##x##IntProc(void) \
{ \
if (LL_TIM_IsActiveFlag_UPDATE(MOTO##x##_TIM) != RESET) /* 检查指定的TIM中断发生与否 */ \
{ \
LL_TIM_ClearFlag_UPDATE(MOTO##x##_TIM); /* 清除TIMx的中断待处理位 */ \
\
if ((g_moto##x##TimerCtrl.currentPos >= g_moto##x##TimerCtrl.totalPos) || /* 发送脉冲完成 */ \
(g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP) || /* 停止信号 */ \
0 ) \
{ \
SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList); /* 关闭脉冲输出 */ \
SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); /* 关闭方向输出 */ \
LL_TIM_DisableCounter(MOTO##x##_TIM); /* 关闭定时器 */ \
if (g_moto##x##TimerCtrl.runState > STM32MOTO_STOP) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_STOP; /* 设置运行状态 */ \
g_moto##x##TimerCtrl.startrunflag = 0; /* 关闭运行标志 */ \
if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
{ \
g_moto##x##TimerCtrl.ExecWhenStop(); \
} \
} \
} \
else \
{ \
LL_TIM_SetAutoReload(MOTO##x##_TIM, (u16)((MOTO_FIN/g_moto##x##TimerCtrl.currentSpd)/2)); /* 设置计数值 */ \
\
if (g_moto##x##TimerCtrl.pulseState == 0) \
{ \
g_moto##x##TimerCtrl.pulseState = 1; \
g_moto##x##TimerCtrl.currentPos++; /* 计数值递增 */ \
g_moto##x##TimerCtrl.realpos += g_moto##x##TimerCtrl.dir; \
\
if (g_moto##x##TimerCtrl.pulseMode == MODE_CWCCW) /* 正反脉冲 */ \
{ \
if (g_moto##x##TimerCtrl.dir == 1) \
{ \
SetMotoPulseOn(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOn(); */\
} \
else \
{ \
SetMotoSignOn(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOn(); */\
} \
} \
else \
{ \
SetMotoPulseOn(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOn(); */\
} \
} \
else \
{ \
g_moto##x##TimerCtrl.pulseState = 0; \
if (g_moto##x##TimerCtrl.pulseMode == MODE_CWCCW) \
{ \
SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOff();*/ \
SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOff();*/ \
} \
else \
{ \
SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOff();*/ \
} \
} \
\
/* 加速中 */ \
if (g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) \
{ \
if (g_moto##x##TimerCtrl.currentPos >= (g_moto##x##TimerCtrl.totalPos>>1)) \
{/* 当前位移大于总位移的一半,进入降速状态 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_DEC; \
g_moto##x##TimerCtrl.decPos = g_moto##x##TimerCtrl.currentPos; /* 减速点位移 */ \
/*printf("enter dec, spd=%d, pos=%d\r\n", g_moto##x##TimerCtrl.currentSpd, g_moto##x##TimerCtrl.decPos);*/ \
} \
} \
\
/* 匀速中 */ \
if (g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) \
{ \
if (g_moto##x##TimerCtrl.currentPos >= g_moto##x##TimerCtrl.decPos) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_DEC; \
g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.addPPSG; /* 当前加速度 */ \
} \
} \
} \
} \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：电机速度控制 */ \
/* 速度控制中断内响应 */ \
void STM32Moto##x##Speed (void) \
{ \
/* 停止中 */ \
if (g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP || g_moto##x##TimerCtrl.startrunflag == 0) \
{ \
return; \
} \
if ((g_moto##x##TimerCtrl.blockRunflag == 0) && (g_moto##x##TimerCtrl.funTestTime != 0)) /* 阻塞运行标志 */ \
{ \
g_moto##x##TimerCtrl.funTestCount++; \
if ((g_moto##x##TimerCtrl.funTestCount >= g_moto##x##TimerCtrl.funTestTime)) \
{ \
g_moto##x##TimerCtrl.funTestCount = 0; \
if (g_moto##x##TimerCtrl.GetNormalStop != NULL) /* 一般停止 */ \
{ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) || /* 加速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) || /* 匀速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) || /* 计数降速 */ \
0 ) \
{ \
if (g_moto##x##TimerCtrl.GetNormalStop(g_moto##x##TimerCtrl.condPara1, g_moto##x##TimerCtrl.condPara2) != 0) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP; /* 条件停车 */ \
} \
} \
} \
/* 判断快速停止 */ \
if (g_moto##x##TimerCtrl.GetQuickStop != NULL) /* 快速停止 */ \
{ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) || /* 加速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) || /* 匀速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) || /* 计数降速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) || /* 条件停车 */ \
0 ) \
{ \
if (g_moto##x##TimerCtrl.GetQuickStop(g_moto##x##TimerCtrl.condPara1, g_moto##x##TimerCtrl.condPara2) != 0) \
{ \
g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.brkPPSG; /* 当前加速度 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_EMC; /* 条件停车 */ \
} \
} \
} \
if (g_moto##x##TimerCtrl.ExecWhenRun != NULL) /* 电机运行过程中执行的函数*/ \
{ \
g_moto##x##TimerCtrl.ExecWhenRun(); \
} \
} \
} \
\
/* 加速中 */ \
if (g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) \
{ \
g_moto##x##TimerCtrl.totalAcc += g_moto##x##TimerCtrl.currentAcc; /* 累加速度 */ \
do\
{\
if (g_moto##x##TimerCtrl.totalAcc >= 10000)\
{\
g_moto##x##TimerCtrl.currentSpd += 1; /* 当前速度 */ \
g_moto##x##TimerCtrl.totalAcc -= 10000; /* 累加速度 */ \
}\
else\
{\
break;\
}\
}while(1);\
if (g_moto##x##TimerCtrl.currentSpd >= (s32)g_moto##x##TimerCtrl.runPPS) \
{/* 当前速度大于运行速度,进入匀速状态 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_RUN; \
g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.runPPS; \
g_moto##x##TimerCtrl.decPos = (g_moto##x##TimerCtrl.totalPos - g_moto##x##TimerCtrl.currentPos); /* 减速点位移 */ \
/*printf("enter run, spd=%d, decpos=%d\r\n", g_moto##x##TimerCtrl.currentSpd, g_moto##x##TimerCtrl.decPos);*/ \
} \
} \
\
/* 减速中 */ \
if (g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) /* 计数降速 */ \
{ \
g_moto##x##TimerCtrl.totalAcc -= g_moto##x##TimerCtrl.currentAcc; /* 累加速度 */ \
do\
{\
if ((g_moto##x##TimerCtrl.totalAcc < 0) && (g_moto##x##TimerCtrl.currentSpd > 0))\
{\
g_moto##x##TimerCtrl.currentSpd -= 1; /* 当前速度 */ \
g_moto##x##TimerCtrl.totalAcc += 10000; /* 累加速度 */ \
}\
else\
{\
break;\
}\
}while(1);\
if (g_moto##x##TimerCtrl.currentSpd <= (s32)g_moto##x##TimerCtrl.startPPS) \
{ \
g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.startPPS; \
} \
} \
\
/* 条件停车中 */ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) || /* 条件停车 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_EMC) || /* 条件刹车 */ \
0) \
{ \
g_moto##x##TimerCtrl.totalAcc -= g_moto##x##TimerCtrl.currentAcc; /* 累加速度 */ \
do\
{\
if ((g_moto##x##TimerCtrl.totalAcc < 0) && (g_moto##x##TimerCtrl.currentSpd > 0))\
{\
g_moto##x##TimerCtrl.currentSpd -= 1; /* 当前速度 */ \
g_moto##x##TimerCtrl.totalAcc += 10000; /* 累加速度 */ \
}\
else\
{\
break;\
}\
}while(1);\
if (g_moto##x##TimerCtrl.currentSpd <= (s32)g_moto##x##TimerCtrl.startPPS) \
{/* 当前速度小于启动速度,进入停止状态 */ \
SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOff();*/ \
SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOff();*/ \
LL_TIM_DisableCounter(MOTO##x##_TIM); /* 关闭定时器 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_STOP; /* 设置运行状态 */ \
g_moto##x##TimerCtrl.startrunflag = 0; /* 关闭运行标志 */ \
if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
{ \
g_moto##x##TimerCtrl.ExecWhenStop(); \
} \
} \
} \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：启动电机 */ \
int STM32Moto##x##Start(MotoCtrl * pCtrl) \
{ \
int runflag = 1;\
int rslt = 0;\
if (g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP) /* 停车/报警状态允许启动电机 */ \
{ \
if (pCtrl->movement == 0) /* 电机运行完毕 */\
{ \
runflag = 0;\
} \
if (pCtrl->GetNormalStop != NULL) /* 一般停止 */ \
{\
rslt = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2); \
if (rslt != 0) \
{ \
runflag = 0;\
} \
} \
if (pCtrl->GetQuickStop != NULL) /* 快速停止 */ \
{ \
rslt = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2); \
if (rslt != 0) \
{ \
runflag = 0;\
} \
} \
if (runflag == 0) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_STOP; \
g_moto##x##TimerCtrl.startrunflag = 0; /* 关闭运行标志 */ \
return rslt; \
} \
if (pCtrl->ExecWhenStart != NULL) /* 启动执行 */ \
{ \
pCtrl->ExecWhenStart(); \
} \
/* 位移配置 */ \
g_moto##x##TimerCtrl.currentPos = 0; /* 当前位移 */ \
g_moto##x##TimerCtrl.pulseState = 0; /* 当前输出为 0 */ \
g_moto##x##TimerCtrl.totalPos = abs(pCtrl->movement); /* 总位移 */ \
g_moto##x##TimerCtrl.decPos = g_moto##x##TimerCtrl.totalPos; /* 减速位置 */ \
if (pCtrl->movement > 0) \
{/* 方向 */ \
g_moto##x##TimerCtrl.dir = 1; \
} \
else \
{ \
g_moto##x##TimerCtrl.dir = -1; \
} \
\
/* 速度配置 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_ADD; /* 加速中 */ \
g_moto##x##TimerCtrl.startPPS = pCtrl->startPPS; /* 启停速度 */ \
g_moto##x##TimerCtrl.runPPS = pCtrl->runPPS; /* 运行速度 */ \
g_moto##x##TimerCtrl.addPPSG = pCtrl->addPPSG; /* 启停加速度 */ \
g_moto##x##TimerCtrl.brkPPSG = pCtrl->brkPPSG; /* 刹车加速度 */ \
g_moto##x##TimerCtrl.totalAcc = 0; /* 累加加速度 */ \
\
g_moto##x##TimerCtrl.funTestTime = pCtrl->funTestTime; /* 运行过程中外部函数检测间隔时间 */ \
g_moto##x##TimerCtrl.funTestCount = 0; /* 运行过程中外部函数检测间隔时间计数器 */ \
\
g_moto##x##TimerCtrl.syncList[0] = pCtrl->syncList[0]; /* 同步移动选择 */ \
g_moto##x##TimerCtrl.syncList[1] = pCtrl->syncList[1]; /* 同步移动选择 */ \
g_moto##x##TimerCtrl.syncList[2] = pCtrl->syncList[2]; /* 同步移动选择 */ \
g_moto##x##TimerCtrl.syncList[3] = pCtrl->syncList[3]; /* 同步移动选择 */ \
g_moto##x##TimerCtrl.syncList[4] = pCtrl->syncList[4]; /* 同步移动选择 */ \
g_moto##x##TimerCtrl.syncList[5] = pCtrl->syncList[5]; /* 同步移动选择 */ \
\
g_moto##x##TimerCtrl.GetNormalStop = pCtrl -> GetNormalStop; /* 一般停止 */ \
g_moto##x##TimerCtrl.GetQuickStop = pCtrl -> GetQuickStop; /* 快速停止 */ \
g_moto##x##TimerCtrl.condPara1 = pCtrl -> condPara1; /* 参数1 */ \
g_moto##x##TimerCtrl.condPara2 = pCtrl -> condPara2; /* 参数2 */ \
\
g_moto##x##TimerCtrl.blockRunflag = pCtrl -> blockRunflag; /* 阻塞运行标志 */ \
g_moto##x##TimerCtrl.ExecWhenRun = pCtrl -> ExecWhenRun; /* 电机运行过程中执行的函数 */ \
g_moto##x##TimerCtrl.ExecWhenStart = pCtrl -> ExecWhenStart; /* 电机启动时执行的函数 */ \
g_moto##x##TimerCtrl.ExecWhenStop = pCtrl -> ExecWhenStop; /* 电机停止时执行的函数 */ \
\
/* 输入速度校验 */ \
if (g_moto##x##TimerCtrl.startPPS > SPD_MAX) \
{ \
g_moto##x##TimerCtrl.startPPS = SPD_MAX; \
} \
if (g_moto##x##TimerCtrl.startPPS < SPD_MIN) \
{ \
g_moto##x##TimerCtrl.startPPS = SPD_MIN; \
} \
if (g_moto##x##TimerCtrl.runPPS > SPD_MAX) \
{ \
g_moto##x##TimerCtrl.runPPS = SPD_MAX; \
} \
if (g_moto##x##TimerCtrl.runPPS < SPD_MIN) \
{ \
g_moto##x##TimerCtrl.runPPS = SPD_MIN; \
} \
if (g_moto##x##TimerCtrl.startPPS > g_moto##x##TimerCtrl.runPPS) \
{ \
g_moto##x##TimerCtrl.startPPS = g_moto##x##TimerCtrl.runPPS; \
} \
\
/* 输入加速度校验 */ \
if (g_moto##x##TimerCtrl.addPPSG > ADD_MAX) \
{ \
g_moto##x##TimerCtrl.addPPSG = ADD_MAX; \
} \
if (g_moto##x##TimerCtrl.addPPSG <= ADD_MIN) \
{ \
g_moto##x##TimerCtrl.addPPSG = ADD_MIN; \
} \
if (g_moto##x##TimerCtrl.brkPPSG > ADD_MAX) \
{ \
g_moto##x##TimerCtrl.brkPPSG = ADD_MAX; \
} \
if (g_moto##x##TimerCtrl.brkPPSG <= ADD_MIN) \
{ \
g_moto##x##TimerCtrl.brkPPSG = ADD_MIN; \
} \
g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.startPPS; /* 当前速度 */ \
g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.addPPSG; /* 当前加速度 */ \
\
/* 输出配置 */ \
SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##PulseOff();*/ \
if (g_moto##x##TimerCtrl.pulseMode != MODE_CWCCW) \
{ \
if (g_moto##x##TimerCtrl.dir == 1) \
{ \
SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOff();*/ \
} \
else \
{ \
SetMotoSignOn(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOn(); */\
} \
} \
else \
{ \
SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); /*SetMoto##x##SignOff();*/ \
} \
\
LL_TIM_DisableIT_UPDATE(MOTO##x##_TIM); /* 关闭中断 */ \
LL_TIM_SetAutoReload(MOTO##x##_TIM, (u16)((MOTO_FIN/g_moto##x##TimerCtrl.currentSpd)/2)); /* 设置计数值 */ \
LL_TIM_EnableIT_UPDATE(MOTO##x##_TIM); /* 开中断 */ \
LL_TIM_EnableCounter(MOTO##x##_TIM); /* 开定时器 */ \
g_moto##x##TimerCtrl.startrunflag = 1; /* 设置启动运行标志 */ \
\
return STM32Moto##x##Run(pCtrl); \
} \
else \
{ \
printf("moto is running, start failed\r\n"); \
return -1; /* 电机正在运行 */ \
} \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：停止电机 */ \
void STM32Moto##x##Stop(void) \
{ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) || /* 加速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) || /* 匀速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) || /* 降速 */ \
0 ) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP; /* 进入降速停车状态 */ \
if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
{ \
g_moto##x##TimerCtrl.ExecWhenStop(); \
} \
} \
} \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：电机运行 */ \
int STM32Moto##x##Run(MotoCtrl * pCtrl) \
{ \
/* 判断一般停止 */ \
do \
{ \
if (pCtrl == NULL || \
g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP) /* */ \
{ \
break; \
} \
if (pCtrl->GetNormalStop != NULL) /* 一般停止 */ \
{ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) || /* 加速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) || /* 匀速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) || /* 计数降速 */ \
0 ) \
{ \
if (pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2) != 0) \
{ \
g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP; /* 条件停车 */ \
if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
{ \
g_moto##x##TimerCtrl.ExecWhenStop(); \
} \
} \
} \
} \
/* 判断快速停止 */ \
if (pCtrl->GetQuickStop != NULL) /* 快速停止 */ \
{ \
if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) || /* 加速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) || /* 匀速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) || /* 计数降速 */ \
(g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) || /* 条件停车 */ \
0 ) \
{ \
if (pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2) != 0) \
{ \
g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.brkPPSG; /* 当前加速度 */ \
g_moto##x##TimerCtrl.runState = STM32MOTO_EMC; /* 条件停车 */ \
} \
} \
} \
if (pCtrl->ExecWhenRun != NULL) /* 电机运行过程中执行的函数*/ \
{ \
pCtrl->ExecWhenRun(); \
} \
if (pCtrl->blockRunflag == 0) /* 阻塞运行标志 */ \
{ \
break; \
} \
}while(1); \
return g_moto##x##TimerCtrl.runState; \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：得到电机运行状态 */ \
int GetSTM32Moto##x##State(void) \
{ \
return g_moto##x##TimerCtrl.runState; \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：得到电机当前坐标 */ \
s32 GetSTM32Moto##x##RealPos(void) \
{ \
return g_moto##x##TimerCtrl.realpos; \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：设置电机当前坐标 */ \
void SetSTM32Moto##x##RealPos(s32 pos) \
{ \
g_moto##x##TimerCtrl.realpos = pos; \
} \
\
/*-----------------------------------------------------------------------------------*/ \
/* 功能：设置电机输出脉冲模式 */ \
void SetSTM32Moto##x##Mode(int mode) \
{ \
g_moto##x##TimerCtrl.pulseMode = mode; \
} \
/*-----------------------------------------------------------------------------------*/ \
//----------------------------------------------------------------------
// 宏定义电机
#if (STM32_MOTOS_NUM >= 1)
USE_MOTO(1);
#endif
#if (STM32_MOTOS_NUM >= 2)
USE_MOTO(2);
#endif
#if (STM32_MOTOS_NUM >= 3)
USE_MOTO(3);
#endif
#if (STM32_MOTOS_NUM >= 4)
USE_MOTO(4);
#endif
#if (STM32_MOTOS_NUM >= 5)
USE_MOTO(5);
#endif
#if (STM32_MOTOS_NUM >= 6)
USE_MOTO(6);
#endif
//----------------------------------------------------------------------
// 初始化速度控制定时器中断
void InitSTM32Speed(void)
{
#if (STM32_MOTOS_NUM >= 1 || STM32_SOFT_PWM > 0)
LL_TIM_ClearFlag_UPDATE(SPEED_TIM); // 清除溢出中断标志
LL_TIM_EnableIT_UPDATE(SPEED_TIM); // 开启溢出中断
LL_TIM_EnableCounter(SPEED_TIM); // 使能定时器
#endif
}
/*-----------------------------------------------------------------------------------*/
// 速度控制中断处理
void STM32MotoSpeedIntProc(void)
{
#if (STM32_MOTOS_NUM >= 1 || STM32_SOFT_PWM > 0)
if (LL_TIM_IsActiveFlag_UPDATE(SPEED_TIM) != RESET)
{
LL_TIM_ClearFlag_UPDATE(SPEED_TIM);
#if (STM32_SOFT_PWM > 0)
SoftPwmProc();
#endif
#if (STM32_MOTOS_NUM >= 1)
STM32Moto1Speed();
#endif
#if (STM32_MOTOS_NUM >= 2)
STM32Moto2Speed();
#endif
#if (STM32_MOTOS_NUM >= 3)
STM32Moto3Speed();
#endif
#if (STM32_MOTOS_NUM >= 4)
STM32Moto4Speed();
#endif
#if (STM32_MOTOS_NUM >= 5)
STM32Moto5Speed();
#endif
#if (STM32_MOTOS_NUM >= 6)
STM32Moto6Speed();
#endif
}
#endif
}
//----------------------------------------------------------------------
void InitSTM32SoftPwm(OutFunc pwmProc)
{
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
InitSTM32Speed();
#endif
SoftPwmProc = pwmProc;
}
//----------------------------------------------------------------------
void InitSTM32Motos(void)
{
#if (STM32_MOTOS_NUM > 0)
InitSTM32Speed();
#endif
#if (STM32_MOTOS_NUM >= 1)
InitSTM32Moto1Ctrl();
#endif
#if (STM32_MOTOS_NUM >= 2)
InitSTM32Moto2Ctrl();
#endif
#if (STM32_MOTOS_NUM >= 3)
InitSTM32Moto3Ctrl();
#endif
#if (STM32_MOTOS_NUM >= 4)
InitSTM32Moto4Ctrl();
#endif
#if (STM32_MOTOS_NUM >= 5)
InitSTM32Moto5Ctrl();
#endif
#if (STM32_MOTOS_NUM >= 6)
InitSTM32Moto6Ctrl();
#endif
// AddShellCmd("TMR", "test stm32 moto run", TestStm32Motos);
}
// -----------------------------------------------------------
//终端调试接口
void TestStm32Motos(char * para1, char * para2)
{
#if (STM32_MOTOS_NUM >= 1)
int p1, p2;
MotoCtrl mtcfg; // 配置参数
memset(&mtcfg, 0, sizeof(MotoCtrl));
if (para1 == NULL || para2 == NULL)
{
return;
}
printf("para1=%s, para2=%s\r\n", para1, para2);
p1 = 0;
p2 = 0;
if (strcmp(para1, "") != 0)
{
p1 = atoi(para1);
if (strcmp(para2, "") != 0)
{
p2 = atoi(para2);
}
}
if (p1 == 0)
{//读取电机状态
if (p2 == 0)
{
g_moto1TimerCtrl.realpos = 0;
}
else if (p2 == 1)
{
printf("moto1pos=%ld\r\n", g_moto1TimerCtrl.realpos);
}
if (p2 == 100)
{// 电机状态
printf("moto1 state =%d\r\n",GetSTM32Moto1State());
}
}
if (p1 == 1)
{//moto1
if (p2 == 0)
{// 停止电机
STM32Moto1Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 300;
mtcfg.runPPS = 1000;
mtcfg.addPPSG = 10;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto1Start (&mtcfg);
}
}
#if (STM32_MOTOS_NUM >= 2)
if (p1 == 2)
{//moto2
if (p2 == 0)
{// 停止电机
STM32Moto2Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 300;
mtcfg.runPPS = 1000;
mtcfg.addPPSG = 10;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto2Start (&mtcfg);
}
}
#endif
#if (STM32_MOTOS_NUM >= 3)
if (p1 == 3)
{//moto3
if (p2 == 0)
{// 停止电机
STM32Moto3Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 50;
mtcfg.runPPS = 7000;
mtcfg.addPPSG = 5;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto3Start (&mtcfg);
}
}
#endif
#if (STM32_MOTOS_NUM >= 4)
if (p1 == 4)
{//moto4
if (p2 == 0)
{// 停止电机
STM32Moto4Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 50;
mtcfg.runPPS = 4000;
mtcfg.addPPSG = 5;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto4Start (&mtcfg);
}
}
#endif
#if (STM32_MOTOS_NUM >= 5)
if (p1 == 5)
{//moto5
if (p2 == 0)
{// 停止电机
STM32Moto5Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 100;
mtcfg.runPPS = 2000;
mtcfg.addPPSG = 50;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto5Start (&mtcfg);
}
}
#endif
#if (STM32_MOTOS_NUM >= 6)
if (p1 == 6)
{//moto5
if (p2 == 0)
{// 停止电机
STM32Moto6Stop();
}
else
{
mtcfg.movement = p2; // 位移量
mtcfg.startPPS = 100;
mtcfg.runPPS = 2000;
mtcfg.addPPSG = 50;
mtcfg.brkPPSG = 50;
mtcfg.GetNormalStop = NULL;
mtcfg.GetQuickStop = NULL;
mtcfg.condPara1 = 0;
mtcfg.condPara2 = 0;
STM32Moto6Start (&mtcfg);
}
}
#endif
#endif
}
Reference in New Issue View Git Blame Copy Permalink