【AI解析】一个简单的 C 库，用于控制工业机器人源码注释（一）
·
calibration.h calibration.c
calibration.h 文件
#ifndefCALIBRATION_H // 如果宏 CALIBRATION_H 未被定义
#defineCALIBRATION_H// 则定义宏 CALIBRATION_H，这是标准的头文件保护宏，用于防止头文件被重复包含
#include<math.h>// 包含标准数学库头文件，提供例如 sqrt (开方) 等数学函数
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了如 Coord_Type 等类型
#include"Frame.h"// 包含坐标系相关的头文件，可能定义了 Frame_Type 等数据结构
/* Declaration of calibration functions */// 校准函数声明的注释块开始
unsignedshortToolCalibration(Frame_Type P[5], Coord_Type *Result);// 声明工具校准函数，该函数接收5个机器人法兰盘位姿点，计算并返回工具的坐标数据
unsignedshortTriangulate(double A[4][4]);// 声明三角化函数，该函数对一个4x4矩阵进行上三角化处理，通常用于求解线性方程组
unsignedshortFrameCalibration(Coord_Type P[3], Frame_Type *Result);// 声明用户坐标系校准函数，该函数接收3个空间点，计算并返回一个新的坐标系
#endif// 结束 #ifndef CALIBRATION_H 的宏定义条件块calibration.c 文件
// calibration.c // 注释：源文件名为 calibration.c
#include<math.h>// 包含标准数学库头文件，提供数学函数
#include"Calibration.h"// 包含此文件对应的头文件 "Calibration.h"，其中有函数的声明
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了机器人相关的常量和类型
#include"Frame.h"// 包含坐标系相关的头文件，定义了 Frame_Type 等结构
#include"PathPlanner.h"// 包含路径规划相关的头文件，此处可能未使用，但被包含进来
#include"Misc.h"// 包含一些杂项功能或工具函数，例如 sign() 或 RoundToEpsilon()
doubleNorm(double a[4],int i)// 定义一个名为 Norm 的函数，用于计算向量的范数（模长）
{//norm of vector a starting from element i // 函数体开始，注释说明：计算向量a从索引i开始的范数
double norm =0;// 声明一个双精度浮点数变量 norm 并初始化为0，用于累加平方和
int j;// 声明一个整型变量 j，用作循环计数器
for(j=i;j<4;j++) norm+=(a[j]*a[j]);// 循环从索引 i 到 3，计算向量元素的平方和
returnsqrt(norm);// 返回平方和的平方根，即欧几里得范数
}
unsignedshortToolCalibration(Frame_Type P[5], Coord_Type *Result)// 定义工具校准函数，输入为5个机器人法兰盘位姿点 P 和一个指向结果坐标的指针 Result
{// 函数体开始
unsignedshort status =0;// 声明一个无符号短整型变量 status 并初始化为0，用于存储函数执行状态
//build matrix A // 注释：构建矩阵 A
double A[4][4];// 声明一个4x4的双精度浮点数矩阵 A，用于构建线性方程组
A[0][0]=2*(P[0].Axes[0]- P[1].Axes[0]);// 计算矩阵A的第0行第0列元素，基于点0和点1的X坐标差
A[0][1]=2*(P[0].Axes[1]- P[1].Axes[1]);// 计算矩阵A的第0行第1列元素，基于点0和点1的Y坐标差
A[0][2]=2*(P[0].Axes[2]- P[1].Axes[2]);// 计算矩阵A的第0行第2列元素，基于点0和点1的Z坐标差
A[0][3]= P[0].Axes[0]*P[0].Axes[0]- P[1].Axes[0]*P[1].Axes[0]+ P[0].Axes[1]*P[0].Axes[1]- P[1].Axes[1]*P[1].Axes[1]+ P[0].Axes[2]*P[0].Axes[2]- P[1].Axes[2]*P[1].Axes[2];// 计算矩阵A的第0行第3列元素，基于点0和点1坐标的平方差之和
A[1][0]=2*(P[0].Axes[0]- P[2].Axes[0]);// 计算矩阵A的第1行第0列元素，基于点0和点2的X坐标差
A[1][1]=2*(P[0].Axes[1]- P[2].Axes[1]);// 计算矩阵A的第1行第1列元素，基于点0和点2的Y坐标差
A[1][2]=2*(P[0].Axes[2]- P[2].Axes[2]);// 计算矩阵A的第1行第2列元素，基于点0和点2的Z坐标差
A[1][3]= P[0].Axes[0]*P[0].Axes[0]- P[2].Axes[0]*P[2].Axes[0]+ P[0].Axes[1]*P[0].Axes[1]- P[2].Axes[1]*P[2].Axes[1]+ P[0].Axes[2]*P[0].Axes[2]- P[2].Axes[2]*P[2].Axes[2];// 计算矩阵A的第1行第3列元素，基于点0和点2坐标的平方差之和
A[2][0]=2*(P[0].Axes[0]- P[3].Axes[0]);// 计算矩阵A的第2行第0列元素，基于点0和点3的X坐标差
A[2][1]=2*(P[0].Axes[1]- P[3].Axes[1]);// 计算矩阵A的第2行第1列元素，基于点0和点3的Y坐标差
A[2][2]=2*(P[0].Axes[2]- P[3].Axes[2]);// 计算矩阵A的第2行第2列元素，基于点0和点3的Z坐标差
A[2][3]= P[0].Axes[0]*P[0].Axes[0]- P[3].Axes[0]*P[3].Axes[0]+ P[0].Axes[1]*P[0].Axes[1]- P[3].Axes[1]*P[3].Axes[1]+ P[0].Axes[2]*P[0].Axes[2]- P[3].Axes[2]*P[3].Axes[2];// 计算矩阵A的第2行第3列元素，基于点0和点3坐标的平方差之和
A[3][0]=2*(P[0].Axes[0]- P[4].Axes[0]);// 计算矩阵A的第3行第0列元素，基于点0和点4的X坐标差
A[3][1]=2*(P[0].Axes[1]- P[4].Axes[1]);// 计算矩阵A的第3行第1列元素，基于点0和点4的Y坐标差
A[3][2]=2*(P[0].Axes[2]- P[4].Axes[2]);// 计算矩阵A的第3行第2列元素，基于点0和点4的Z坐标差
A[3][3]= P[0].Axes[0]*P[0].Axes[0]- P[4].Axes[0]*P[4].Axes[0]+ P[0].Axes[1]*P[0].Axes[1]- P[4].Axes[1]*P[4].Axes[1]+ P[0].Axes[2]*P[0].Axes[2]- P[4].Axes[2]*P[4].Axes[2];// 计算矩阵A的第3行第3列元素，基于点0和点4坐标的平方差之和
//triangulate matrix A // 注释：对矩阵 A 进行三角化
status =Triangulate(A);// 调用 Triangulate 函数，将矩阵 A 转换为上三角矩阵
if(status !=0)return status;// 如果三角化过程出错（返回非0状态），则立即返回错误状态
//solve for TCP.xyz // 注释：求解工具中心点（TCP）的xyz坐标
double TCP[6];// 声明一个包含6个双精度浮点数的数组 TCP，用于存储工具中心点的位姿（X, Y, Z, A, B, C）
if(fabs(A[0][0])<TRF_EPSILON ||fabs(A[1][1])<TRF_EPSILON ||fabs(A[2][2])<TRF_EPSILON)// 检查上三角矩阵的对角线元素是否接近于零
{// if 块开始
return ERR_CALIBRATION;// 如果任何一个对角线元素过小（可能导致奇异矩阵），则返回校准错误
}// if 块结束
else// 否则，如果矩阵是可逆的
{// else 块开始
TCP[2]= A[2][3]/A[2][2];// 通过回代法求解 TCP 的 Z 坐标
TCP[1]=(A[1][3]-TCP[2]*A[1][2])/A[1][1];// 通过回代法求解 TCP 的 Y 坐标
TCP[0]=(A[0][3]-TCP[2]*A[0][2]-TCP[1]*A[0][1])/A[0][0];// 通过回代法求解 TCP 的 X 坐标
TCP[3]= P[0].Axes[3];// TCP 的姿态角 A 被假定为与第一个测量点的姿态角相同
TCP[4]= P[0].Axes[4];// TCP 的姿态角 B 被假定为与第一个测量点的姿态角相同
TCP[5]= P[0].Axes[5];// TCP 的姿态角 C 被假定为与第一个测量点的姿态角相同
}// else 块结束
//calculate tool size // 注释：计算工具尺寸（即工具坐标系相对于法兰坐标系的数据）
double Tool[6];// 声明一个包含6个双精度浮点数的数组 Tool，用于存储最终的工具数据
AddFrame3D(TCP,P[0].Axes,P[0].Axes[3],P[0].Axes[4],P[0].Axes[5],Tool);// 调用一个函数（可能用于坐标系变换），计算相对于法兰盘的工具数据
(*Result).X = Tool[0];// 将计算出的工具 X 坐标赋值给结果结构体
(*Result).Y = Tool[1];// 将计算出的工具 Y 坐标赋值给结果结构体
(*Result).Z = Tool[2];// 将计算出的工具 Z 坐标赋值给结果结构体
return0;// 如果一切顺利，返回成功状态 0
}// ToolCalibration 函数体结束
unsignedshortTriangulate(double A[4][4])// 定义 Triangulate 函数，使用Householder变换将矩阵A转换为上三角形式
{// 函数体开始
int i,j,k;// 声明三个整型变量 i, j, k，用作循环计数器
double v[4];// 声明一个包含4个双精度浮点数的数组 v，用于存储矩阵的列向量
double e[4]={1,0,0,0};// 声明并初始化一个包含4个双精度浮点数的数组 e，用作基向量
double u[4];// 声明一个包含4个双精度浮点数的数组 u，用于存储Householder向量
double h[4];// 声明一个包含4个双精度浮点数的数组 h，用于存储中间计算结果
double B[4][4];// 声明一个4x4的双精度浮点数矩阵 B，用于存储中间计算结果
for(i=0;i<3;i++)// 外层循环，对矩阵的前3列进行处理
{// for 循环体开始
for(j=i;j<4;j++) v[j]=A[j][i];// 将当前列（从对角线元素开始）的元素复制到向量 v 中
e[i]=Norm(v,i)*sign(v[i]);// 计算向量 v 的范数，并乘以其首个元素的符号，结果存入 e[i]
for(j=i;j<4;j++) u[j]=v[j]+e[j];// 计算Householder向量 u
double u_n =Norm(u,i);// 计算Householder向量 u 的范数
if(fabs(u_n)<TRF_EPSILON)return ERR_CALIBRATION;// 如果 u 的范数接近于零，则表示计算不稳定，返回校准错误
for(j=i;j<4;j++) u[j]=u[j]/u_n;// 将Householder向量 u 单位化
//h = uT * A // 注释：计算 h = u的转置乘以A（实际上是 h = u^T * A_sub）
for(j=i;j<4;j++)// 循环遍历矩阵A的列（从第i列开始）
{// for 循环体开始
h[j]=0;// 初始化 h 向量的当前元素
for(k=i;k<4;k++) h[j]+=u[k]*A[k][j];// 计算 u 的转置与矩阵 A 的一列的点积
}// for 循环体结束
//B = u * h // 注释：计算 B = u * h (外积)
for(j=i;j<4;j++)// 循环遍历矩阵 B 的行
{// for 循环体开始
for(k=i;k<4;k++)// 循环遍历矩阵 B 的列
{// for 循环体开始
B[j][k]=u[j]*h[k];// 计算外积 u * h^T，填充矩阵 B
}// for 循环体结束
}// for 循环体结束
//A = A-2B // 注释：更新矩阵 A，A = A - 2*B
for(j=i;j<4;j++)// 循环遍历矩阵 A 的行
{// for 循环体开始
for(k=i;k<4;k++)// 循环遍历矩阵 A 的列
{// for 循环体开始
A[j][k]=A[j][k]-2*B[j][k];// 应用Householder变换：A_new = A - 2 * u * u^T * A
}// for 循环体结束
}// for 循环体结束
}// 外层 for 循环体结束
return0;// 函数成功完成，返回状态 0
}// Triangulate 函数体结束
unsignedshortFrameCalibration(Coord_Type P[3], Frame_Type *Result){// 定义坐标系校准函数，输入为3个点 P 和一个指向结果坐标系的指针 Result
int i;// 声明一个整型变量 i，用作循环计数器
double Vx[3],Vy[3],Vz[3];// 声明三个数组，分别用于存储新坐标系的X, Y, Z轴单位向量
double A[3],B[3],C[3];// 声明三个数组，用于存储输入的三个点的坐标
A[0]= P[0].X;// 将第一个点(P[0])的X坐标存入数组 A
A[1]= P[0].Y;// 将第一个点(P[0])的Y坐标存入数组 A
A[2]= P[0].Z;// 将第一个点(P[0])的Z坐标存入数组 A
B[0]= P[1].X;// 将第二个点(P[1])的X坐标存入数组 B
B[1]= P[1].Y;// 将第二个点(P[1])的Y坐标存入数组 B
B[2]= P[1].Z;// 将第二个点(P[1])的Z坐标存入数组 B
C[0]= P[2].X;// 将第三个点(P[2])的X坐标存入数组 C
C[1]= P[2].Y;// 将第三个点(P[2])的Y坐标存入数组 C
C[2]= P[2].Z;// 将第三个点(P[2])的Z坐标存入数组 C
//calculate and normalize Vx // 注释：计算并单位化 Vx 向量
PointsToVector(A, B, Vx);// 调用函数计算从点A到点B的向量，该向量定义了新坐标系的X轴方向
if(VectorLength(Vx)< TRF_EPSILON){// 检查向量长度是否过小（即A、B两点是否重合）
return ERR_CALIBRATION;// 如果向量长度过小，返回校准错误
}// if 块结束
Normalize(Vx);// 调用函数将 Vx 向量单位化
//find origin // 注释：寻找原点
double Vca[3];// 声明一个数组 Vca，用于存储从A到C的向量
PointsToVector(A, C, Vca);// 调用函数计算从点A到点C的向量
if(VectorLength(Vca)< TRF_EPSILON){// 检查向量长度是否过小（即A、C两点是否重合）
return ERR_CALIBRATION;// 如果向量长度过小，返回校准错误
}// if 块结束
double d =DotProduct(Vx, Vca);// 计算向量 Vca 在 Vx 上的投影长度 d
double O[3];// 声明一个数组 O，用于存储新坐标系的原点坐标
for(i=0;i<3;i++) O[i]= A[i]+ d*Vx[i];// 计算原点O的坐标，O是点C在直线AB上的投影点
//calculate and normalize Vy // 注释：计算并单位化 Vy 向量
PointsToVector(O, C, Vy);// 调用函数计算从新原点O到点C的向量，该向量定义了Y轴方向
if(VectorLength(Vy)< TRF_EPSILON){// 检查向量长度是否过小（即三点是否共线）
return ERR_CALIBRATION;// 如果向量长度过小，返回校准错误
}// if 块结束
Normalize(Vy);// 调用函数将 Vy 向量单位化
//calculate and normalize Vz // 注释：计算并单位化 Vz 向量
CrossProduct(Vx, Vy, Vz);// 调用函数计算 Vx 和 Vy 的叉积，得到 Z 轴向量 Vz
Normalize(Vz);// 调用函数将 Vz 向量单位化
//compose rotation matrix // 注释：构建旋转矩阵
double RM[3][3];// 声明一个3x3的双精度浮点数矩阵 RM，用于存储旋转矩阵
for(i=0;i<3;i++) RM[i][0]= Vx[i];// 将 Vx 向量作为旋转矩阵的第一列
for(i=0;i<3;i++) RM[i][1]= Vy[i];// 将 Vy 向量作为旋转矩阵的第二列
for(i=0;i<3;i++) RM[i][2]= Vz[i];// 将 Vz 向量作为旋转矩阵的第三列
//decompose roation matrix into Euler angles // 注释：将旋转矩阵分解为欧拉角（原文有拼写错误 "roation"）
double EulerAngleA, EulerAngleB, EulerAngleC;// 声明三个双精度浮点数变量，用于存储分解出的欧拉角 A, B, C
DecomposeMatrix(RM,0,0,0,&EulerAngleA,&EulerAngleB,&EulerAngleC);// 调用函数将旋转矩阵 RM 分解为欧拉角
(*Result).Axes[0]=RoundToEpsilon(O[0]);// 将计算出的原点X坐标，经过精度处理后，存入结果结构体
(*Result).Axes[1]=RoundToEpsilon(O[1]);// 将计算出的原点Y坐标，经过精度处理后，存入结果结构体
(*Result).Axes[2]=RoundToEpsilon(O[2]);// 将计算出的原点Z坐标，经过精度处理后，存入结果结构体
(*Result).Axes[3]=RoundToEpsilon(EulerAngleA);// 将计算出的欧拉角A，经过精度处理后，存入结果结构体
(*Result).Axes[4]=RoundToEpsilon(EulerAngleB);// 将计算出的欧拉角B，经过精度处理后，存入结果结构体
(*Result).Axes[5]=RoundToEpsilon(EulerAngleC);// 将计算出的欧拉角C，经过精度处理后，存入结果结构体
return0;// 函数成功完成，返回状态 0
}// FrameCalibration 函数体结束constants.h
#ifndefCONSTANTS_H // 如果宏 CONSTANTS_H 未被定义，则执行以下代码。这是一个防止头文件被重复包含的预处理指令。
#defineCONSTANTS_H// 定义宏 CONSTANTS_H，以标记该头文件已经被包含。
#defineMOVE_UNDEF0// 定义一个名为 MOVE_UNDEF 的宏，其值为0，表示未定义的运动类型。
#defineMOVE_LINE1// 定义一个名为 MOVE_LINE 的宏，其值为1，表示直线运动。
#defineMOVE_PTP2// 定义一个名为 MOVE_PTP 的宏，其值为2，表示点对点（Point-to-Point）运动。
#defineMOVE_CIRCLE3// 定义一个名为 MOVE_CIRCLE 的宏，其值为3，表示圆弧运动。
#defineMOVE_MCODE4// 定义一个名为 MOVE_MCODE 的宏，其值为4，表示执行M代码（辅助功能指令）。
#defineMOVE_DELAY5// 定义一个名为 MOVE_DELAY 的宏，其值为5，表示延时或暂停指令。
#defineMOVE_HOME6// 定义一个名为 MOVE_HOME 的宏，其值为6，表示返回原点（Home）的运动。
#defineMOVE_SUB7// 定义一个名为 MOVE_SUB 的宏，其值为7，表示调用子程序。
#defineMOVE_END8// 定义一个名为 MOVE_END 的宏，其值为8，表示程序结束。
#defineMOVE_GOTO9// 定义一个名为 MOVE_GOTO 的宏，其值为9，表示无条件跳转（GOTO）指令。
#defineMOVE_MCODE_SYNCH10// 定义一个名为 MOVE_MCODE_SYNCH 的宏，其值为10，表示同步执行M代码（等待M代码完成）。
#defineMOVE_TRK11// 定义一个名为 MOVE_TRK 的宏，其值为11，表示跟踪（Tracking）运动。
#defineMOVE_SPLINE12// 定义一个名为 MOVE_SPLINE 的宏，其值为12，表示样条曲线运动。
#defineMOVE_TANG13// 定义一个名为 MOVE_TANG 的宏，其值为13，表示相切运动。
#defineMOVE_SETDO14// 定义一个名为 MOVE_SETDO 的宏，其值为14，表示设置数字输出（Set Digital Output）。
#defineMOVE_RESETDO15// 定义一个名为 MOVE_RESETDO 的宏，其值为15，表示复位数字输出（Reset Digital Output）。
#defineMOVE_WAITDI16// 定义一个名为 MOVE_WAITDI 的宏，其值为16，表示等待数字输入（Wait for Digital Input）。
#defineMOVE_TOOL17// 定义一个名为 MOVE_TOOL 的宏，其值为17，表示工具切换或设置指令。
#defineTRF_EPSILON0.00001// 定义一个名为 TRF_EPSILON 的宏，这是一个非常小的值（epsilon），用于浮点数比较，以避免精度问题。
#defineSTATUS_ABORT3// 定义一个名为 STATUS_ABORT 的宏，其值为3，表示一个“中止”状态码。
#definePHASE_START0// 定义一个名为 PHASE_START 的宏，其值为0，表示运动或过程的“开始”阶段。
#definePHASE_MIDDLE1// 定义一个名为 PHASE_MIDDLE 的宏，其值为1，表示运动或过程的“中间”阶段。
#definePHASE_END2// 定义一个名为 PHASE_END 的宏，其值为2，表示运动或过程的“结束”阶段。
#defineFEED_DEFAULT0// 定义一个名为 FEED_DEFAULT 的宏，其值为0，表示默认的进给率类型。
#defineFEED_CART1// 定义一个名为 FEED_CART 的宏，其值为1，表示笛卡尔坐标系下的进给率（如 mm/s）。
#defineFEED_ANG2// 定义一个名为 FEED_ANG 的宏，其值为2，表示关节角度的进给率（如 deg/s）。
#defineBEZIER_CUBIC3// 定义一个名为 BEZIER_CUBIC 的宏，其值为3，可能表示使用三次贝塞尔曲线。
#defineBEZIER_QUARTIC4// 定义一个名为 BEZIER_QUARTIC 的宏，其值为4，可能表示使用四次贝塞尔曲线。
#defineBEZIER_XYZ3// 定义一个名为 BEZIER_XYZ 的宏，其值为3，可能特指用于XYZ空间坐标的三次贝塞尔曲线。
#defineBEZIER_SEGMENT0.333// 定义一个名为 BEZIER_SEGMENT 的宏，其值为0.333，约等于1/3，可能用于贝塞尔曲线的参数计算。
#defineWS_SUBPOINTS10// 定义一个名为 WS_SUBPOINTS 的宏，其值为10，可能表示在工作空间（Workspace）中检查路径时，每段路径的细分点数。
#defineMAX_SUBLEVEL10// 定义一个名为 MAX_SUBLEVEL 的宏，其值为10，表示子程序或函数调用的最大嵌套层数。
#definePOINT_SAME2// 定义一个名为 POINT_SAME 的宏，其值为2，可能是一个状态码，表示两个点被认为是相同的（在容差范围内）。
#defineAUX_MAX6// 定义一个名为 AUX_MAX 的宏，其值为6，表示机器人所支持的最大辅助轴数量。
#endif// 结束 #ifndef CONSTANTS_H 的条件编译块。Frame.h Frame.c
FRAME.H 文件
#ifndefFRAME_H // 如果宏 FRAME_H 未被定义
#defineFRAME_H// 则定义宏 FRAME_H，这是标准的头文件保护宏，用于防止头文件被重复包含
#include<math.h>// 包含标准数学库头文件，提供数学函数
#include"constants.h"// 包含项目自定义的常量定义头文件
#include"trig.h"// 包含三角函数相关的头文件，可能定义了如 cosd, sind 等以角度为单位的三角函数
/* quaternion datatype */// 注释块：四元数数据类型
typedefstructQuat_Type// 定义一个名为 Quat_Type 的结构体
{// 结构体开始
double w,x,y,z;// 定义四个双精度浮点数成员，分别代表四元数的 w, x, y, z 分量
} Quat_Type;// 结构体定义结束，并为其指定别名 Quat_Type
/* Declaration of common functions used for frames operations: translations, rotations... */// 注释块：声明用于坐标系操作（平移、旋转等）的常用函数
unsignedshortComposeMatrix(double RM[3][3],double A,double B,double C);// 声明函数：根据RPY（Roll, Pitch, Yaw）欧拉角(A, B, C)构建一个3x3的旋转矩阵RM
unsignedshortDecomposeMatrix(double RM[3][3],double A_actual,double B_actual,double C_actual,double*A,double*B,double*C);// 声明函数：将一个3x3的旋转矩阵RM分解为RPY欧拉角(A, B, C)，并参考当前的角度值来选择最接近的解
unsignedshortMatMult(double M1[3][3],double M2[3][3],double M3[3][3]);// 声明函数：计算两个3x3矩阵M1和M2的乘积，结果存入M3
unsignedshortSubFrame3D(double P1[6],double F1[6],double A_actual,double B_actual,double C_actual,double P0[6]);// 声明函数：在3D空间中，将点P1从坐标系F1变换到基坐标系，结果存入P0
unsignedshortAddFrame3D(double P1[6],double F1[6],double A_actual,double B_actual,double C_actual,double P0[6]);// 声明函数：在3D空间中，将基坐标系中的点P1变换到坐标系F1下，结果存入P0
unsignedshortSubFrame2D(double P1[6],double F1[6],double P0[6]);// 声明函数：在2D空间中，将点P1从坐标系F1变换到基坐标系，结果存入P0（仅考虑Z轴旋转）
unsignedshortAddFrame2D(double P1[6],double F1[6],double P0[6]);// 声明函数：在2D空间中，将基坐标系中的点P1变换到坐标系F1下，结果存入P0（仅考虑Z轴旋转）
unsignedshortSubFrameTool2D(double Path[6],double Frame[6],double Tool[6],double Mount[6]);// 声明函数：在2D中，根据路径点、用户坐标系和工具坐标系，计算机器人法兰盘的位姿
unsignedshortAddFrameTool2D(double Mount[6],double Frame[6],double Tool[6],double Path[6]);// 声明函数：在2D中，根据机器人法兰盘位姿、用户坐标系和工具坐标系，计算工具末端的路径点
unsignedshortAddFrameTool3D(double Mount[6],double Frame[6],double Tool[6],double A_actual,double B_actual,double C_actual,double Path[6]);// 声明函数：在3D中，根据机器人法兰盘位姿、用户坐标系和工具坐标系，计算工具末端的路径点
unsignedshortSubFrameTool3D(double Path[6],double Frame[6],double Tool[6],double A_actual,double B_actual,double C_actual,double Mount[6]);// 声明函数：在3D中，根据路径点、用户坐标系和工具坐标系，计算机器人法兰盘的位姿
unsignedshortNormalizeQuat(Quat_Type* q);// 声明函数：将一个四元数 q 单位化
unsignedshortMatrixToQuat(double RM[3][3], Quat_Type* q);// 声明函数：将一个3x3的旋转矩阵RM转换为四元数 q
unsignedshortQuatToMatrix(Quat_Type q,double RM[3][3]);// 声明函数：将一个四元数 q 转换为3x3的旋转矩阵RM
doubleAngleBetweenQuat(Quat_Type q1, Quat_Type* q2);// 声明函数：计算两个四元数q1和q2之间的夹角，并可能反转q2以确保夹角为锐角
unsignedshortSlerp(Quat_Type q1, Quat_Type q2, Quat_Type* q,double angle,double u);// 声明函数：执行球面线性插值（Slerp），根据参数u在四元数q1和q2之间插值，结果存入q
unsignedshortEulerToQuat(double A,double B,double C, Quat_Type* q);// 声明函数：将RPY欧拉角(A, B, C)转换为四元数 q
unsignedshortQuatToEuler(Quat_Type q,double A_actual,double B_actual,double C_actual,double*A,double*B,double*C);// 声明函数：将四元数 q 转换为RPY欧拉角(A, B, C)，并参考当前角度选择解
#endif// 结束 #ifndef FRAME_H 的宏定义条件块Frame.c 文件
#include"Frame.h"// 包含此源文件对应的头文件 "Frame.h"
#include<math.h>// 包含标准数学库头文件
#include"constants.h"// 包含项目自定义的常量定义头文件
#include"trig.h"// 包含三角函数相关的头文件
#definemax(a,b)((a>b)?a:b )// 定义一个宏函数 max，返回两个数中的较大者
unsignedshortComposeMatrix(double RM[3][3],double A,double B,double C)// 定义函数：根据RPY欧拉角(A, B, C)构建旋转矩阵RM
{// compose rotation matrix from RPY angles // 函数体开始，注释：从RPY角构建旋转矩阵
RM[0][0]=cosd(C)*cosd(B);// 计算旋转矩阵的第0行第0列元素，使用以角度为单位的余弦函数cosd
RM[0][1]=cosd(C)*sind(B)*sind(A)-sind(C)*cosd(A);// 计算旋转矩阵的第0行第1列元素
RM[0][2]=cosd(C)*sind(B)*cosd(A)+sind(C)*sind(A);// 计算旋转矩阵的第0行第2列元素
RM[1][0]=sind(C)*cosd(B);// 计算旋转矩阵的第1行第0列元素
RM[1][1]=sind(C)*sind(B)*sind(A)+cosd(C)*cosd(A);// 计算旋转矩阵的第1行第1列元素
RM[1][2]=sind(C)*sind(B)*cosd(A)-cosd(C)*sind(A);// 计算旋转矩阵的第1行第2列元素
RM[2][0]=-sind(B);// 计算旋转矩阵的第2行第0列元素
RM[2][1]=cosd(B)*sind(A);// 计算旋转矩阵的第2行第1列元素
RM[2][2]=cosd(B)*cosd(A);// 计算旋转矩阵的第2行第2列元素
return0;// 函数执行成功，返回0
}
unsignedshortDecomposeMatrix(double RM[3][3],double A_actual,double B_actual,double C_actual,double*A,double*B,double*C)// 定义函数：将旋转矩阵分解为RPY欧拉角
{// decompose rotation matrix into RPY angles // 函数体开始，注释：将旋转矩阵分解为RPY角
double A_temp[2],B_temp[2],C_temp[2],ABC_dist[2];// 声明数组用于存放两组可能的欧拉角解及其与当前值的距离
double cosd_B =sqrt(1-RM[2][0]*RM[2][0]);// 计算B角的余弦值，B角可以从RM[2][0] = -sin(B)中得到
B_temp[0]=atan2d(-RM[2][0],cosd_B);// 计算B角的第一个可能解
B_temp[1]=atan2d(-RM[2][0],-cosd_B);// 计算B角的第二个可能解
if(fabs(cosd_B)>TRF_EPSILON){// 检查是否处于奇异点（万向节死锁），即cos(B)是否接近于0
C_temp[0]=atan2d(RM[1][0],RM[0][0]);// 如果不是奇异点，计算C角的第一个解
C_temp[1]=atan2d(-RM[1][0],-RM[0][0]);// 计算C角的第二个解
A_temp[0]=atan2d(RM[2][1],RM[2][2]);// 计算A角的第一个解
A_temp[1]=atan2d(-RM[2][1],-RM[2][2]);// 计算A角的第二个解
}else{//singularity - choose A=A_actual // 如果是奇异点，注释：奇异点 - 选择 A=A_actual
A_temp[0]= A_temp[1]= A_actual;// 在奇异点情况下，将A角固定为当前A角值，以避免不确定性
C_temp[0]= C_temp[1]= A_actual -sign(-RM[2][0])*atan2d(RM[0][1]*sign(-RM[2][0]),RM[1][1]);// 根据A角和矩阵元素计算C角
}
//A, C modulo +-2PI to bring them closer to current values // 注释：对A和C进行模2PI运算，使其更接近当前值
A_temp[0]=Modulo2PI(A_temp[0],A_actual);// 将第一个A解调整到以A_actual为中心的2PI范围内
A_temp[1]=Modulo2PI(A_temp[1],A_actual);// 将第二个A解调整到以A_actual为中心的2PI范围内
C_temp[0]=Modulo2PI(C_temp[0],C_actual);// 将第一个C解调整到以C_actual为中心的2PI范围内
C_temp[1]=Modulo2PI(C_temp[1],C_actual);// 将第二个C解调整到以C_actual为中心的2PI范围内
//calculate distance of the two solutions from actual values // 注释：计算两组解与当前值的距离
ABC_dist[0]=fabs(A_temp[0]-A_actual)+fabs(B_temp[0]-B_actual)+fabs(C_temp[0]-C_actual);// 计算第一组解的欧拉角与当前角度的总绝对差
ABC_dist[1]=fabs(A_temp[1]-A_actual)+fabs(B_temp[1]-B_actual)+fabs(C_temp[1]-C_actual);// 计算第二组解的欧拉角与当前角度的总绝对差
//keep same pose for wrist // 注释：保持手腕姿态一致
if(B_actual <0){//use solution with negative B // 如果当前B角为负，注释：使用B为负的解
if((B_temp[0]<0)&&(B_temp[1]>=0)){//use B_temp[0] // 如果解0的B为负而解1的B为正，注释：使用B_temp[0]
*A=A_temp[0];// 选择第一组解
*B=B_temp[0];// 选择第一组解
*C=C_temp[0];// 选择第一组解
}elseif((B_temp[1]<0)&&(B_temp[0]>=0)){//use B_temp[1] // 如果解1的B为负而解0的B为正，注释：使用B_temp[1]
*A=A_temp[1];// 选择第二组解
*B=B_temp[1];// 选择第二组解
*C=C_temp[1];// 选择第二组解
}else{//use closest solution to current values // 否则（两个解的B符号相同或都为0），注释：使用离当前值最近的解
if(ABC_dist[0]<= ABC_dist[1]){// 比较两组解的距离
*A = A_temp[0];// 选择距离更近的第一组解
*B = B_temp[0];// 选择距离更近的第一组解
*C = C_temp[0];// 选择距离更近的第一组解
}else{// 否则
*A = A_temp[1];// 选择距离更近的第二组解
*B = B_temp[1];// 选择距离更近的第二组解
*C = C_temp[1];// 选择距离更近的第二组解
}
}
}else{//use solution with positive B // 如果当前B角为正或零，注释：使用B为正的解
if((B_temp[0]>=0)&&(B_temp[1]<0)){//use B_temp[0] // 如果解0的B为正而解1的B为负，注释：使用B_temp[0]
*A = A_temp[0];// 选择第一组解
*B = B_temp[0];// 选择第一组解
*C = C_temp[0];// 选择第一组解
}elseif((B_temp[1]>=0)&&(B_temp[0]<0)){//use B_temp[1] // 如果解1的B为正而解0的B为负，注释：使用B_temp[1]
*A = A_temp[1];// 选择第二组解
*B = B_temp[1];// 选择第二组解
*C = C_temp[1];// 选择第二组解
}else{//use closest solution to current values // 否则，注释：使用离当前值最近的解
if(ABC_dist[0]<= ABC_dist[1]){// 比较两组解的距离
*A = A_temp[0];// 选择距离更近的第一组解
*B = B_temp[0];// 选择距离更近的第一组解
*C = C_temp[0];// 选择距离更近的第一组解
}else{// 否则
*A = A_temp[1];// 选择距离更近的第二组解
*B = B_temp[1];// 选择距离更近的第二组解
*C = C_temp[1];// 选择距离更近的第二组解
}
}
}
//adjust positions of C with +-2PI to bring it closer to desired value // 注释：调整C的位置，使用+/-2PI使其更接近期望值
*C =Modulo2PI(*C,C_actual);// 最终对C角进行模2PI运算，使其靠近C_actual
return0;// 函数执行成功，返回0
}
unsignedshortMatMult(double M1[3][3],double M2[3][3],double M3[3][3])// 定义函数：计算矩阵乘法 M3 = M1 * M2
{// calculate M3 = M1*M2 // 函数体开始，注释：计算M3 = M1*M2
int i,j,k;// 声明三个整型变量 i, j, k 作为循环计数器
for(i=0;i<3;i++){// 遍历结果矩阵M3的行
for(j=0;j<3;j++){// 遍历结果矩阵M3的列
M3[i][j]=0;// 将当前元素初始化为0
}
}
for(i=0;i<3;i++){// 遍历结果矩阵M3的行
for(j=0;j<3;j++){// 遍历结果矩阵M3的列
for(k=0;k<3;k++){// 遍历M1的列和M2的行
M3[i][j]= M3[i][j]+(M1[i][k]* M2[k][j]);// 执行矩阵乘法的核心计算：行乘以列
}
}
}
return0;// 函数执行成功，返回0
}
unsignedshortSubFrame3D(double P1[6],double F1[6],double A_actual,double B_actual,double C_actual,double P0[6])// 定义函数：将点P1从坐标系F1变换到基坐标系
{//calculate point P0 in base frame given point P1 in frame F1 // 函数体开始，注释：已知F1坐标系中的点P1，计算其在基坐标系中的坐标P0
double Frame_RotMat[3][3];// 声明一个3x3矩阵，用于存储坐标系F1的旋转矩阵
double Start_RotMat[3][3];// 声明一个3x3矩阵，用于存储点P1的姿态对应的旋转矩阵
double End_RotMat[3][3];// 声明一个3x3矩阵，用于存储最终的旋转矩阵
double A,B,C;// 声明三个双精度浮点数，用于存储分解出的欧拉角
ComposeMatrix(Frame_RotMat,F1[3],F1[4],F1[5]);// 根据F1的欧拉角构建其旋转矩阵
ComposeMatrix(Start_RotMat,P1[3],P1[4],P1[5]);// 根据P1的欧拉角构建其旋转矩阵
P0[0]= Frame_RotMat[0][0]* P1[0]+ Frame_RotMat[0][1]* P1[1]+ Frame_RotMat[0][2]* P1[2]+ F1[0];// 计算P0的X坐标：旋转+平移
P0[1]= Frame_RotMat[1][0]* P1[0]+ Frame_RotMat[1][1]* P1[1]+ Frame_RotMat[1][2]* P1[2]+ F1[1];// 计算P0的Y坐标：旋转+平移
P0[2]= Frame_RotMat[2][0]* P1[0]+ Frame_RotMat[2][1]* P1[1]+ Frame_RotMat[2][2]* P1[2]+ F1[2];// 计算P0的Z坐标：旋转+平移
MatMult(Frame_RotMat,Start_RotMat,End_RotMat);// 计算最终的姿态：F1的旋转矩阵乘以P1的旋转矩阵
DecomposeMatrix(End_RotMat,A_actual,B_actual,C_actual,&A,&B,&C);// 将最终的旋转矩阵分解为欧拉角
//keep orientation within 2PI from origin // 注释：保持姿态角在原点的2PI范围内
P0[3]=Modulo2PI(A,0);// 将计算出的A角调整到以0为中心的2PI范围内
P0[4]=Modulo2PI(B,0);// 将计算出的B角调整到以0为中心的2PI范围内
P0[5]=Modulo2PI(C,0);// 将计算出的C角调整到以0为中心的2PI范围内
return0;// 函数执行成功，返回0
}
unsignedshortAddFrame3D(double P1[6],double F1[6],double A_actual,double B_actual,double C_actual,double P0[6])// 定义函数：将基坐标系中的点P1变换到坐标系F1
{//calculate point P0 in frame F1 given point P1 in base frame // 函数体开始，注释：已知基坐标系中的点P1，计算其在F1坐标系中的坐标P0
double Frame_RotMat[3][3];// 声明一个3x3矩阵，用于存储坐标系F1的旋转矩阵
double Start_RotMat[3][3];// 声明一个3x3矩阵，用于存储点P1的姿态对应的旋转矩阵
double End_RotMat[3][3];// 声明一个3x3矩阵，用于存储最终的旋转矩阵
double A,B,C;// 声明三个双精度浮点数，用于存储分解出的欧拉角
ComposeMatrix(Frame_RotMat,F1[3],F1[4],F1[5]);// 根据F1的欧拉角构建其旋转矩阵
ComposeMatrix(Start_RotMat,P1[3],P1[4],P1[5]);// 根据P1的欧拉角构建其旋转矩阵
//transpose Frame rotation matrix // 注释：转置坐标系F1的旋转矩阵（因为需要进行逆变换）
double tmpFrame;// 声明一个临时变量用于交换元素
tmpFrame = Frame_RotMat[0][1];// 交换 [0][1] 和 [1][0]
Frame_RotMat[0][1]= Frame_RotMat[1][0];// ...
Frame_RotMat[1][0]= tmpFrame;// ...
tmpFrame = Frame_RotMat[0][2];// 交换 [0][2] 和 [2][0]
Frame_RotMat[0][2]= Frame_RotMat[2][0];// ...
Frame_RotMat[2][0]= tmpFrame;// ...
tmpFrame = Frame_RotMat[1][2];// 交换 [1][2] 和 [2][1]
Frame_RotMat[1][2]= Frame_RotMat[2][1];// ...
Frame_RotMat[2][1]= tmpFrame;// ...
P0[0]= Frame_RotMat[0][0]* P1[0]+ Frame_RotMat[0][1]* P1[1]+ Frame_RotMat[0][2]* P1[2]-(Frame_RotMat[0][0]* F1[0]+ Frame_RotMat[0][1]* F1[1]+ Frame_RotMat[0][2]* F1[2]);// 计算P0的X坐标：逆旋转+逆平移
P0[1]= Frame_RotMat[1][0]* P1[0]+ Frame_RotMat[1][1]* P1[1]+ Frame_RotMat[1][2]* P1[2]-(Frame_RotMat[1][0]* F1[0]+ Frame_RotMat[1][1]* F1[1]+ Frame_RotMat[1][2]* F1[2]);// 计算P0的Y坐标
P0[2]= Frame_RotMat[2][0]* P1[0]+ Frame_RotMat[2][1]* P1[1]+ Frame_RotMat[2][2]* P1[2]-(Frame_RotMat[2][0]* F1[0]+ Frame_RotMat[2][1]* F1[1]+ Frame_RotMat[2][2]* F1[2]);// 计算P0的Z坐标
MatMult(Frame_RotMat,Start_RotMat,End_RotMat);// 计算最终的姿态：F1的逆旋转矩阵乘以P1的旋转矩阵
DecomposeMatrix(End_RotMat,A_actual,B_actual,C_actual,&A,&B,&C);// 将最终的旋转矩阵分解为欧拉角
//keep orientation within 2PI from origin // 注释：保持姿态角在原点的2PI范围内
P0[3]=Modulo2PI(A,0);// 将计算出的A角调整到以0为中心的2PI范围内
P0[4]=Modulo2PI(B,0);// 将计算出的B角调整到以0为中心的2PI范围内
P0[5]=Modulo2PI(C,0);// 将计算出的C角调整到以0为中心的2PI范围内
return0;// 函数执行成功，返回0
}
unsignedshortSubFrame2D(double P1[6],double F1[6],double P0[6])// 定义函数：2D情况下，将点P1从F1变换到基坐标系
{//calculate point P0 in base frame given point P1 in frame F1 - 2D version, only rotations around Z are possible // 函数体开始，注释：2D版本，只可能绕Z轴旋转
P0[0]= P1[0]*cosd(F1[3])- P1[1]*sind(F1[3])+ F1[0];// 计算P0的X坐标，应用Z轴旋转和平移
P0[1]= P1[0]*sind(F1[3])+ P1[1]*cosd(F1[3])+ F1[1];// 计算P0的Y坐标，应用Z轴旋转和平移
P0[2]= P1[2]+ F1[2];// Z坐标直接相加
P0[3]= P1[3]+ F1[3];// Z轴旋转角度直接相加 (这里F1[3]是绕Z的角，P1[3]也是)
//keep orientation within 2PI from origin // 注释：保持姿态角在原点的2PI范围内
P0[3]=Modulo2PI(P0[3],0);// 将结果角度调整到以0为中心的2PI范围内
return0;// 函数执行成功，返回0
}
unsignedshortAddFrame2D(double P1[6],double F1[6],double P0[6])// 定义函数：2D情况下，将基坐标系的点P1变换到F1
{//calculate point P0 in frame F1 given point P1 in base frame - 2D version, only rotations around Z are possible // 函数体开始，注释：2D版本，只可能绕Z轴旋转
P0[0]= P1[0]*cosd(F1[3])+ P1[1]*sind(F1[3])- F1[0]*cosd(F1[3])- F1[1]*sind(F1[3]);// 计算P0的X坐标，应用逆向的Z轴旋转和平移
P0[1]=- P1[0]*sind(F1[3])+ P1[1]*cosd(F1[3])+ F1[0]*sind(F1[3])- F1[1]*cosd(F1[3]);// 计算P0的Y坐标，应用逆向的Z轴旋转和平移
P0[2]= P1[2]- F1[2];// Z坐标直接相减
P0[3]= P1[3]- F1[3];// Z轴旋转角度直接相减
//keep orientation within 2PI from origin // 注释：保持姿态角在原点的2PI范围内
P0[3]=Modulo2PI(P0[3],0);// 将结果角度调整到以0为中心的2PI范围内
return0;// 函数执行成功，返回0
}
unsignedshortSubFrameTool2D(double Path[6],double Frame[6],double Tool[6],double Mount[6])// 定义函数：在2D中，计算法兰盘位姿
{//calculate mounting point (in base frame) given path axes positions, current frame and tool // 函数体开始，注释：已知路径点、当前坐标系和工具，计算法兰盘点位
double tmpAxes[6];// 声明一个临时数组，用于存储中间结果
double InvertedTool[6];// 声明一个数组，用于存储反向的工具变换
double ZeroFrame[6]={0,0,0,0,0,0};// 声明一个零坐标系，用于计算逆变换
SubFrame2D(Path,Frame,tmpAxes);// 1. 将路径点(Path)从用户坐标系(Frame)变换到基坐标系，结果存入tmpAxes
AddFrame2D(ZeroFrame,Tool,InvertedTool);// 2. 计算工具(Tool)的逆变换，结果存入InvertedTool
SubFrame2D(InvertedTool,tmpAxes,Mount);// 3. 将逆工具应用到基坐标系下的路径点上，得到法兰盘位姿(Mount)
return0;// 函数执行成功，返回0
}
unsignedshortAddFrameTool2D(double Mount[6],double Frame[6],double Tool[6],double Path[6])// 定义函数：在2D中，计算路径点
{//calculate path axes positions given mounting point (in base frame), current frame and tool // 函数体开始，注释：已知法兰盘点位、当前坐标系和工具，计算路径轴位置
double tmpAxes[6];// 声明一个临时数组，用于存储中间结果
SubFrame2D(Tool,Mount,tmpAxes);// 1. 将工具(Tool)变换应用到法兰盘位姿(Mount)上，得到基坐标系下的TCP位置
AddFrame2D(tmpAxes,Frame,Path);// 2. 将基坐标系下的TCP位置变换到用户坐标系(Frame)中，得到路径点(Path)
return0;// 函数执行成功，返回0
}
unsignedshortAddFrameTool3D(double Mount[6],double Frame[6],double Tool[6],double A_actual,double B_actual,double C_actual,double Path[6])// 定义函数：在3D中，计算路径点
{//calculate path axes positions given mounting point (in base frame), current frame and tool // 函数体开始，注释：已知法兰盘点位、当前坐标系和工具，计算路径轴位置
double tmpAxes[6];// 声明一个临时数组，用于存储中间结果
SubFrame3D(Tool,Mount,A_actual,B_actual,C_actual,tmpAxes);// 1. 将工具(Tool)变换应用到法兰盘位姿(Mount)上，得到基坐标系下的TCP位置
AddFrame3D(tmpAxes,Frame,A_actual,B_actual,C_actual,Path);// 2. 将基坐标系下的TCP位置变换到用户坐标系(Frame)中，得到路径点(Path)
return0;// 函数执行成功，返回0
}
unsignedshortSubFrameTool3D(double Path[6],double Frame[6],double Tool[6],double A_actual,double B_actual,double C_actual,double Mount[6])// 定义函数：在3D中，计算法兰盘位姿
{//calculate mounting point (in base frame) given path axes positions, current frame and tool // 函数体开始，注释：已知路径点、当前坐标系和工具，计算法兰盘点位
double tmpAxes[6];// 声明一个临时数组，用于存储中间结果
double InvertedTool[6];// 声明一个数组，用于存储反向的工具变换
double ZeroFrame[6]={0,0,0,0,0,0};// 声明一个零坐标系，用于计算逆变换
SubFrame3D(Path,Frame,A_actual,B_actual,C_actual,tmpAxes);// 1. 将路径点(Path)从用户坐标系(Frame)变换到基坐标系
AddFrame3D(ZeroFrame,Tool,A_actual,B_actual,C_actual,InvertedTool);// 2. 计算工具(Tool)的逆变换
SubFrame3D(InvertedTool,tmpAxes,A_actual,B_actual,C_actual,Mount);// 3. 将逆工具应用到基坐标系下的路径点上，得到法兰盘位姿(Mount)
return0;// 函数执行成功，返回0
}
unsignedshortNormalizeQuat(Quat_Type* q)// 定义函数：单位化四元数q
{// 函数体开始
double norm =sqrt(q->w*q->w+q->x*q->x+q->y*q->y+q->z*q->z);// 计算四元数的模（范数）
if(norm >= TRF_EPSILON){// 检查模是否大于一个很小的数，避免除以零
q->w /= norm;// 将w分量除以模
q->x /= norm;// 将x分量除以模
q->y /= norm;// 将y分量除以模
q->z /= norm;// 将z分量除以模
}// if 结束
return0;// 函数执行成功，返回0
}
unsignedshortMatrixToQuat(double RM[3][3], Quat_Type* q)// 定义函数：将旋转矩阵RM转换为四元数q
{// 函数体开始
q->w =sqrt(max(0,1+ RM[0][0]+ RM[1][1]+ RM[2][2]))/2.0;// 计算四元数的w分量，使用max(0, ...)避免对负数开方
q->x =sqrt(max(0,1+ RM[0][0]- RM[1][1]- RM[2][2]))/2.0;// 计算四元数的x分量
q->y =sqrt(max(0,1- RM[0][0]+ RM[1][1]- RM[2][2]))/2.0;// 计算四元数的y分量
q->z =sqrt(max(0,1- RM[0][0]- RM[1][1]+ RM[2][2]))/2.0;// 计算四元数的z分量
q->x =copysign(q->x, RM[2][1]- RM[1][2]);// 根据旋转矩阵元素的差值确定x分量的符号
q->y =copysign(q->y, RM[0][2]- RM[2][0]);// 根据旋转矩阵元素的差值确定y分量的符号
q->z =copysign(q->z, RM[1][0]- RM[0][1]);// 根据旋转矩阵元素的差值确定z分量的符号
NormalizeQuat(q);// 将转换后的四元数单位化
return0;// 函数执行成功，返回0
}
unsignedshortQuatToMatrix(Quat_Type q,double RM[3][3])// 定义函数：将四元数q转换为旋转矩阵RM
{// 函数体开始
double w;// 声明一个双精度浮点数w，用于存储中间计算结果
double n = q.w*q.w+q.x*q.x+q.y*q.y+q.z*q.z;// 计算四元数的模的平方
if(fabs(n)< TRF_EPSILON){// 检查模的平方是否接近于零
w =0;// 如果是，则设置w为0
}else{// 否则
w =2/n;// 计算w = 2 / (模的平方)
}// else 结束
double wx = w * q.w * q.x;// 预计算一些乘积项以提高效率
double wy = w * q.w * q.y;// ...
double wz = w * q.w * q.z;// ...
double xx = w * q.x * q.x;// ...
double xy = w * q.x * q.y;// ...
double xz = w * q.x * q.z;// ...
double yy = w * q.y * q.y;// ...
double yz = w * q.y * q.z;// ...
double zz = w * q.z * q.z;// ...
RM[0][0]=1-(yy+zz);// 根据四元数分量计算旋转矩阵的元素
RM[0][1]=(xy-wz);// ...
RM[0][2]=(xz+wy);// ...
RM[1][0]=(xy+wz);// ...
RM[1][1]=1-(xx+zz);// ...
RM[1][2]=(yz-wx);// ...
RM[2][0]=(xz-wy);// ...
RM[2][1]=(yz+wx);// ...
RM[2][2]=1-(yy+xx);// ...
//added for numerical stability - otherwise it is not robust for 90deg angles // 注释：为数值稳定性添加 - 否则对于90度角不够鲁棒
int i, j;// 声明两个整型变量 i, j 作为循环计数器
for(i=0;i<3;i++){// 遍历矩阵的行
for(j=0;j<3;j++){// 遍历矩阵的列
if(RM[i][j]>1) RM[i][j]=1;// 如果元素值因计算误差略大于1，则钳位到1
if(RM[i][j]<-1) RM[i][j]=-1;// 如果元素值因计算误差略小于-1，则钳位到-1
}
}
return0;// 函数执行成功，返回0
}
doubleAngleBetweenQuat(Quat_Type q1, Quat_Type* q2)// 定义函数：计算两个四元数之间的夹角
{//calculate angle between quaternions (in radians) -> reverse q2 if angle is obtuse // 函数体开始，注释：计算四元数之间的夹角（弧度） -> 如果角度是钝角则反转q2
double q1_norm =sqrt(q1.w*q1.w+q1.x*q1.x+q1.y*q1.y+q1.z*q1.z);// 计算四元数q1的模
double q2_norm =sqrt(q2->w*q2->w+q2->x*q2->x+q2->y*q2->y+q2->z*q2->z);// 计算四元数q2的模
double q12 = q1.w*q2->w+q1.x*q2->x+q1.y*q2->y+q1.z*q2->z;// 计算两个四元数的点积
double cos_alpha = q12/(q1_norm*q2_norm);// 计算夹角的余弦值
//clamp to avoid numerical errors // 注释：钳位以避免数值错误
if(cos_alpha >1){// 如果余弦值因计算误差略大于1
cos_alpha =1;// 钳位到1
}elseif(cos_alpha <-1){// 如果余弦值因计算误差略小于-1
cos_alpha =-1;// 钳位到-1
}
if(cos_alpha<0){// 如果夹角是钝角（余弦值为负）
q2->w =- q2->w;// 反转q2的所有分量，因为q和-q表示相同的旋转，但这样可以得到锐角路径
q2->x =- q2->x;// ...
q2->y =- q2->y;// ...
q2->z =- q2->z;// ...
}
returnacos(fabs(cos_alpha));// 返回夹角的绝对值（弧度），确保是锐角
}
unsignedshortSlerp(Quat_Type q1, Quat_Type q2, Quat_Type* q,double angle,double u)// 定义函数：执行球面线性插值（Slerp）
{// 函数体开始
/* Slerp works also with u outside 0..1 -> it linearly extrapolates
if ((u>1)||(u<0))
return 255;
*/// 注释块：Slerp也适用于u在0..1之外的情况 -> 它会进行线性外插。此部分代码被注释掉了。
if(angle < TRF_EPSILON){//use linear interpolation for small angles // 如果夹角非常小，注释：对小角度使用线性插值
q->w = q1.w*(1-u)+ q2.w*u;// 对w分量进行线性插值（Nlerp）
q->x = q1.x*(1-u)+ q2.x*u;// 对x分量进行线性插值
q->y = q1.y*(1-u)+ q2.y*u;// 对y分量进行线性插值
q->z = q1.z*(1-u)+ q2.z*u;// 对z分量进行线性插值
}else{//slerp // 否则，角度足够大，执行标准的Slerp，注释：slerp
q->w = q1.w*sin((1-u)*angle)/sin(angle)+ q2.w*sin(u*angle)/sin(angle);// 根据Slerp公式计算插值后四元数的w分量
q->x = q1.x*sin((1-u)*angle)/sin(angle)+ q2.x*sin(u*angle)/sin(angle);// 计算x分量
q->y = q1.y*sin((1-u)*angle)/sin(angle)+ q2.y*sin(u*angle)/sin(angle);// 计算y分量
q->z = q1.z*sin((1-u)*angle)/sin(angle)+ q2.z*sin(u*angle)/sin(angle);// 计算z分量
}
NormalizeQuat(q);// 将插值结果四元数单位化，特别是对于线性插值（Nlerp）这是必须的
return0;// 函数执行成功，返回0
}
unsignedshortEulerToQuat(double A,double B,double C, Quat_Type* q)// 定义函数：将欧拉角转换为四元数
{// 函数体开始
double RotMat[3][3];// 声明一个3x3的旋转矩阵
ComposeMatrix(RotMat,A,B,C);// 首先根据欧拉角构建旋转矩阵
MatrixToQuat(RotMat,q);// 然后将旋转矩阵转换为四元数
return0;// 函数执行成功，返回0
}
unsignedshortQuatToEuler(Quat_Type q,double A_actual,double B_actual,double C_actual,double*A,double*B,double*C)// 定义函数：将四元数转换为欧拉角
{// 函数体开始
double RotMat[3][3];// 声明一个3x3的旋转矩阵
NormalizeQuat(&q);// 首先单位化输入四元数以确保其为单位四元数
QuatToMatrix(q,RotMat);// 将单位四元数转换为旋转矩阵
DecomposeMatrix(RotMat,A_actual,B_actual,C_actual,A,B,C);// 将旋转矩阵分解为欧拉角，并参考当前角度选择最近的解
return0;// 函数执行成功，返回0
}Interpreter.h Interpreter.c
interpreter.h 文件
#ifndefINTERPRETER_H // 如果宏 INTERPRETER_H 未被定义
#defineINTERPRETER_H// 则定义宏 INTERPRETER_H，这是标准的头文件保护宏，防止头文件被重复包含
#include<string.h>// 包含标准字符串处理库头文件，提供如 strlen, strstr 等函数
#include<stdlib.h>// 包含标准库头文件，提供如 atoi, atof 等字符串转换函数
#include<math.h>// 包含标准数学库头文件，提供数学函数
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了 MotionPackage_Type 等结构
#include"Misc.h"// 包含一些杂项功能或工具函数的头文件
/* Declaration of interpreter functions */// 注释块：解释器函数声明
char*my_strcasestr(constchar*arg1,constchar*arg2);// 声明一个自定义函数，用于在字符串 arg1 中不区分大小写地查找子字符串 arg2
/* converts a string block into a motion package */// 注释块：将一个字符串块转换为一个运动包
unsignedshortInterpreter(char* Block, MotionPackage_Type* Package);// 声明解释器主函数，该函数解析一个字符串指令（Block），并填充一个运动包结构体（Package）
#endif// 结束 #ifndef INTERPRETER_H 的宏定义条件块interpreter.c 文件
#include"interpreter.h"// 包含此源文件对应的头文件 "interpreter.h"
#include<string.h>// 包含标准字符串处理库头文件
#include<stdlib.h>// 包含标准库头文件
#include<math.h>// 包含标准数学库头文件
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了错误码如 ERR_IP_CONFLICT
#include"Misc.h"// 包含一些杂项功能或工具函数的头文件
char*my_strcasestr(constchar*arg1,constchar*arg2)// 定义一个不区分大小写的字符串查找函数
{// 函数体开始
constchar*a,*b;// 声明两个字符指针 a 和 b
for(;*arg1;*arg1++){// 遍历主字符串 arg1 中的每一个字符
a = arg1;// 指针 a 指向当前主字符串的扫描位置
b = arg2;// 指针 b 指向要查找的子字符串的开头
while((*a++|32)==(*b++|32))// 循环比较字符，使用'| 32'将字符转换为小写进行比较
if(!*b)// 如果子字符串 b 已经比较到末尾（即所有字符都匹配）
return(char*)arg1;// 返回在主字符串中找到的匹配起始位置
}// for 循环结束
returnNULL;// 如果遍历完主字符串仍未找到匹配，返回 NULL
}
staticdoublestr2double(char* s)// 定义一个静态函数，将字符串 s 转换为 double 类型浮点数
{// converts string into double - does not support exponential notation - atoff does not work in AS C // 函数体开始，注释：将字符串转为double，不支持科学计数法，因为atof在AS C中不工作
double result =0;// 声明一个双精度浮点数变量 result 并初始化为0，用于存放转换结果
int i =0;// 声明一个整型变量 i 并初始化为0，用作字符串索引
int sign =1;// 声明一个整型变量 sign 并初始化为1，用于记录数字的符号
short digits =0;//声明一个短整型变量 digits，标记是否已开始解析数字
short decpoint =0;//声明一个短整型变量 decpoint，标记是否已遇到小数点
if(!strlen(s))return(0.0);// 如果字符串为空，直接返回0.0
int decimalPointLoc =strlen(s);// 声明一个整型变量，记录小数点的位置，默认为字符串末尾
for(;s[i]!='\0';i++)// 遍历字符串 s 中的每一个字符
{// for 循环体开始
if((s[i]==' ')&&(digits ==0)&&(sign ==1))//如果当前字符是空格，并且还未开始解析数字和符号
{// ignore empty spaces // 注释：忽略空格
continue;// 继续下一次循环
}
if((s[i]=='-')&&(digits ==0)&&(sign ==1))//如果当前字符是负号，并且还未解析数字和符号
{// detect negative sign // 注释：检测到负号
sign =-1;// 将符号标记为-1
continue;// 继续下一次循环
}
if((s[i]=='.')&&(decpoint ==0))//如果当前字符是小数点，并且之前没有遇到过小数点
{// detect decimal point // 注释：检测到小数点
decpoint =1;// 标记已遇到小数点
decimalPointLoc = i;// 记录小数点的位置
continue;// 继续下一次循环
}
if((s[i]<'0')||(s[i]>'9'))break;// 如果字符不是数字，则停止解析（不支持科学记数法）
digits =1;// 标记已检测到数字，此后不允许再出现符号或前导空格
if(i < decimalPointLoc)// 如果当前位置在小数点之前
{// integer part // 注释：整数部分
result *=10.0;// 将现有结果乘以10
result +=(int)(s[i]-'0');// 加上当前数字的值
}
else// 如果当前位置在小数点之后
{// fractional part // 注释：小数部分
result +=(double)(s[i]-'0')*(pow(10,decimalPointLoc-i));// 加上当前数字乘以其对应的权重（10的负幂）
}
}// for 循环体结束
return(result * sign);// 返回最终结果乘以符号
}
/* converts a string block into a motion package */// 注释块：将一个字符串块转换为一个运动包
unsignedshortInterpreter(char* Block, MotionPackage_Type* Package)// 定义解释器主函数
{// 函数体开始
char*strMovement;// 声明一个字符指针，用于查找运动指令字符串
char*strParameter;// 声明一个字符指针，用于查找参数字符串
int i =0;// 声明一个整型变量 i 并初始化为0，用作循环或索引
//save modal feedrate (if exists) before clearing package // 注释：在清空包之前保存模态进给率（如果存在）
double ModalFeedrate = Package->Feedrate;// 保存当前的进给率值
double ModalFeedrateType = Package->FeedrateType;// 保存当前的进给率类型
memset(Package,0,sizeof(MotionPackage_Type));// 使用 memset 将 Package 结构体的内存清零
Package->Feedrate = ModalFeedrate;// 恢复模态进给率
Package->FeedrateType = ModalFeedrateType;// 恢复模态进给率类型
/* look for comment sign */// 注释块：查找注释符号
strMovement =strstr(Block,"//");// 在 Block 字符串中查找 "//"
if(strMovement !=0)// 如果找到了注释
{// if 块开始
*strMovement =0;// 将注释开始的位置设置为字符串结束符，从而截断字符串
}// if 块结束
/* look for label sign */// 注释块：查找标签符号
strMovement =strstr(Block,":");// 在 Block 字符串中查找 ":"
if(strMovement !=0)// 如果找到了标签
{// if 块开始
return0;// 这是一个标签行，不是运动指令，直接返回成功，不进行后续解析
}// if 块结束
/* look for GOTO jump */// 注释块：查找 GOTO 跳转指令
strMovement =my_strcasestr(Block,"GOTO ");// 不区分大小写地查找 "GOTO "
if(strMovement !=0)// 如果找到了
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 如果已经解析出其他运动类型，则返回冲突错误
Package->MovementType = MOVE_GOTO;// 设置运动类型为 GOTO
/* extract label string */// 注释块：提取标签字符串
strMovement+=5;// 指针向后移动5个字符，跳过 "GOTO "
while(*strMovement ==' ') strMovement++;//remove preceeding spaces // 移除标签名前面的空格
for(i=1;strMovement[i]!='\0'&& strMovement[i]!=' ';i++){}// 找到标签名的结束位置（遇到字符串末尾或空格）
char*tmpStr = strMovement;// 创建一个临时指针指向标签名开始处
tmpStr[i]=0;// 在标签名后添加字符串结束符，以提取标签名
strcpy(Package->Label.Name,tmpStr);// 将提取的标签名复制到 Package 中
char*tmpIndex = strMovement + i;// 指针移动到标签名之后的位置
for(i=1;tmpIndex[i]!='\0';i++)// 遍历标签名后的字符串
{// for 循环体开始
if(tmpIndex[i]==' ')// 如果是空格
continue;//ignore emtpy spaces // 忽略空格
if((tmpIndex[i]<'1')||(tmpIndex[i]>'9'))// 如果不是 '1' 到 '9' 的数字
return ERR_IP_SYNTAX;//only digits are allowed // 返回语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出循环准备转换
}// for 循环体结束
Package->Label.Counter =atoi(tmpIndex+1);// 将标签后的数字（跳转计数器）转换为整数
return0;//to prevent conflicts with other commands // 解析完成，返回成功，防止与其他指令冲突
}// if 块结束
/* look for SUB jump */// 注释块：查找 SUB 子程序调用指令
strMovement =my_strcasestr(Block,"SUB ");// 不区分大小写地查找 "SUB "
if(strMovement !=0)// 如果找到了
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 如果已定义运动类型，则冲突
Package->MovementType = MOVE_SUB;// 设置运动类型为调用子程序
/* extract label string */// 注释块：提取标签字符串
strMovement+=4;// 指针跳过 "SUB "
while(*strMovement ==' ') strMovement++;//remove preceeding spaces // 移除前导空格
for(i=1;strMovement[i]!='\0'&& strMovement[i]!=' ';i++){}// 查找标签名结束位置
char*tmpStr = strMovement;// 临时指针指向标签名开始
tmpStr[i]=0;// 截取标签名
strcpy(Package->Label.Name,tmpStr);// 复制标签名到 Package
char*tmpIndex = strMovement + i;// 指针移到标签名后
for(i=1;tmpIndex[i]!='\0';i++)// 遍历后续字符串
{// for 循环体开始
if(tmpIndex[i]==' ')// 如果是空格
continue;//ignore emtpy spaces // 忽略
if((tmpIndex[i]<'1')||(tmpIndex[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
Package->Label.Counter =atoi(tmpIndex+1);// 将调用计数器转换为整数
return0;//to prevent conflicts with other commands // 解析完成，返回成功
}// if 块结束
/* look for Move Line */// 注释块：查找直线运动指令
strMovement =my_strcasestr(Block,"ML ");// 不区分大小写查找 "ML "
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查是否冲突
Package->MovementType = MOVE_LINE;// 设置运动类型为直线运动
}// if 块结束
/* look for Move PTP */// 注释块：查找点对点运动指令
strMovement =my_strcasestr(Block,"MJ ");// 不区分大小写查找 "MJ "
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查是否冲突
Package->MovementType = MOVE_PTP;// 设置运动类型为点对点运动
}// if 块结束
/* look for Move Circle */// 注释块：查找圆弧运动指令
strMovement =my_strcasestr(Block,"MC ");// 不区分大小写查找 "MC "
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查是否冲突
Package->MovementType = MOVE_CIRCLE;// 设置运动类型为圆弧运动
}// if 块结束
/* look for Move Spline */// 注释块：查找样条运动指令
strMovement =my_strcasestr(Block,"MS ");// 不区分大小写查找 "MS "
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查是否冲突
Package->MovementType = MOVE_SPLINE;// 设置运动类型为样条运动
}// if 块结束
/* look for Move HOME */// 注释块：查找回原点运动指令
strMovement =my_strcasestr(Block,"HOME ");// 不区分大小写查找 "HOME "
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查是否冲突
Package->MovementType = MOVE_HOME;// 设置运动类型为回原点
}// if 块结束
/* look for target point */// 注释块：查找目标点参数
strParameter =my_strcasestr(Block,"P");// 不区分大小写查找 "P"
if(strParameter !=0)// 如果找到
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "P" 后面的字符
{// for 循环体开始
if(strParameter[i]==' ')// 如果是空格
continue;//ignore emtpy spaces // 忽略
if((strParameter[i]<'0')||(strParameter[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "P" 后面没有数字
{// if 块开始
return ERR_IP_POINTINDEX;// 返回点索引错误
}// if 块结束
Package->TargetPoint =atoi(strParameter+1);// 将 "P" 后面的数字转换为整数，作为目标点索引
if(Package->TargetPoint >=MAX_POINT)// 如果点索引超出最大值
{// if 块开始
return ERR_IP_POINTINDEX;// 返回点索引错误
}// if 块结束
}// if 块结束
elseif((Package->MovementType == MOVE_LINE)||(Package->MovementType == MOVE_PTP)||(Package->MovementType == MOVE_CIRCLE)||(Package->MovementType == MOVE_SPLINE))// 如果是需要目标点的运动类型但没有找到 "P"
{// else if 块开始
return ERR_IP_NOPOINT;// 返回缺少目标点错误
}// else if 块结束
/* look for center point */// 注释块：查找中心点参数
strParameter =my_strcasestr(Block,"Q");// 不区分大小写查找 "Q"
if(strParameter !=0)// 如果找到
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "Q" 后面的字符
{// for 循环体开始
if(strParameter[i]==' ')// 如果是空格
continue;//ignore emtpy spaces // 忽略
if((strParameter[i]<'0')||(strParameter[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "Q" 后面没有数字
{// if 块开始
return ERR_IP_POINTINDEX;// 返回点索引错误（可复用）
}// if 块结束
Package->CenterPoint =atoi(strParameter+1);// 将 "Q" 后面的数字转换为整数，作为圆心点索引
if(Package->CenterPoint >=MAX_POINT)// 如果索引超出最大值
{// if 块开始
return ERR_IP_POINTINDEX;// 返回点索引错误
}// if 块结束
}// if 块结束
elseif(Package->MovementType == MOVE_CIRCLE)// 如果是圆弧运动但没有找到 "Q"
{// else if 块开始
return ERR_IP_NOCENTER;// 返回缺少圆心点错误
}// else if 块结束
/* look for reference positions (only valid for MJ movements) */// 注释块：查找参考位置（仅对MJ运动有效）
strParameter =my_strcasestr(Block,"J1=");// 查找 "J1="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 如果不是 PTP 运动，则冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J1=" 后面的字符
{// for 循环体开始
if(strParameter[i]==' ')// 如果是空格
continue;//ignore emtpy spaces // 忽略
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 如果不是数字、小数点或负号
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoff to read them // 找到合法字符，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J1=" 后面没有值
{//end of line - no position programmed after Ji= // 注释：行尾 - Ji=后没有编程位置
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[0]=1;// 标记 J1 已定义
Package->RefPoint.Axes[0]=str2double(strParameter+3);// 将 "J1=" 后面的字符串转换为 double 值
}// if 块结束
strParameter =my_strcasestr(Block,"J2=");// 查找 "J2="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 检查冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J2=" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查合法性
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J2=" 后面没有值
{//end of line - no position programmed after Ji=
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[1]=1;// 标记 J2 已定义
Package->RefPoint.Axes[1]=str2double(strParameter+3);// 转换 J2 的值
}// if 块结束
strParameter =my_strcasestr(Block,"J3=");// 查找 "J3="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 检查冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J3=" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查合法性
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J3=" 后面没有值
{//end of line - no position programmed after Ji=
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[2]=1;// 标记 J3 已定义
Package->RefPoint.Axes[2]=str2double(strParameter+3);// 转换 J3 的值
}// if 块结束
strParameter =my_strcasestr(Block,"J4=");// 查找 "J4="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 检查冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J4=" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查合法性
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J4=" 后面没有值
{//end of line - no position programmed after Ji=
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[3]=1;// 标记 J4 已定义
Package->RefPoint.Axes[3]=str2double(strParameter+3);// 转换 J4 的值
}// if 块结束
strParameter =my_strcasestr(Block,"J5=");// 查找 "J5="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 检查冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J5=" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查合法性
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J5=" 后面没有值
{//end of line - no position programmed after Ji=
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[4]=1;// 标记 J5 已定义
Package->RefPoint.Axes[4]=str2double(strParameter+3);// 转换 J5 的值
}// if 块结束
strParameter =my_strcasestr(Block,"J6=");// 查找 "J6="
if(strParameter !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_PTP)return ERR_IP_CONFLICT;// 检查冲突
for(i=3;strParameter[i]!='\0';i++)// 遍历 "J6=" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查合法性
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "J6=" 后面没有值
{//end of line - no position programmed after Ji=
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->RefPoint.Defined[5]=1;// 标记 J6 已定义
Package->RefPoint.Axes[5]=str2double(strParameter+3);// 转换 J6 的值
}// if 块结束
/* look for feedrate */// 注释块：查找进给率参数
strParameter =my_strcasestr(Block,"F");// 不区分大小写查找 "F"
if(strParameter !=0)// 如果找到
{// if 块开始
unsignedchar offset;// 声明一个无符号字符变量 offset，用于记录数值部分的偏移量
if(strParameter[1]=='C')//cartesian speed definition // 如果 "F" 后面是 'C'，注释：笛卡尔速度定义
{// if 块开始
Package->FeedrateType = FEED_CART;// 设置进给率类型为笛卡尔
offset =2;// 数值从第2个字符后开始
}// if 块结束
elseif(strParameter[1]=='A')//angular speed definition // 如果 "F" 后面是 'A'，注释：关节角速度定义
{// else if 块开始
Package->FeedrateType = FEED_ANG;// 设置进给率类型为关节角度
offset =2;// 数值从第2个字符后开始
}// else if 块结束
else//default speed definition // 否则，注释：默认速度定义
{// else 块开始
Package->FeedrateType = FEED_DEFAULT;// 设置为默认类型
offset =1;// 数值从第1个字符后开始
}// else 块结束
for(i=offset;strParameter[i]!='\0';i++)// 从偏移量处开始遍历
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&((strParameter[i]!='.')))// 如果不是数字或小数点
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoff to read them // 找到合法字符，跳出
}// for 循环体结束
Package->Feedrate =str2double(strParameter+offset);// 将数值部分转换为 double
if(Package->Feedrate <=0)// 如果进给率小于或等于0
{// if 块开始
return ERR_IP_FEEDRATE;// 返回进给率错误
}// if 块结束
}// if 块结束
elseif(((Package->MovementType == MOVE_LINE)||(Package->MovementType == MOVE_PTP)||(Package->MovementType == MOVE_CIRCLE)||(Package->MovementType == MOVE_HOME))&&(Package->Feedrate <=0))// 如果是需要进给率的运动，但没有定义或模态值为0
{// else if 块开始
return ERR_IP_FEEDRATE;// 返回进给率错误
}// else if 块结束
/* look for Delay time or for Signal input */// 注释块：查找延时时间或信号输入
strMovement =my_strcasestr(Block,"WAIT");// 不区分大小写查找 "WAIT"
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
for(i=4;strMovement[i]!='\0';i++)// 遍历 "WAIT" 后的字符
{// for 循环体开始
if(strMovement[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strMovement[i]>='0'&& strMovement[i]<='9')|| strMovement[i]=='.')// 如果是数字或小数点
{//digits found -> wait for time // 注释：找到数字 -> 等待时间
Package->MovementType = MOVE_DELAY;// 设置运动类型为延时
break;// 跳出循环
}// if 块结束
elseif(strMovement[i]=='D'&& strMovement[i+1]=='I')// 如果是 "DI"
{//DI found -> wait for signal // 注释：找到DI -> 等待信号
Package->MovementType = MOVE_WAITDI;// 设置运动类型为等待数字输入
break;// 跳出循环
}// else if 块结束
else// 否则
return ERR_IP_SYNTAX;// 语法错误
}// for 循环体结束
if(Package->MovementType == MOVE_DELAY)// 如果是延时指令
{// if 块开始
if(strMovement[i]==0)return ERR_IP_SYNTAX;//end of line - no delay time programmed // 如果 "WAIT" 后没有数值，语法错误
Package->DelayTime =str2double(strMovement+i-1);// 将数值转换为 double 作为延时时间
}// if 块结束
else// 如果是等待输入指令
{// else 块开始
if(strMovement[i+2]==0)return ERR_IP_SYNTAX;//end of line - no delay time programmed // 如果 "WAIT DI" 后没有索引，语法错误
Package->IO_Index =atoi(strMovement+i+2);// 将索引转换为整数
}// else 块结束
}// if 块结束
/* look for Tracking TRK */// 注释块：查找跟踪指令 TRK
strMovement =my_strcasestr(Block,"TRK");// 不区分大小写查找 "TRK"
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
Package->MovementType = MOVE_TRK;// 设置运动类型为跟踪
for(i=3;strMovement[i]!='\0';i++)// 遍历 "TRK" 后的字符
{// for 循环体开始
if(strMovement[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strMovement[i]<'0')||(strMovement[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
Package->TrkIndex =atoi(strMovement+3);// 将索引转换为整数
if((Package->TrkIndex >2)||(strMovement[i]=='\0'))//either wrong or no index given // 如果索引错误或未提供
{// if 块开始
return ERR_IP_TRK_INDEX;// 返回跟踪索引错误
}// if 块结束
}// if 块结束
/* look for Tangential TANG */// 注释块：查找相切指令 TANG
strMovement =my_strcasestr(Block,"TANG");// 不区分大小写查找 "TANG"
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
Package->MovementType = MOVE_TANG;// 设置运动类型为相切
for(i=4;strMovement[i]!='\0';i++)// 遍历 "TANG" 后的字符
{// for 循环体开始
if(strMovement[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strMovement[i]<'0')||(strMovement[i]>'1'))// 如果不是 '0' 或 '1'
return ERR_IP_SYNTAX;//only 0-1 are allowed // 语法错误
else// 否则
break;//0-1 found -> use atoi to read them // 找到合法字符，跳出
}// for 循环体结束
Package->TangCmd =atoi(strMovement+4);// 将命令值（0或1）转换为整数
if(strMovement[i]=='\0')//tangential command not complete // 如果 "TANG" 后没有命令值
{// if 块开始
return ERR_IP_TANG;// 返回相切指令错误
}// if 块结束
}// if 块结束
/* look for RESET */// 注释块：查找 RESET 指令
strMovement =my_strcasestr(Block,"RESET");// 不区分大小写查找 "RESET"
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
Package->MovementType = MOVE_RESETDO;// 设置运动类型为复位数字输出
/* // 这段被注释掉的代码原本用于解析 RESET DOx
for(i=8;strMovement[i]!='\0';i++)
{
if (strMovement[i]==' ')
continue; //ignore emtpy spaces
if ((strMovement[i]<'0')||(strMovement[i]>'9'))
return ERR_IP_SYNTAX; //only digits are allowed
else
break; //digits found -> use atoi to read them
}
Package->IO_Index = atoi(strMovement+8);
if (strMovement[i]=='\0') //IO index not provided
{
return ERR_IP_IO_INDEX;
}
if (Package->IO_Index > MAX_IO) return ERR_IP_IO_INDEX;
*/// 被注释代码块结束
}// if 块结束
/* look for SET */// 注释块：查找 SET 指令
strMovement =my_strcasestr(Block,"SET");// 不区分大小写查找 "SET"
if((strMovement !=0)&&(Package->MovementType != MOVE_RESETDO))//avoid conflict with reset_do // 如果找到 "SET" 且不是 "RESET"
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
Package->MovementType = MOVE_SETDO;// 设置运动类型为设置数字输出
/* // 这段被注释掉的代码原本用于解析 SET DOx
for(i=6;strMovement[i]!='\0';i++)
{
if (strMovement[i]==' ')
continue; //ignore emtpy spaces
if ((strMovement[i]<'0')||(strMovement[i]>'9'))
return ERR_IP_SYNTAX; //only digits are allowed
else
break; //digits found -> use atoi to read them
}
Package->IO_Index = atoi(strMovement+6);
if (strMovement[i]=='\0') //IO index not provided
{
return ERR_IP_IO_INDEX;
}
if (Package->IO_Index > MAX_IO) return ERR_IP_IO_INDEX;
*/// 被注释代码块结束
}// if 块结束
/* look for target DO point */// 注释块：查找目标 DO 点
strParameter =my_strcasestr(Block,"DO");// 不区分大小写查找 "DO"
if(strParameter !=0)// 如果找到
{// if 块开始
for(i=2;strParameter[i]!='\0';i++)// 遍历 "DO" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strParameter[i]<'0')||(strParameter[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "DO" 后面没有索引
{// if 块开始
return ERR_IP_IO_INDEX;// 返回 IO 索引错误
}// if 块结束
Package->IO_Index =atoi(strParameter+2);// 将索引转换为整数
if(Package->IO_Index > MAX_IO)return ERR_IP_IO_INDEX;// 如果索引超出最大值，返回错误
}// if 块结束
elseif((Package->MovementType == MOVE_RESETDO)||(Package->MovementType == MOVE_SETDO))// 如果是 SET/RESET 指令但没有找到 "DO"
{// else if 块开始
return ERR_IP_IO_INDEX;// 返回 IO 索引错误
}// else if 块结束
/* look for rotation angle (or tangential offset) */// 注释块：查找旋转角度（或相切偏移）
strParameter =my_strcasestr(Block,"H");// 不区分大小写查找 "H"
if((strParameter !=0)&&(Package->MovementType != MOVE_HOME))// 如果找到 "H" 且不是 HOME 指令
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "H" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.')&&(strParameter[i]!='-'))// 检查是否为合法浮点数
return ERR_IP_SYNTAX;//only digits are allowed
else// 否则
break;//digits found -> use atoff to read them
}// for 循环体结束
if(strParameter[i]==0)// 如果 "H" 后面没有值
{//end of line - no value programmed after H
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->Path.RotAngle =str2double(strParameter+1);// 将数值转换为 double 作为旋转角度
}// if 块结束
/* look for tool */// 注释块：查找工具参数
strParameter =my_strcasestr(Block,"T");// 不区分大小写查找 "T"
if((strParameter !=0)&&(Package->MovementType != MOVE_DELAY)&&(Package->MovementType != MOVE_GOTO)&&(Package->MovementType != MOVE_TRK)&&(Package->MovementType != MOVE_TANG)&&(Package->MovementType != MOVE_RESETDO)&&(Package->MovementType != MOVE_SETDO)&&(Package->MovementType != MOVE_WAITDI))//to avoid conflicts with "T" in other commands // 避免与其他指令中的'T'冲突
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "T" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strParameter[i]<'0')||(strParameter[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "T" 后面没有值
{//end of line - no value programmed after T
return ERR_IP_TOOLINDEX;// 返回工具索引错误
}// if 块结束
Package->Tool =atoi(strParameter+1);// 将索引转换为整数
if(Package->Tool >=MAX_TOOL)// 如果索引超出最大值
{// if 块开始
return ERR_IP_TOOLINDEX;// 返回工具索引错误
}// if 块结束
if(Package->MovementType == MOVE_UNDEF)// 如果之前没有确定运动类型
{// if 块开始
Package->MovementType = MOVE_TOOL;// 这是一个单独的工具设置指令
}// if 块结束
}// if 块结束
/* look for frame */// 注释块：查找坐标系参数
strParameter =my_strcasestr(Block,"Z");// 不区分大小写查找 "Z"
if(strParameter !=0)// 如果找到
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "Z" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if((strParameter[i]<'0')||(strParameter[i]>'9'))// 如果不是数字
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoi to read them // 找到数字，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "Z" 后面没有值
{//end of line - no value programmed after Z
return ERR_IP_FRAMEINDEX;// 返回坐标系索引错误
}// if 块结束
Package->Frame =atoi(strParameter+1);// 将索引转换为整数
if(Package->Frame >=MAX_FRAME)// 如果索引超出最大值
{// if 块开始
return ERR_IP_FRAMEINDEX;// 返回坐标系索引错误
}// if 块结束
}// if 块结束
/* look for round edge */// 注释块：查找过渡（圆角）参数
Package->Round =-1;//round edge not defined // 初始化过渡半径为-1（未定义）
strParameter =my_strcasestr(Block,"R");// 不区分大小写查找 "R"
if((strParameter !=0)&&(Package->MovementType != MOVE_TRK)&&(Package->MovementType != MOVE_RESETDO))//to avoid conflicts with "R" in other commands // 避免与 TRK, RESET 中的 'R' 冲突
{// if 块开始
for(i=1;strParameter[i]!='\0';i++)// 遍历 "R" 后的字符
{// for 循环体开始
if(strParameter[i]==' ')// 忽略空格
continue;//ignore emtpy spaces
if(((strParameter[i]<'0')||(strParameter[i]>'9'))&&(strParameter[i]!='.'))// 如果不是数字或小数点
return ERR_IP_SYNTAX;//only digits are allowed // 语法错误
else// 否则
break;//digits found -> use atoff to read them // 找到合法字符，跳出
}// for 循环体结束
if(strParameter[i]==0)// 如果 "R" 后面没有值
{//end of line - no value programmed after R
return ERR_IP_SYNTAX;// 语法错误
}// if 块结束
Package->Round =str2double(strParameter+1);// 将数值转换为 double 作为过渡半径
}// if 块结束
/* look for End of program */// 注释块：查找程序结束指令
strMovement =my_strcasestr(Block,"END");// 不区分大小写查找 "END"
if(strMovement !=0)// 如果找到
{// if 块开始
if(Package->MovementType != MOVE_UNDEF)return ERR_IP_CONFLICT;// 检查冲突
Package->MovementType = MOVE_END;// 设置运动类型为程序结束
}// if 块结束
/* look for M functions - max 10 can be on a single line */// 注释块：查找 M 函数 - 每行最多10个
int M_count =0;// 初始化 M 函数计数器
strMovement =my_strcasestr(Block,"M");// 不区分大小写查找 "M"
while(strMovement !=0)// 循环查找所有的 "M"
{// while 循环体开始
int index =atoi(strMovement+1);//check that it is an M-function // 转换 "M" 后面的数字
if(index >0)//new M function found - no negative or zero values accepted // 如果是合法的 M 代码（大于0）
{// if 块开始
if((Package->MovementType != MOVE_UNDEF)&&(Package->MovementType != MOVE_MCODE))return ERR_IP_CONFLICT;// 检查冲突
if(M_count >= MAX_MFUNC_INLINE)return ERR_IP_MAXMFUNC;// 检查是否超出单行最大 M 函数数量
if(index>=MAX_MFUNC)return ERR_IP_MFUNCINDEX;// 检查 M 代码索引是否有效
Package->MovementType = MOVE_MCODE;// 设置运动类型为 M 代码
Package->M_Index[M_count]= index;// 存储 M 代码
M_count++;// 计数器加一
}// if 块结束
//else return ERR_IP_MFUNCINDEX; //cannot use this line because atoi(ML) returns 0! // 此行被注释，因为像 "ML" 会被 atoi 转换为 0，导致误判
strMovement++;// 指针向后移动一个字符，以查找下一个 "M"
strMovement =my_strcasestr(strMovement,"M");// 从新位置开始查找
}// while 循环体结束
return STATUS_OK;// 所有解析完成，返回 OK 状态
}// Interpreter 函数体结束Mics.h Mics.h
Misc.h 文件
#ifndef_MISC_TYPES // 如果宏 _MISC_TYPES 未被定义
#define_MISC_TYPES// 则定义宏 _MISC_TYPES，这是一个防止头文件被重复包含的预处理指令
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了如 MAX_BLOCK_SIZE 等宏
#include"Frame.h"// 包含坐标系相关的头文件，定义了 Frame_Type, Quat_Type 等
#include<string.h>// 包含标准字符串处理库头文件
/* declare miscellaneous datatypes and functions */// 注释块：声明杂项数据类型和函数
typedefstructRefPoint_Type// 定义一个名为 RefPoint_Type 的结构体，用于存储 PTP 运动的参考点
{// 结构体开始
double Axes[6];// 一个双精度浮点数数组，存储六个关节轴的参考位置
unsignedchar Defined[6];// 一个无符号字符数组，标记每个关节的参考位置是否被定义
} RefPoint_Type;// 结构体定义结束
typedefstructEdge_Type// 定义一个名为 Edge_Type 的结构体，用于描述运动段之间的过渡（圆角/倒角）
{// 结构体开始
double Radius;// 过渡的半径
double Length;// 过渡弧线的长度
Frame_Type CtrlPoint[7];// 存储过渡曲线的控制点（贝塞尔曲线或其他），#5和#6与#0和#4在关节空间中相同
double tangAngleStart;// 过渡开始处的切线角
double tangAngleEnd;// 过渡结束处的切线角
Quat_Type EdgeQuat;//仅用于PTP运动过渡的四元数
double QuatAngle;// 四元数插值的总角度
} Edge_Type;// 结构体定义结束
typedefstructSpline_Type// 定义一个名为 Spline_Type 的结构体，用于描述样条曲线段
{// 结构体开始
double LengthA;// 样条段的一部分长度A
double LengthB;// 样条段的另一部分长度B
Frame_Type CtrlPoint[4];// 存储样条曲线的四个控制点（例如三次贝塞尔曲线）
} Spline_Type;// 结构体定义结束
typedefstructPath_Type// 定义一个名为 Path_Type 的结构体，用于完整描述一段运动路径的几何和动力学信息
{// 结构体开始
double StartPointPath[6];// 路径起点在笛卡尔空间下的位姿（X,Y,Z,A,B,C）
double StartPointJoint[6];// 路径起点在关节空间下的位置
double StartPointAux[6];// 路径起点的辅助轴位置
double MiddlePointPath[6];// 路径中间点（如圆弧上的点）在笛卡尔空间下的位姿
double MiddlePointJoint[6];// 路径中间点在关节空间下的位置
double TargetPointPath[6];// 路径目标点在笛卡尔空间下的位姿
double TargetPointJoint[6];// 路径目标点在关节空间下的位置
double TargetPointAux[6];// 路径目标点的辅助轴位置
double Radius;// 圆弧运动的半径
double Center[3];// 圆弧运动的圆心坐标 (X,Y,Z)
double Length;// 路径总长度
double MiddleLength;// 从起点到中间点的路径长度
double Normal[3];// 圆弧所在平面的法向量
double StartVersor[3];// 从圆心指向起点的单位向量
double CrossVersor[3];// 与StartVersor和EndVersor相关的叉乘向量
double MiddleVersor[3];// 从圆心指向中间点的单位向量
double EndVersor[3];// 从圆心指向终点的单位向量
double RotAngle;// 旋转角度（例如，在2D轮廓跟随中）
Quat_Type StartQuat;// 起点姿态的四元数表示
Quat_Type MiddleQuat;// 中间点姿态的四元数表示
Quat_Type EndQuat;// 终点姿态的四元数表示
double QuatAngle;//from start to end // 从起点到终点的总姿态变化角度
double QuatAngle1;//from start to middle // 从起点到中间点的姿态变化角度
double QuatAngle2;//from middle to end // 从中间点到终点的姿态变化角度
double uM;//position of middle point (0..1) // 中间点在路径上的归一化位置（0到1之间）
Edge_Type StartEdge;// 路径开始处的过渡信息
Edge_Type EndEdge;// 路径结束处的过渡信息
Spline_Type Spline;// 样条曲线信息
} Path_Type;// 结构体定义结束
typedefstructLabel_Type// 定义一个名为 Label_Type 的结构体，用于处理程序中的标签
{// 结构体开始
char Name[MAX_BLOCK_SIZE+1];// 标签的名称
unsignedshort Found;// 标记是否已在程序中找到该标签
unsignedlong Index;// 标签所在程序行的索引
int Counter;// 用于循环或跳转的计数器
} Label_Type;// 结构体定义结束
typedefstructGoto_Type// 定义一个名为 Goto_Type 的结构体，用于管理GOTO和SUB跳转
{// 结构体开始
unsignedlong Line;//line where goto or sub were called // 调用GOTO或SUB指令的行号
int Counter;//number of required iterations // 需要的迭代次数
unsignedlong Index;//line where the target label was found // 目标标签被找到的行号
} Goto_Type;// 结构体定义结束
typedefstructMotionPackage_Type// 定义一个名为 MotionPackage_Type 的结构体，这是解释器解析一行代码后生成的运动指令包
{// 结构体开始
unsignedchar MovementType;// 运动类型（如直线，PTP，圆弧等）
unsignedshort TargetPoint;// 目标点的索引
unsignedshort CenterPoint;// 圆弧中心点的索引
unsignedshort Frame;// 使用的用户坐标系的索引
unsignedshort Tool;// 使用的工具的索引
double Round;// 过渡半径
double MaxSpeed;// 最大速度（可能未使用）
unsignedchar M_Index[MAX_MFUNC+1];// M代码（辅助功能）的索引数组
unsignedchar IO_Index;// IO操作的索引
double DelayTime;// 延时时间
double Feedrate;// 进给速率
unsignedchar FeedrateType;// 进给速率的类型（笛卡尔或关节）
unsignedshort Sub;// 子程序相关的标志或索引
structPath_Type Path;// 路径的详细几何信息
structRefPoint_Type RefPoint;// PTP运动的参考点信息
unsignedlong LineNumber;// 该指令在源程序中的行号
char BlockString[MAX_BLOCK_SIZE+1];// 该指令的原始字符串
Label_Type Label;// 如果是GOTO或SUB指令，相关的标签信息
double BlockLength;// 该运动块的实际长度
double BlockLengthIdeal;// 该运动块的理想长度（用于速度规划）
unsignedchar Planned;// 标记该运动块是否已被路径规划器处理
double StartJointVector[6];// 运动段开始时的关节速度向量
double EndJointVector[6];// 运动段结束时的关节速度向量
double StartPathVector[3];// 运动段开始时的笛卡尔路径切向向量
double EndPathVector[3];// 运动段结束时的笛卡尔路径切向向量
double TransitionAngle;// 两段运动之间的过渡角度
double EndSpeed;// 运动段结束时的速度
double EndAcc;// 运动段结束时的加速度
unsignedshort TrkIndex;// 跟踪功能的索引
unsignedshort TangCmd;// 相切控制命令
} MotionPackage_Type;// 结构体定义结束
typedefstructBuffer_Type// 定义一个名为 Buffer_Type 的结构体，作为运动指令的缓冲区
{// 结构体开始
structMotionPackage_Type MotionPackage[BUFFER_LENGTH+1];// 存储运动包的数组（环形缓冲区）
unsignedchar EXEC_Index;//buffer block being executed // 正在被执行的缓冲块的索引
unsignedchar EXEC_Index_Prev;//last executed block // 上一个被执行的块的索引
unsignedchar PP_Index;//buffer block being planned // 正在被路径规划器处理的缓冲块的索引
unsignedchar PP_Index_Prev;//last planned block // 上一个被规划的块的索引
unsignedchar IP_Index;//buffer block being interpreted // 正在被解释器填充的缓冲块的索引
unsignedlong IP_PrgCount;//program line being interpreted // 解释器正在处理的程序行号
unsignedchar IP_SubLevel;//level of subprogram (0=main program; 1=first subprogram level; ...) // 当前子程序的嵌套层级
unsignedlong IP_ReturnIdx[10];//saved line numbers from subprogram calls // 保存的子程序调用返回地址（行号）
unsignedchar Eof;// End of file 标记，表示程序文件已读取完毕
unsignedchar Planned;// 标记缓冲区中是否有已规划的块
unsignedchar Synch;//used by synch M-functions to restart the halted PP // 用于同步M函数，以重新启动暂停的路径规划器
unsignedchar IP_TrkIndex;//keeps note of what tracking index is currently active (IP-synch) // 记录当前激活的跟踪索引（解释器同步）
double ModalFeedrate;//save feedrate as modal // 保存模态的进给率
unsignedchar ModalFeedrateType;//save also feedrate type as modal // 保存模态的进给率类型
Goto_Type GotoBuffer[MAX_SUBLEVEL];// GOTO跳转信息的缓冲区（栈）
Goto_Type SubBuffer[MAX_SUBLEVEL];// 子程序调用信息的缓冲区（栈）
} Buffer_Type;// 结构体定义结束
typedefstructFilter_Type// 定义一个名为 Filter_Type 的结构体，用于实现数字滤波器
{// 结构体开始
double InputValue;// 滤波器的当前输入值
unsignedchar Enable;// 滤波器使能标志
unsignedshort Window;// 滤波器窗口大小
double OutputValue;// 滤波器的输出值
doubleBuffer[1000];// 存储历史输入值的缓冲区
unsignedshort Index;// 缓冲区当前索引
}Filter_Type;// 结构体定义结束
unsignedshortLineFromString(char* s,char* line,unsignedlong linenumber);// 声明函数：从一个大字符串s中提取指定行号的单行内容到line中
doubleRoundToEpsilon(double Value);// 声明函数：将一个浮点数Value四舍五入到最接近的整数，如果差值小于EPSILON
voidMovingAverageFilter(structFilter_Type* inst);// 声明函数：对一个Filter_Type实例应用移动平均滤波
voidGaussianFilter(structFilter_Type* inst);// 声明函数：对一个Filter_Type实例应用高斯滤波
unsignedshortCheckConst();// 声明函数：检查所有常量定义是否正确
#endif// 结束 #ifndef _MISC_TYPES 的宏定义条件块Misc.c 文件
#include"Misc.h"// 包含此源文件对应的头文件 "Misc.h"
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了错误码和宏
#include"Frame.h"// 包含坐标系相关的头文件
#include<string.h>// 包含标准字符串处理库头文件，提供 memset, memcpy 等函数
unsignedshortLineFromString(char* s,char* line,unsignedlong linenumber)// 定义函数：从字符串s中提取第linenumber行
{//extract line from string // 函数体开始，注释：从字符串中提取行
memset(line,0,MAX_BLOCK_SIZE);// 首先将目标行缓冲区line清零
unsignedlong i=1;// 声明一个长无符号整型变量i，作为当前行号计数器，初始化为1
unsignedshort linesize;// 声明一个无符号短整型变量linesize，用于记录提取出的行的长度
char* tmp = s;// 声明一个字符指针tmp，指向输入字符串s的开头
while(i<linenumber)// 循环直到找到目标行号的前一行
{// while 循环体开始
if((*tmp ==13)&&(*(tmp+1)==10))//carriage return + line feed // 检查是否为Windows风格的换行符(CR+LF)
{// if 块开始
i++;// 行号计数器加一
tmp+=2;// 指针向后移动两个字符
}// if 块结束
else// 如果不是CR+LF
{// else 块开始
tmp++;// 假设是单个换行符或只是向前扫描，指针移动一个字符
}// else 块结束
if(*tmp ==0)return ERR_FILE_END;//end of string reached // 如果到达字符串末尾，说明目标行不存在，返回文件结束错误
}// while 循环体结束
linesize =0;// 初始化行长度为0
char* start = tmp;// 声明一个字符指针start，记录目标行的起始位置
while(((*tmp !=13)||(*(tmp+1)!=10))&&(linesize<MAX_BLOCK_SIZE))//reached carriage return + line feed or max block length // 循环直到遇到换行符或达到最大块长度
{// while 循环体开始
tmp++;// 移动指针
linesize++;// 增加行长度计数
}// while 循环体结束
memcpy(line,start,linesize);// 将找到的行内容从start位置复制linesize个字节到line缓冲区
return STATUS_OK;// 函数执行成功，返回OK状态
}
doubleRoundToEpsilon(double Value)// 定义函数：将浮点数Value四舍五入到最接近的整数（在EPSILON精度内）
{// 函数体开始
int nValue =round(Value);// 使用标准库的round函数计算最接近的整数
if(fabs(Value - nValue)< TRF_EPSILON)// 检查原始值与四舍五入后的整数值的差的绝对值是否小于一个很小的数TRF_EPSILON
return nValue;// 如果是，则返回该整数值
else// 否则
return Value;// 返回原始的浮点数值
}
voidMovingAverageFilter(structFilter_Type* inst)// 定义函数：移动平均滤波器
{// 函数体开始
double Sum =0;// 声明一个双精度浮点数变量Sum并初始化为0，用于累加窗口内的数据
int i;// 声明一个整型变量i，用作循环计数器
if(!inst->Enable)// 如果滤波器未使能
{// if 块开始
for(i=0;i<100;i++)// 遍历一个固定大小的数组（此处为100）
{//flood array with input value so that restart (Enable=1) does not jump // 注释：用输入值填充数组，这样重新使能时输出不会跳变
inst->Buffer[i]= inst->InputValue;// 将缓冲区的每个元素都设置为当前的输入值
}// for 循环体结束
inst->Index =0;// 重置缓冲区索引
inst->OutputValue = inst->InputValue;// 将输出值直接设置为输入值
return;// 函数返回
}// if 块结束
//automatically limit window size // 注释：自动限制窗口大小
if(inst->Window >100)// 如果设定的窗口大小超过了缓冲区大小（此处为100）
inst->Window =100;// 将窗口大小限制为缓冲区的最大值
if(inst->Window <=0)// 如果设定的窗口大小小于或等于0
inst->Window =1;// 将窗口大小设置为1，避免除以零
//insert new element in buffer // 注释：在缓冲区中插入新元素
inst->Index++;// 索引加一
if(inst->Index>=inst->Window)// 如果索引超出了窗口范围
inst->Index =0;// 将索引重置为0，实现环形缓冲
inst->Buffer[inst->Index]= inst->InputValue;// 在当前索引位置存入新的输入值
for(i=0;i<inst->Window;i++)// 遍历整个窗口
{// for 循环体开始
Sum += inst->Buffer[i];// 累加窗口内所有元素的值
}// for 循环体结束
inst->OutputValue = Sum /(double) inst->Window;// 计算平均值并作为输出
}
voidGaussianFilter(structFilter_Type* inst)// 定义函数：高斯滤波器
{// 函数体开始
double Sum =0;// 声明一个双精度浮点数变量Sum并初始化为0，用于累加加权后的值
double GSum =0;// 声明一个双精度浮点数变量GSum并初始化为0，用于累加高斯权重
int i,idx;// 声明两个整型变量i和idx，用作循环和索引
if(!inst->Enable)// 如果滤波器未使能
{// if 块开始
for(i=0;i<1000;i++)// 遍历整个缓冲区（大小为1000）
{//flood array with input value so that restart (Enable=1) does not jump // 注释：用输入值填充数组，这样重新使能时输出不会跳变
inst->Buffer[i]= inst->InputValue;// 将缓冲区的每个元素都设置为当前的输入值
}// for 循环体结束
inst->Index =0;// 重置索引
inst->OutputValue = inst->InputValue;// 将输出值直接设置为输入值
return;// 函数返回
}// if 块结束
//automatically limit window size // 注释：自动限制窗口大小
if(inst->Window >1000)// 如果设定的窗口大小超过了缓冲区大小（1000）
inst->Window =1000;// 将窗口大小限制为最大值
if(inst->Window <=0)// 如果设定的窗口大小小于或等于0
inst->Window =1;// 将窗口大小设置为1
//insert new element in buffer // 注释：在缓冲区中插入新元素
inst->Index++;// 索引加一
if(inst->Index>=inst->Window)// 如果索引超出窗口范围
inst->Index =0;// 索引重置为0，实现环形缓冲
inst->Buffer[inst->Index]= inst->InputValue;// 在当前索引位置存入新的输入值
for(i=0;i<inst->Window;i+=((inst->Window+99)/100))//decrease resolution to speed up calculation // 为了加速计算，降低分辨率进行遍历（步长不为1）
{// for 循环体开始
double tmpVal =(i-inst->Window/2.0)/(0.4*inst->Window/2.0);// 计算高斯函数中的变量部分（标准化距离）
double Gauss =exp(-0.5* tmpVal*tmpVal);// 计算高斯权重值
GSum += Gauss;// 累加高斯权重
//start counting signal from first element inserted in time // 注释：从时间上最早插入的元素开始计算信号
idx = i+inst->Index+1;// 计算在环形缓冲区中的实际数据索引
if(idx>=inst->Window)// 如果索引越界
idx -= inst->Window;// 回绕到缓冲区开头
Sum +=(inst->Buffer[idx]* Gauss);// 将缓冲区中的值乘以其对应的高斯权重，然后累加
}// for 循环体结束
//normalize output // 注释：归一化输出
inst->OutputValue = Sum/GSum;// 将加权和除以权重和，得到最终的滤波输出
}
unsignedshortCheckConst()// 定义函数：检查常量定义
{// 函数体开始
//check all constants // 注释：检查所有常量
unsignedshort check =1;// 声明一个无符号短整型变量check并初始化为1，作为检查结果
check *=(STATUS_OK ==0);// 检查 STATUS_OK 是否为 0，如果为真(1)，check不变；如果为假(0)，check变为0
check *=(CNC ==0);// 检查 CNC 是否为 0
check *=(JOG_JOINTS ==0);// 检查 JOG_JOINTS 是否为 0
check *=(JOG_POSITIVE ==0);// 检查 JOG_POSITIVE 是否为 0
check *=(POINT_PATH ==1);// 检查 POINT_PATH 是否为 1
check *=(SCARA ==1);// 检查 SCARA 是否为 1
check *=(JOG_NEGATIVE ==1);// 检查 JOG_NEGATIVE 是否为 1
check *=(JOG_BASE ==1);// 检查 JOG_BASE 是否为 1
check *=(POINT_JOINTS ==0);// 检查 POINT_JOINTS 是否为 0
check *=(JOG_TOOL ==2);// 检查 JOG_TOOL 是否为 2
check *=(JOG_GOTO ==2);// 检查 JOG_GOTO 是否为 2
check *=(JOG_AUX ==3);// 检查 JOG_AUX 是否为 3
check *=(ZONE_DISABLED ==0);// 检查 ZONE_DISABLED 是否为 0
check *=(ZONE_SAFE ==1);// 检查 ZONE_SAFE 是否为 1
check *=(ZONE_FORBIDDEN ==2);// 检查 ZONE_FORBIDDEN 是否为 2
check *=(DELTA ==3);// 检查 DELTA 是否为 3
check *=(PALLETIZER ==4);// 检查 PALLETIZER 是否为 4
check *=(RTCP ==5);// 检查 RTCP 是否为 5
check *=(ARM ==6);// 检查 ARM 是否为 6
check *=(USER ==10);// 检查 USER 是否为 10
check *=(MAX_IO ==100);// 检查 MAX_IO 是否为 100
check *=(MAX_MFUNC ==100);// 检查 MAX_MFUNC 是否为 100
check *=(MAX_POINT ==200);// 检查 MAX_POINT 是否为 200
check *=(MAX_BLOCK_SIZE ==100);// 检查 MAX_BLOCK_SIZE 是否为 100
check *=(MAX_LOOP_COUNT ==100);// 检查 MAX_LOOP_COUNT 是否为 100
check *=(MAX_FRAME ==20);// 检查 MAX_FRAME 是否为 20
check *=(MAX_TOOL ==20);// 检查 MAX_TOOL 是否为 20
check *=(MAX_ERR ==20);// 检查 MAX_ERR 是否为 20
check *=(MAX_MFUNC_INLINE ==10);// 检查 MAX_MFUNC_INLINE 是否为 10
check *=(MAX_ROBOTS ==8);// 检查 MAX_ROBOTS 是否为 8
check *=(MAX_PRG_SIZE ==10000);// 检查 MAX_PRG_SIZE 是否为 10000
check *=(MAX_ZONE ==10);// 检查 MAX_ZONE 是否为 10
check *=(BUFFER_LENGTH ==20);// 检查 BUFFER_LENGTH 是否为 20
check *=(RING_BUFFER_SIZE ==100);// 检查 RING_BUFFER_SIZE 是否为 100
check *=(TRF_POSE_BACK ==4);// 检查 TRF_POSE_BACK 是否为 4
check *=(TRF_POSE_CONCAVE ==2);// 检查 TRF_POSE_CONCAVE 是否为 2
check *=(TRF_POSE_CONVEX ==0);// 检查 TRF_POSE_CONVEX 是否为 0
check *=(TRF_POSE_LEFT ==1);// 检查 TRF_POSE_LEFT 是否为 1
check *=(TRF_POSE_RIGHT ==0);// 检查 TRF_POSE_RIGHT 是否为 0
check *=(TRF_INVERSE ==1);// 检查 TRF_INVERSE 是否为 1
check *=(TRF_DIRECT ==0);// 检查 TRF_DIRECT 是否为 0
check *=(TRF_POSE_FRONT ==0);// 检查 TRF_POSE_FRONT 是否为 0
check *=(ERR_DISABLED ==99);// 检查 ERR_DISABLED 是否为 99
check *=(ERR_CYCLETIME ==1005);// 检查 ERR_CYCLETIME 是否为 1005
check *=(ERR_CHECKSUM ==1006);// 检查 ERR_CHECKSUM 是否为 1006
check *=(ERR_MAX_ROBOTS ==1010);// 检查 ERR_MAX_ROBOTS 是否为 1010
check *=(ERR_ROBOT_LICENSE ==1011);// 检查 ERR_ROBOT_LICENSE 是否为 1011
check *=(ERR_JOG_PAR ==1012);// 检查 ERR_JOG_PAR 是否为 1012
check *=(ERR_JOG_GOTOPOS ==1013);// 检查 ERR_JOG_GOTOPOS 是否为 1013
check *=(ERR_WRONG_JOINT_LIMITS ==1014);// 检查 ERR_WRONG_JOINT_LIMITS 是否为 1014
check *=(ERR_UNITS_SCALING ==1015);// 检查 ERR_UNITS_SCALING 是否为 1015
check *=(ERR_POINT_TYPE ==1016);// 检查 ERR_POINT_TYPE 是否为 1016
check *=(ERR_WRONG_AUX_LIMITS ==1017);// 检查 ERR_WRONG_AUX_LIMITS 是否为 1017
check *=(ERR_TRF_MODE ==1020);// 检查 ERR_TRF_MODE 是否为 1020
check *=(ERR_TRF_MECH ==1021);// 检查 ERR_TRF_MECH 是否为 1021
check *=(ERR_TRF_WORKSPACE ==1022);// 检查 ERR_TRF_WORKSPACE 是否为 1022
check *=(ERR_TRF_MECH_NOT_SUPPORTED ==1023);// 检查 ERR_TRF_MECH_NOT_SUPPORTED 是否为 1023
check *=(ERR_TRF_POSE ==1024);// 检查 ERR_TRF_POSE 是否为 1024
check *=(ERR_TRF_POINTER ==1025);// 检查 ERR_TRF_POINTER 是否为 1025
check *=(ERR_TRF_AXESNUM ==1026);// 检查 ERR_TRF_AXESNUM 是否为 1026
check *=(ERR_TRF_ROT ==1027);// 检查 ERR_TRF_ROT 是否为 1027
check *=(ERR_CALIBRATION ==1030);// 检查 ERR_CALIBRATION 是否为 1030
check *=(ERR_NOT_SUPPORTED ==1050);// 检查 ERR_NOT_SUPPORTED 是否为 1050
check *=(ERR_FILE_NOT_FOUND ==1051);// 检查 ERR_FILE_NOT_FOUND 是否为 1051
check *=(ERR_FILE_EMPTY ==1052);// 检查 ERR_FILE_EMPTY 是否为 1052
check *=(ERR_FILE_END ==1053);// 检查 ERR_FILE_END 是否为 1053
check *=(ERR_FILE_NOMEMORY ==1054);// 检查 ERR_FILE_NOMEMORY 是否为 1054
check *=(ERR_IP_EMPTYSTRING ==1100);// 检查 ERR_IP_EMPTYSTRING 是否为 1100
check *=(ERR_IP_COMMENT ==1101);// 检查 ERR_IP_COMMENT 是否为 1101
check *=(ERR_IP_SYNTAX ==1102);// 检查 ERR_IP_SYNTAX 是否为 1102
check *=(ERR_IP_CONFLICT ==1103);// 检查 ERR_IP_CONFLICT 是否为 1103
check *=(ERR_IP_NOPOINT ==1104);// 检查 ERR_IP_NOPOINT 是否为 1104
check *=(ERR_IP_NOCENTER ==1105);// 检查 ERR_IP_NOCENTER 是否为 1105
check *=(ERR_IP_MAXMFUNC ==1106);// 检查 ERR_IP_MAXMFUNC 是否为 1106
check *=(ERR_IP_POINTINDEX ==1107);// 检查 ERR_IP_POINTINDEX 是否为 1107
check *=(ERR_IP_TOOLINDEX ==1108);// 检查 ERR_IP_TOOLINDEX 是否为 1108
check *=(ERR_IP_FRAMEINDEX ==1109);// 检查 ERR_IP_FRAMEINDEX 是否为 1109
check *=(ERR_IP_MFUNCINDEX ==1110);// 检查 ERR_IP_MFUNCINDEX 是否为 1110
check *=(ERR_IP_FEEDRATE ==1111);// 检查 ERR_IP_FEEDRATE 是否为 1111
check *=(ERR_IP_NOBLOCKS ==1112);// 检查 ERR_IP_NOBLOCKS 是否为 1112
check *=(ERR_IP_LABEL ==1113);// 检查 ERR_IP_LABEL 是否为 1113
check *=(ERR_IP_TRK_INDEX ==1114);// 检查 ERR_IP_TRK_INDEX 是否为 1114
check *=(ERR_IP_JUMP ==1115);// 检查 ERR_IP_JUMP 是否为 1115
check *=(ERR_IP_SUBLEVEL ==1116);// 检查 ERR_IP_SUBLEVEL 是否为 1116
check *=(ERR_IP_TANG ==1117);// 检查 ERR_IP_TANG 是否为 1117
check *=(ERR_IP_IO_INDEX ==1118);// 检查 ERR_IP_IO_INDEX 是否为 1118
check *=(ERR_PP_CIRCLEPOINTS ==1150);// 检查 ERR_PP_CIRCLEPOINTS 是否为 1150
check *=(ERR_PP_CIRCLE_LENGTH ==1151);// 检查 ERR_PP_CIRCLE_LENGTH 是否为 1151
check *=(ERR_PP_CIRCLE_MIDDLEPOINT ==1152);// 检查 ERR_PP_CIRCLE_MIDDLEPOINT 是否为 1152
check *=(ERR_WORKSPACE_ZONE1 ==1160);// 检查 ERR_WORKSPACE_ZONE1 是否为 1160
check *=(ERR_WORKSPACE_ZONE2 ==1161);// 检查 ERR_WORKSPACE_ZONE2 是否为 1161
check *=(ERR_WORKSPACE_ZONE3 ==1162);// 检查 ERR_WORKSPACE_ZONE3 是否为 1162
check *=(ERR_WORKSPACE_ZONE4 ==1163);// 检查 ERR_WORKSPACE_ZONE4 是否为 1163
check *=(ERR_WORKSPACE_ZONE5 ==1164);// 检查 ERR_WORKSPACE_ZONE5 是否为 1164
check *=(ERR_WORKSPACE_ZONE6 ==1165);// 检查 ERR_WORKSPACE_ZONE6 是否为 1165
check *=(ERR_WORKSPACE_ZONE7 ==1166);// 检查 ERR_WORKSPACE_ZONE7 是否为 1166
check *=(ERR_WORKSPACE_ZONE8 ==1167);// 检查 ERR_WORKSPACE_ZONE8 是否为 1167
check *=(ERR_WORKSPACE_ZONE9 ==1168);// 检查 ERR_WORKSPACE_ZONE9 是否为 1168
check *=(ERR_WORKSPACE_ZONE10 ==1169);// 检查 ERR_WORKSPACE_ZONE10 是否为 1169
check *=(ERR_LIMIT_X ==1171);// 检查 ERR_LIMIT_X 是否为 1171
check *=(ERR_LIMIT_Y ==1172);// 检查 ERR_LIMIT_Y 是否为 1172
check *=(ERR_LIMIT_Z ==1173);// 检查 ERR_LIMIT_Z 是否为 1173
check *=(ERR_LIMIT_A ==1174);// 检查 ERR_LIMIT_A 是否为 1174
check *=(ERR_LIMIT_B ==1175);// 检查 ERR_LIMIT_B 是否为 1175
check *=(ERR_LIMIT_C ==1176);// 检查 ERR_LIMIT_C 是否为 1176
check *=(ERR_LIMIT_J1 ==1181);// 检查 ERR_LIMIT_J1 是否为 1181
check *=(ERR_LIMIT_J2 ==1182);// 检查 ERR_LIMIT_J2 是否为 1182
check *=(ERR_LIMIT_J3 ==1183);// 检查 ERR_LIMIT_J3 是否为 1183
check *=(ERR_LIMIT_J4 ==1184);// 检查 ERR_LIMIT_J4 是否为 1184
check *=(ERR_LIMIT_J5 ==1185);// 检查 ERR_LIMIT_J5 是否为 1185
check *=(ERR_LIMIT_J6 ==1186);// 检查 ERR_LIMIT_J6 是否为 1186
check *=(ERR_LIMIT_VEL_J1 ==1187);// 检查 ERR_LIMIT_VEL_J1 是否为 1187
check *=(ERR_LIMIT_VEL_J2 ==1188);// 检查 ERR_LIMIT_VEL_J2 是否为 1188
check *=(ERR_LIMIT_VEL_J3 ==1189);// 检查 ERR_LIMIT_VEL_J3 是否为 1189
check *=(ERR_LIMIT_VEL_J4 ==1190);// 检查 ERR_LIMIT_VEL_J4 是否为 1190
check *=(ERR_LIMIT_VEL_J5 ==1191);// 检查 ERR_LIMIT_VEL_J5 是否为 1191
check *=(ERR_LIMIT_VEL_J6 ==1192);// 检查 ERR_LIMIT_VEL_J6 是否为 1192
check *=(ERR_SPG ==1200);// 检查 ERR_SPG 是否为 1200
check *=(ERR_SPG_CYCLETIME ==1201);// 检查 ERR_SPG_CYCLETIME 是否为 1201
check *=(ERR_SPG_OVERRIDE ==1202);// 检查 ERR_SPG_OVERRIDE 是否为 1202
check *=(ERR_SPG_LIMITS_REACHED ==1203);// 检查 ERR_SPG_LIMITS_REACHED 是否为 1203
check *=(ERR_SPG_DYNCALC ==1204);// 检查 ERR_SPG_DYNCALC 是否为 1204
check *=(ERR_SPG_LIMIT_POS ==1205);// 检查 ERR_SPG_LIMIT_POS 是否为 1205
check *=(ERR_SPG_LIMIT_VEL ==1206);// 检查 ERR_SPG_LIMIT_VEL 是否为 1206
check *=(ERR_SPG_LIMIT_ACC ==1207);// 检查 ERR_SPG_LIMIT_ACC 是否为 1207
check *=(ERR_SPG_LIMIT_JERK ==1208);// 检查 ERR_SPG_LIMIT_JERK 是否为 1208
check *=(ERR_LIMIT_A1 ==1220);// 检查 ERR_LIMIT_A1 是否为 1220
check *=(ERR_LIMIT_A2 ==1221);// 检查 ERR_LIMIT_A2 是否为 1221
check *=(ERR_LIMIT_A3 ==1222);// 检查 ERR_LIMIT_A3 是否为 1222
check *=(ERR_LIMIT_A4 ==1223);// 检查 ERR_LIMIT_A4 是否为 1223
check *=(ERR_LIMIT_A5 ==1224);// 检查 ERR_LIMIT_A5 是否为 1224
check *=(ERR_LIMIT_A6 ==1225);// 检查 ERR_LIMIT_A6 是否为 1225
check *=(ERR_LIMIT_VEL_A1 ==1230);// 检查 ERR_LIMIT_VEL_A1 是否为 1230
check *=(ERR_LIMIT_VEL_A2 ==1231);// 检查 ERR_LIMIT_VEL_A2 是否为 1231
check *=(ERR_LIMIT_VEL_A3 ==1232);// 检查 ERR_LIMIT_VEL_A3 是否为 1232
check *=(ERR_LIMIT_VEL_A4 ==1233);// 检查 ERR_LIMIT_VEL_A4 是否为 1233
check *=(ERR_LIMIT_VEL_A5 ==1234);// 检查 ERR_LIMIT_VEL_A5 是否为 1234
check *=(ERR_LIMIT_VEL_A6 ==1235);// 检查 ERR_LIMIT_VEL_A6 是否为 1235
check *=(ERR_TRK1 ==1250);// 检查 ERR_TRK1 是否为 1250
check *=(ERR_TRK2 ==1251);// 检查 ERR_TRK2 是否为 1251
check *=(ERR_OPTMOT ==1260);// 检查 ERR_OPTMOT 是否为 1260
return check;// 返回最终的检查结果，如果所有检查都通过，则为1，否则为0
}
更多推荐
8
0- 0
已为社区贡献6条内容
所有评论(0)