425 lines
11 KiB
C
425 lines
11 KiB
C
|
||
#ifndef __MOVECTRL_H__
|
||
#define __MOVECTRL_H__
|
||
|
||
#include "config.h"
|
||
#include "motos.h"
|
||
#include "stm32motos.h"
|
||
#include "funcs.h"
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
typedef struct
|
||
{
|
||
u32 stspSpd; // 启停速度，单位pps
|
||
u32 runSpd; // 运行速度，单位pps
|
||
u32 spdAdd; // 起停加速度，单位pps2
|
||
u32 brkAdd; // 刹车加速度，单位pps2
|
||
} __attribute__ ((packed)) MotoSpd;
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 周期性转动找零
|
||
typedef struct
|
||
{
|
||
u16 vAxisId; // 虚轴
|
||
u16 axisIdx; // 运动轴
|
||
u16 axisCfg; // 配置
|
||
u16 axisOtp; // 输出
|
||
u16 newCfg; // 重新配置
|
||
|
||
s32 circleNum; // 整圈脉冲数
|
||
|
||
MotoSpd rspd; // 速度参数
|
||
|
||
int syncflag; // 同步移动标志
|
||
int syncList[AXIS_NUM]; // 同步移动选择
|
||
|
||
u8 (*sensorscan)(void);
|
||
//-------------------
|
||
int (*stopscan)(void); // 停止条件
|
||
//-------------------
|
||
int zeroflag; // 已经在位置标志
|
||
int cansflag; // 跳过检测标志
|
||
int canscount; // 跳过位移
|
||
int cansmvmt; // 跳过位移
|
||
|
||
int val; // 零位状态
|
||
|
||
}RSenserCheck;
|
||
|
||
int RoundRunToSensor(RSenserCheck * pck);
|
||
int RoundRunToSensorEdge(RSenserCheck * pck);
|
||
int RoundRunToSensorCenter(RSenserCheck * pck);
|
||
int RoundRunToSensorBase(RSenserCheck * pck, int flag);
|
||
|
||
//-------------------------------------------------------------------------------
|
||
// 周期型转动到某个位置
|
||
|
||
typedef struct
|
||
{
|
||
u16 vAxisId; // 虚轴
|
||
u16 newCfg; // 重新配置
|
||
|
||
struct
|
||
{
|
||
int axisIdx; // 索引
|
||
u16 axisCfg; // 配置
|
||
u16 poutType; // 输出类型
|
||
s32 movement; // 运动量
|
||
}config[AXIS_NUM];
|
||
|
||
u32 mrepeat; // 重复延伸
|
||
u32 interLong; // 长轴位移
|
||
u32 stopmode; // 停止模式
|
||
u32 segmentNum; // 分段脉冲数
|
||
|
||
MotoSpd rspd; // 速度
|
||
|
||
//-----------
|
||
int escStop; // 跳过停止条件检测
|
||
|
||
//-----------
|
||
void (*RRunProc)(void);
|
||
|
||
}RMotoRun;
|
||
|
||
int RoundRunAsOffset(RMotoRun * pmr);
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 直线运动找零
|
||
typedef struct
|
||
{
|
||
u16 vAxisId; // 虚轴
|
||
u16 axisIdx; // 运动轴
|
||
|
||
u16 axisCfg; // 配置
|
||
u16 poutType; // 输出类型
|
||
u16 newCfg; // 重新配置
|
||
|
||
s32 movement; // 检测移动位移
|
||
s32 mvposi; // 反向移动位移
|
||
|
||
int syncflag; // 同步移动标志
|
||
int syncList[AXIS_NUM]; // 同步移动选择
|
||
|
||
MotoSpd lspd; // 速度
|
||
|
||
u8 (*slowsensor)(void); // 降速检测传感器
|
||
u8 (*zerosensor)(void); // 零位检测传感器
|
||
int zeroval; // 零位状态
|
||
|
||
u8 (*limitpsensor)(void); // 正向限位传感器
|
||
u8 (*limitnsensor)(void); // 反向限位传感器
|
||
|
||
int (*emcstopscan)(void); // 停止条件
|
||
|
||
u32 para;
|
||
|
||
int (*moveasoffset)(s32, MotoSpd*); // 移动函数
|
||
|
||
//-------------------------
|
||
int slowsta; // 降速状态
|
||
|
||
}MTSCtrl;
|
||
|
||
int LineRunToSensorCtrl(MTSCtrl * pmtsc);
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 直线运动到零位传感器且挡片在中心位置
|
||
typedef struct
|
||
{
|
||
u16 vAxisId; // 虚轴
|
||
u16 axisIdx; // 运动轴
|
||
|
||
u16 axisCfg; // 配置
|
||
u16 poutType; // 输出类型
|
||
u16 newCfg; // 重新配置
|
||
|
||
s32 movement; // 移动位移
|
||
u32 bafflewidth;// 挡片宽度
|
||
|
||
int syncflag; // 同步移动标志
|
||
int syncList[AXIS_NUM]; // 同步移动选择
|
||
|
||
MotoSpd lspd; // 找零速度
|
||
MotoSpd mposspd; // 找挡片中心位置速度
|
||
|
||
u8 (*zerosensor)(void); // 零位检测传感器
|
||
int zeroval; // 零位状态
|
||
|
||
u8 (*limitpsensor)(void); // 正向限位传感器
|
||
u8 (*limitnsensor)(void); // 反向限位传感器
|
||
|
||
int (*emcstopscan)(void); // 停止条件
|
||
|
||
}MTSAMPCtrl;
|
||
|
||
int LineRunToSensorAsMidPosCtrl(MTSAMPCtrl * pmtsampc);
|
||
|
||
//------------------------------------------------------------------------------------
|
||
|
||
typedef struct
|
||
{
|
||
u16 vAxisId; // 虚轴
|
||
u16 axisIdx; // 运动轴
|
||
|
||
u16 axisCfg; // 配置
|
||
u16 poutType; // 输出类型
|
||
u16 newCfg; // 重新配置
|
||
|
||
s32 movement; // 最大移动位移
|
||
|
||
int syncflag; // 同步移动标志
|
||
int syncList[AXIS_NUM]; // 同步移动选择
|
||
|
||
u32 ppSegment;
|
||
|
||
MotoSpd hspd; // 速度
|
||
|
||
u8 (*limitpsensor)(u32); // 正向限位传感器
|
||
u8 (*limitnsensor)(u32); // 反向限位传感器
|
||
|
||
int (*precondition)(u32); // 先决条件
|
||
int (*stopscan)(u32); // 停止条件
|
||
int (*slowscan)(u32);
|
||
|
||
u32 para;
|
||
|
||
}MVByHandCtrl;
|
||
|
||
int MoveByHandCtrl(MVByHandCtrl * pmbhc);
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
// 单轴异步运动
|
||
// 使用STM32计算速度&发送脉冲
|
||
typedef struct
|
||
{
|
||
u16 axisIdx; // 运动轴
|
||
u16 axisCfg; // 配置
|
||
u16 poutType; // 输出类型
|
||
u16 newCfg; // 重新配置
|
||
|
||
u16 useOutPort; // 是否使用IO口输出 0:使用M口输出,1:使用IO口输出
|
||
|
||
s32 movement; // 位移量
|
||
|
||
u32 startPPS; // 启动速度
|
||
u32 runPPS; // 运动速度
|
||
u32 addPPSG; // 启停加速度
|
||
u32 brkPPSG; // 刹车加速度
|
||
|
||
u32 funTestTime; // 运行过程中外部函数检测间隔时间 0:不检测外部函数 (单位:0.1ms)
|
||
int syncList[AXIS_NUM]; // 同步移动选择
|
||
|
||
// 函数指针
|
||
int (*GetNormalStop)(u32,u32); // 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按启停加速度降速）
|
||
int (*GetQuickStop)(u32,u32); // 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度降速）
|
||
u32 condPara1, condPara2; // 条件函数参数
|
||
|
||
int blockRunflag; // 阻塞运行标志
|
||
void (*ExecWhenRun)(void); // 电机运行过程中执行的函数
|
||
void (*ExecWhenStart)(void); // 电机启动时执行的函数
|
||
void (*ExecWhenStop)(void); // 电机停止时执行的函数
|
||
}SepMoveCtrl;
|
||
|
||
int SeparateExerStart(SepMoveCtrl * pCtrl); // 单轴异步运动 启动
|
||
int SeparateExerStop(u16 axisIdx); // 单轴异步运动 停止
|
||
int GetSeparateExerState(u16 axisIdx); // 单轴异步运动 得到当前状态
|
||
|
||
//-------------------------------------------------------------------------------
|
||
int GetCommonNStop(void);
|
||
int GetCommonQStop(void);
|
||
int GetCommonEms(void);
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#define MMPSS_TO_ST_PPSG(mmpss) (((mmpss)*PULSE_PER_MM)/(10*1000)) // 0.1ms的时间间隔
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
|
||
#define MTM1_IDX 0 // 电机口1
|
||
#define MTM1_CFG MOTOENAB_CFG // 配置
|
||
#define MTM1_OTP POUT_CWCCW
|
||
#define MTM1_MOD 0x0001
|
||
|
||
#define MTM2_IDX 1 // 电机口2
|
||
#define MTM2_CFG MOTOENAB_CFG // 配置
|
||
#define MTM2_OTP POUT_CWCCW
|
||
#define MTM2_MOD 0x0002
|
||
|
||
#define MTM3_IDX 2 // 电机口3
|
||
#define MTM3_CFG MOTOENAB_CFG // 配置
|
||
#define MTM3_OTP POUT_CWCCW
|
||
#define MTM3_MOD 0x0004
|
||
|
||
#define MTM4_IDX 3 // 电机口4
|
||
#define MTM4_CFG MOTOENAB_CFG // 配置
|
||
#define MTM4_OTP POUT_CWCCW
|
||
#define MTM4_MOD 0x0008
|
||
|
||
#define MTM5_IDX 4 // 电机口5
|
||
#define MTM5_CFG MOTOENAB_CFG // 配置 DEFAULT_CFG
|
||
#define MTM5_OTP POUT_CWCCW
|
||
#define MTM5_MOD 0x0010
|
||
|
||
#define MTM6_IDX 5 // 电机口6
|
||
#define MTM6_CFG MOTOENAB_CFG // 配置
|
||
#define MTM6_OTP POUT_CWCCW
|
||
#define MTM6_MOD 0x0020
|
||
|
||
|
||
#define ALL_MOTO_MOD (MTM1_MOD|MTM2_MOD|MTM3_MOD|MTM4_MOD|MTM5_MOD|MTM6_MOD)
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 单位转换，由“毫米”转换为“脉冲个数”
|
||
#define MM_TO_PULSE(mm) ((mm)*PULSE_PER_MM)
|
||
|
||
// 单位转换，由“毫米每秒”转换为“脉冲个数每秒”
|
||
#define MMPS_TO_PPS(mmps) ((mmps)*PULSE_PER_MM)
|
||
|
||
// 单位转换， STM32异步运动加速度
|
||
#define MMPSS_ASYN_TO_PPSG(mmpss) ((mmpss)*PULSE_PER_MM)
|
||
|
||
// 单位转换，由“毫米每秒平方”转换为“脉冲个数每秒间隔”，间隔单位为1/72000000秒
|
||
#define MMPSS_TO_PPSG(mmpss) (FPGA_CLK/(PULSE_PER_MM)/(mmpss))
|
||
|
||
// 单位转换，由“毫米每秒立方”转换为“脉冲个数每秒平方每间隔”，间隔单位为1/72000000秒
|
||
#define MMPSSS_TO_PPSSG(mmpsss) (FPGA_CLK/(PULSE_PER_MM)/(mmpsss))
|
||
|
||
// 单位转换，由“脉冲每秒平方”转换为“脉冲个数每秒间隔”，间隔单位为1/72000000秒
|
||
#define PPSS_TO_PPSG(ppss) (FPGA_CLK/(ppss))
|
||
|
||
// 单位转换，由“毫米每秒”转换为“脉冲个数每秒”
|
||
#define PPS_TO_MMPS(pps) ((pps)/PULSE_PER_MM)
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#ifndef MS_PULSE_PER_CIRCLE
|
||
#define MS_PULSE_PER_CIRCLE (10000) // 主轴电机每转脉冲数
|
||
#endif
|
||
|
||
// 单位转换，由主轴的“度”转换为“脉冲个数”
|
||
#define MS_DEG_TO_PULSE(deg) ((deg)*MS_PULSE_PER_CIRCLE/DEG_PER_CIRCLE)
|
||
|
||
// 单位转换，由“0.01度”转换为“脉冲个数”
|
||
#define MS_001DEG_TO_PULSE(deg) ((deg)*MS_PULSE_PER_CIRCLE/DEG001_PER_CIRCLE)
|
||
|
||
// 单位转换，由“转每分”转换为“脉冲个数每秒”
|
||
#define RPM_TO_PPS(rpm) (((rpm)*MS_PULSE_PER_CIRCLE)/60)
|
||
|
||
// 单位转换，由“转每秒平方”转换为“脉冲个数每秒方”
|
||
#define RPSS_TO_PPSS(rpss) ((rpss)*MS_PULSE_PER_CIRCLE)
|
||
|
||
// 单位转换，由“转每秒平方”转换为“脉冲个数每秒间隔”，间隔单位为1/72000000秒
|
||
#define RPSS_TO_PPSG(rpss) (FPGA_CLK/((rpss)*MS_PULSE_PER_CIRCLE))
|
||
|
||
// 单位转换，由 “脉冲个数每秒”转换为“转每分”
|
||
#define PPS_TO_RPM(pps) ((pps)* 60 / MS_PULSE_PER_CIRCLE)
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#ifndef ECD_PULSE_PER_CIRCLE
|
||
#define ECD_PULSE_PER_CIRCLE (1000) // 编码器每转脉冲数
|
||
#endif
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
#ifndef PULSE_PER_SEGMENT
|
||
#define PULSE_PER_SEGMENT (1000) // 直线插补填充数据每段脉冲数
|
||
#endif
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 每圈的度数（单位 度）
|
||
#define DEG_PER_CIRCLE (360)
|
||
|
||
#define DEG001_PER_DEG (100)
|
||
|
||
// 每圈的度数（单位 0.01度）
|
||
#define DEG001_PER_CIRCLE (DEG_PER_CIRCLE*DEG001_PER_DEG)
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
// 旋转电机每转脉冲数 10000
|
||
#ifndef ROT_PULSE_PER_CIRCLE
|
||
#define ROT_PULSE_PER_CIRCLE (10000)
|
||
#endif
|
||
|
||
// 单位转换，由“度”转换为“脉冲个数”
|
||
#define ROT_DEG_TO_PULSE(deg) ((deg)*(s32)(ROT_PULSE_PER_CIRCLE)/DEG_PER_CIRCLE)
|
||
|
||
// 单位转换，由“0.01度”转换为“脉冲个数”
|
||
#define ROT_001DEG_TO_PULSE(deg) ((deg)*(s32)(ROT_PULSE_PER_CIRCLE)/DEG001_PER_CIRCLE)
|
||
|
||
// 单位转换，由“脉冲个数”转换为“0.01度”
|
||
#define PULSE_TO_ROT_001DEG(pls) ((pls)*(s32)(DEG001_PER_CIRCLE)/(ROT_PULSE_PER_CIRCLE))
|
||
|
||
// 单位转换，由“度每秒”转换为“脉冲个数每秒”
|
||
#define DPS_TO_PPS(dps) ((dps)*ROT_PULSE_PER_CIRCLE/DEG_PER_CIRCLE)
|
||
|
||
// 单位转换，由“度每秒平方”转换为“脉冲个数每秒间隔”，间隔单位为1/72000000秒
|
||
#define DPSS_TO_PPSG(dpss) (FPGA_CLK/((dpss)*ROT_PULSE_PER_CIRCLE/DEG_PER_CIRCLE))
|
||
|
||
// 单位转换，由“度”转换为“万分之一弧度”
|
||
#define ROT_DEG_TO_PITK(deg) ((double)(deg)*2*PI*RADIAN_ACCURACY/DEG_PER_CIRCLE)
|
||
|
||
// 单位转换，由“0.01度”转换为“万分之一弧度”
|
||
#define ROT_001DEG_TO_PITK(deg) ((double)(deg)*2*PI*RADIAN_ACCURACY/DEG001_PER_CIRCLE)
|
||
|
||
// 单位转换，由“万分之一弧度”转换为“脉冲个数”
|
||
#define PITK_TO_RPP(dr) (((double)(dr))*ROT_PULSE_PER_CIRCLE/RADIAN_ACCURACY/PI/2+0.5*(dr>0?1:-1))
|
||
|
||
// 单位转换，由“脉冲个数”转换为“万分之一弧度”
|
||
#define RPP_TO_PITK(rpp) (((double)(rpp)*2*PI*RADIAN_ACCURACY/ROT_PULSE_PER_CIRCLE)+0.5*(rpp>0?1:-1))
|
||
|
||
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
|
||
// 旋转转化为 -180--180度之间的数
|
||
void ChangeRotToNP180(s32 * pval);
|
||
|
||
// 旋转转化为 -360--360度之间的数
|
||
void ChangeRotToNP360(s32 * pval);
|
||
|
||
// 旋转转化为 0--360度之间的数
|
||
void ChangeRotTo0To360(s32 * pval);
|
||
|
||
// 旋转转化为 -360--0度之间的数
|
||
void ChangeRotToN360To0(s32 * pval);
|
||
|
||
// 主轴转化为 -180--180度之间的数
|
||
void ChangeMsToNP180(s32 * pval, int pdir);
|
||
|
||
// 主轴转化为 -360--360度之间的数
|
||
void ChangeMsToNP360(s32 * pval);
|
||
|
||
void ChangeMsTo0To360(s32 * pval);
|
||
|
||
//-------------------------------------------------------------------------------
|
||
|
||
int GetMoveResultError(int rslt, int errinfo);
|
||
|
||
//-------------------------------------------------------------------------------
|
||
#endif
|
||
|