机器人学基础-ST_code

向量运算

向量运算主要是两个向量的加、减、点积和叉积运算

Vector_Plus

//==========ST  Declaration section==========//
//向量求和
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的加法定义为：
//a+b=(a1+b1,a2+b2,……,an+bn)。
FUNCTION Vector_Plus : ARRAY [1..4] OF  LREAL;
VAR_INPUT
	Vector_A 	:  ARRAY [1..4] OF  LREAL;
	Vector_B 	:  ARRAY [1..4] OF  LREAL;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
//==========ST implementation section==========//
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的加法定义为：
//a+b=(a1+b1,a2+b2,……,an+bn)。
Vector_Plus[1] := Vector_A[1] + Vector_B[1];
Vector_Plus[2] := Vector_A[2] + Vector_B[2];
Vector_Plus[3] := Vector_A[3] + Vector_B[3];
Vector_Plus[4] := Vector_A[4] + Vector_B[4];
bDone := TRUE;

Vector_minus

//==========ST  Declaration section==========//
//向量减法
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的减法定义为：
//a+b=(a1-b1,a2-b2,……,an-bn)。
FUNCTION Vector_minus : ARRAY [1..4] OF  LREAL;
VAR_INPUT
	Vector_A 	:  ARRAY [1..4] OF  LREAL;
	Vector_B 	:  ARRAY [1..4] OF  LREAL;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
//==========ST implementation section==========//
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的减法定义为：
//a+b=(a1-b1,a2-b2,……,an-bn)。
Vector_minus[1] := Vector_A[1] - Vector_B[1];
Vector_minus[2] := Vector_A[2] - Vector_B[2];
Vector_minus[3] := Vector_A[3] - Vector_B[3];
Vector_minus[4] := Vector_A[4] - Vector_B[4];
bDone := TRUE;

Vector_dotProduct

//==========ST  Declaration section==========//
//向量点积
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的点积定义为：
//a·b=a1b1+a2b2+……+anbn。
FUNCTION Vector_dotProduct : LREAL
VAR_INPUT
	Vector_A 	:  ARRAY [1..4] OF  LREAL;
	Vector_B 	:  ARRAY [1..4] OF  LREAL;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR


//==========ST implementation section==========//
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的点积定义为：
//a·b=a1b1+a2b2+……+anbn。
Vector_dotProduct := Vector_A[1]*Vector_B[1]+Vector_A[2]*Vector_B[2]+Vector_A[3]*Vector_B[3]+Vector_A[4]*Vector_B[4];
bDone := TRUE;

Vector_CrossProduct

//==========ST  Declaration section==========//
//向量叉积
//两个向量a = [a1, a2, a3]和b = [b1, b2 b3]的叉积定义为：
//a×b=(a2*b3-a3*b2,a3*b1-a1*b3,a1*b2-a2*b1)。
FUNCTION Vector_CrossProduct :ARRAY [1..3] OF  LREAL
VAR_INPUT
	Vector_A 	:  ARRAY [1..3] OF  LREAL;
	Vector_B 	:  ARRAY [1..3] OF  LREAL;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR

//==========ST implementation section==========//
//两个向量a = [a1, a2, a3]和b = [b1, b2 b3]的叉积定义为：
//a×b=(a2*b3-a3*b2,a3*b1-a1*b3,a1*b2-a2*b1)。
Vector_CrossProduct[1] := Vector_A[2]*Vector_B[3] - Vector_A[3]*Vector_B[2];
Vector_CrossProduct[2] := Vector_A[3]*Vector_B[1] - Vector_A[1]*Vector_B[3];
Vector_CrossProduct[3] := Vector_A[1]*Vector_B[2] - Vector_A[2]*Vector_B[1];
bDone := TRUE;

Vector_dotProduct_str

//==========ST  Declaration section==========//
//向量点积
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的点积定义为：
//a·b=a1b1+a2b2+……+anbn。
FUNCTION Vector_dotProduct_str : STRING
VAR_INPUT
	Vector_A 	:  ARRAY [1..4] OF  STRING;
	Vector_B 	:  ARRAY [1..4] OF  STRING;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
	str1: STRING;
	str2: STRING;
	str3: STRING;
	str4: STRING;
END_VAR


//==========ST implementation section==========//
//两个向量a = [a1, a2,…, an]和b = [b1, b2,…, bn]的点积定义为：
//a·b=a1b1+a2b2+……+anbn。

str1 := CONCAT(STR1:=CONCAT(STR1:=Vector_A[1] , STR2:='*' ), STR2:=Vector_B[1]);
str2 := CONCAT(STR1:=CONCAT(STR1:=Vector_A[2] , STR2:='*' ), STR2:=Vector_B[2]);
str3 := CONCAT(STR1:=CONCAT(STR1:=Vector_A[3] , STR2:='*' ), STR2:=Vector_B[3]);
str4 := CONCAT(STR1:=CONCAT(STR1:=Vector_A[4] , STR2:='*' ), STR2:=Vector_B[4]);

str1 := CONCAT(STR1:=str1 , STR2:= '+');
str2 := CONCAT(STR1:=str2 , STR2:= '+');
str3 := CONCAT(STR1:=str3 , STR2:= '+');

str1 := CONCAT(STR1:=str1 , STR2:=str2 );
str1 := CONCAT(STR1:=str1 , STR2:=str3 );
str1 := CONCAT(STR1:=str1 , STR2:=str4 );

Vector_dotProduct_str :=str1;

bDone := TRUE;

矩阵运算

RotX

//==========ST  Declaration section==========//
//RotX = Rot(x,rad)  4*4  绕x轴的旋转矩阵
//%Rotx       绕x轴的旋转矩阵
//%rad      绕x轴旋转的角度
//Rotx= [
//       1,         0,                          0,                     0;
//       0,         cos(rad),          -sin(rad),             0;
//       0,         sin(rad),            cos(rad),            0;
//       0,          0,                         0,                     1
// ];
FUNCTION RotX :  ARRAY [1..4] OF ARRAY [1..4 ]OF LREAL
VAR_INPUT
	rad 	: 	LREAL;   //绕X轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
//==========ST implementation section==========//
//%Rotx       绕x轴的旋转矩阵
//%rad      绕x轴旋转的角度
//Rotx= [
//       1,         0,                          0,                     0;
//       0,         cos(rad),          -sin(rad),             0;
//       0,         sin(rad),            cos(rad),            0;
//       0,          0,                         0,                     1
// ];
Rotx[1][1]:= 1;
Rotx[1] [2]:= 0;
Rotx[1][3] := 0;
Rotx[1][4] := 0;

Rotx[2][1] := 0;
Rotx[2][2] := COS(rad);
Rotx[2][3] :=  -SIN(rad);
Rotx[2][4] := 0;

Rotx[3][1] := 0;
Rotx[3][2] :=  SIN(rad);
Rotx[3][3] :=  COS(rad);
Rotx[3][4] := 0;

Rotx[4][1] := 0;
Rotx[4][2] := 0;
Rotx[4][3] := 0;
Rotx[4][4] := 1;
bDone := TRUE;

RotX_str

//==========ST  Declaration section==========//
//RotX = Rot(x,rad)  4*4  绕x轴的旋转矩阵
//%Rotx       绕x轴的旋转矩阵
//%rad      绕x轴旋转的角度
//Rotx= [
//       1,         0,                          0,                     0;
//       0,         cos(rad),          -sin(rad),             0;
//       0,         sin(rad),            cos(rad),            0;
//       0,          0,                         0,                     1
// ];
FUNCTION RotX_str :  ARRAY [1..4] OF ARRAY [1..4 ]OF STRING
VAR_INPUT
	rad 	: 	STRING := '0';   //绕X轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR


//==========ST implementation section==========//
//%Rotx       绕x轴的旋转矩阵
//%rad      绕x轴旋转的角度
//Rotx= [
//       1,         0,                          0,                     0;
//       0,         cos(rad),          -sin(rad),             0;
//       0,         sin(rad),            cos(rad),            0;
//       0,          0,                         0,                     1
// ];
RotX_str[1][1]:= '1';
RotX_str[1] [2]:= '0';
RotX_str[1][3] := '0';
RotX_str[1][4] := '0';

RotX_str[2][1] := '0';
RotX_str[2][2] := CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')');
RotX_str[2][3] := CONCAT(STR1:=CONCAT(STR1:='-SIN(' , STR2:= rad) , STR2:= ')');
RotX_str[2][4] := '0';

RotX_str[3][1] := '0';
RotX_str[3][2] :=  CONCAT(STR1:=CONCAT(STR1:='SIN(' , STR2:= rad) , STR2:= ')');
RotX_str[3][3] :=  CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')');
RotX_str[3][4] := '0';

RotX_str[4][1] := '0';
RotX_str[4][2] := '0';
RotX_str[4][3] := '0';
RotX_str[4][4] := '1';
bDone := TRUE;

RotY

//==========ST  Declaration section==========//
//RotY = Rot(y,rad)  4*4  绕y轴的旋转矩阵
//%Roty  绕y轴的旋转矩阵
//%rad      绕y旋转的角度
//Roty= [
//        cos(rad),         0,            sin(rad) ,         0;
//       0,        1,          0,                                       0;
//       -sin(rad),         0,            cos(rad),            0;
//       0,          0,                         0,                      1
// ];
FUNCTION RotY :  ARRAY [1..4] OF ARRAY [1..4 ]OF LREAL
VAR_INPUT
	rad 	: 	LREAL;  //绕Y轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR

//==========ST implementation section==========//
//%Roty  绕y轴的旋转矩阵
//%rad      绕y旋转的角度
//Roty= [
//        cos(rad),         0,            sin(rad) ,         0;
//       0,        1,          0,                                       0;
//       -sin(rad),         0,            cos(rad),            0;
//       0,          0,                         0,                      1
// ];
RotY[1][1]:= COS(rad) ;
RotY[1][2] := 0;
RotY[1][3] := SIN(rad) ;
RotY[1][4] := 0;

RotY[2][1] := 0;
RotY[2][2] := 1;
RotY[2][3] :=  0;
RotY[2][4] := 0;

RotY[3][1] :=  -SIN(rad);
RotY[3][2] :=  0;
RotY[3][3] :=  COS(rad);
RotY[3][4] := 0;

RotY[4][1] := 0;
RotY[4][2] := 0;
RotY[4][3] := 0;
RotY[4][4] := 1;
bDone := TRUE;

RotY_str

//==========ST  Declaration section==========//
//RotY = Rot(y,rad)  4*4  绕y轴的旋转矩阵
//%Roty  绕y轴的旋转矩阵
//%rad      绕y旋转的角度
//Roty= [
//        cos(rad),         0,            sin(rad) ,         0;
//       0,        1,          0,                                       0;
//       -sin(rad),         0,            cos(rad),            0;
//       0,          0,                         0,                      1
// ];
FUNCTION RotY_str :  ARRAY [1..4] OF ARRAY [1..4 ]OF STRING
VAR_INPUT
	rad 	: 	STRING;  //绕Y轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR



//==========ST implementation section==========//
//%Roty  绕y轴的旋转矩阵
//%rad      绕y旋转的角度
//Roty= [
//        cos(rad),         0,            sin(rad) ,         0;
//       0,        1,          0,                                       0;
//       -sin(rad),         0,            cos(rad),            0;
//       0,          0,                         0,                      1
// ];
RotY_str[1][1] := CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')'); 
RotY_str[1][2] := '0';
RotY_str[1][3] := CONCAT(STR1:=CONCAT(STR1:='SIN(' , STR2:= rad) , STR2:= ')'); 
RotY_str[1][4] := '0';

RotY_str[2][1] := '0';
RotY_str[2][2] := '1';
RotY_str[2][3] :=  '0';
RotY_str[2][4] := '0';

RotY_str[3][1] := CONCAT(STR1:=CONCAT(STR1:='-SIN(' , STR2:= rad) , STR2:= ')');
RotY_str[3][2] :=  '0';
RotY_str[3][3] := CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')');
RotY_str[3][4] := '0';

RotY_str[4][1] := '0';
RotY_str[4][2] := '0';
RotY_str[4][3] := '0';
RotY_str[4][4] := '1';
bDone := TRUE;

RotZ

//==========ST  Declaration section==========//
//RotZ= Rot(z,rad)  4*4  绕Z轴的旋转矩阵
//%RotZ   关于z轴的旋转矩阵
//%rad      绕z旋转的角度
//Rotz= [
//        cos(rad),     -sin(rad) ,   0,              0;
//       sin(rad),        cos(rad),     0,             0;
//       0,         0,                1,             0;
//       0,          0,                            0,               1
// ];
FUNCTION RotZ :  ARRAY [1..4] OF ARRAY [1..4 ]OF LREAL
VAR_INPUT
	rad 	: 	LREAL; //绕Z轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR


//==========ST implementation section==========//
//%RotZ   关于z轴的旋转矩阵
//%rad      绕z旋转的角度
//Rotz= [
//        cos(rad),     -sin(rad) ,   0,              0;
//       sin(rad),        cos(rad),     0,             0;
//       0,         0,                1,             0;
//       0,          0,                            0,               1
// ];
RotZ[1][1]:= COS(rad);
RotZ[1][2] := -SIN(rad);
RotZ[1][3] := 0;
RotZ[1][4] := 0;

RotZ[2][1] :=  SIN(rad);
RotZ[2][2] := COS(rad);
RotZ[2][3] :=  0;
RotZ[2][4] := 0;

RotZ[3][1] := 0;
RotZ[3][2] := 0;
RotZ[3][3] := 1;
RotZ[3][4] := 0;

RotZ[4][1] := 0;
RotZ[4][2] := 0;
RotZ[4][3] := 0;
RotZ[4][4] := 1;
bDone := TRUE;

RotZ_str

//==========ST  Declaration section==========//
//RotZ= Rot(z,rad)  4*4  绕Z轴的旋转矩阵
//%RotZ   关于z轴的旋转矩阵
//%rad      绕z旋转的角度
//Rotz= [
//        cos(rad),     -sin(rad) ,   0,              0;
//       sin(rad),        cos(rad),     0,             0;
//       0,         0,                1,             0;
//       0,          0,                            0,               1
// ];
FUNCTION RotZ_str :  ARRAY [1..4] OF ARRAY [1..4 ]OF STRING
VAR_INPUT
	rad 	: 	STRING; //绕Z轴旋转角度 单位弧度
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR

//==========ST implementation section==========//
//%RotZ   关于z轴的旋转矩阵
//%rad      绕z旋转的角度
//Rotz= [
//        cos(rad),     -sin(rad) ,   0,              0;
//       sin(rad),        cos(rad),     0,             0;
//       0,         0,                1,             0;
//       0,          0,                            0,               1
// ];
RotZ_str[1][1] := CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')'); 
RotZ_str[1][2] := CONCAT(STR1:=CONCAT(STR1:='-SIN(' , STR2:= rad) , STR2:= ')'); 
RotZ_str[1][3] := '0';
RotZ_str[1][4] := '0';

RotZ_str[2][1] := CONCAT(STR1:=CONCAT(STR1:='SIN(' , STR2:= rad) , STR2:= ')'); 
RotZ_str[2][2] := CONCAT(STR1:=CONCAT(STR1:='COS(' , STR2:= rad) , STR2:= ')'); 
RotZ_str[2][3] :=  '0';
RotZ_str[2][4] := '0';

RotZ_str[3][1] := '0';
RotZ_str[3][2] := '0';
RotZ_str[3][3] := '1';
RotZ_str[3][4] := '0';

RotZ_str[4][1] := '0';
RotZ_str[4][2] := '0';
RotZ_str[4][3] := '0';
RotZ_str[4][4] := '1';
bDone := TRUE;

直接输入1次#，并按下space后，将生成1级标题。
输入2次#，并按下space后，将生成2级标题。
以此类推，我们支持6级标题。有助于使用TOC语法后生成一个完美的目录。

如何改变文本的样式

强调文本 强调文本

加粗文本 加粗文本

标记文本

删除文本

引用文本

H2O is是液体。

210 运算结果是 1024.

插入链接与图片

链接: link.

图片: Alt

带尺寸的图片: Alt

居中的图片: Alt

居中并且带尺寸的图片: Alt

当然，我们为了让用户更加便捷，我们增加了图片拖拽功能。

如何插入一段漂亮的代码片

//向量叉积
//两个向量a = [a1, a2, a3]和b = [b1, b2 b3]的叉积定义为：
//a×b=(a2*b3-a3*b2,a3*b1-a1*b3,a1*b2-a2*b1)。
FUNCTION Vector_CrossProduct :ARRAY [1..3] OF  LREAL
VAR_INPUT
	Vector_A 	:  ARRAY [1..3] OF  LREAL;
	Vector_B 	:  ARRAY [1..3] OF  LREAL;
END_VAR
VAR_OUTPUT
	bDone : BOOL; //计算完成标志位
END_VAR
VAR
END_VAR
//两个向量a = [a1, a2, a3]和b = [b1, b2 b3]的叉积定义为：
//a×b=(a2*b3-a3*b2,a3*b1-a1*b3,a1*b2-a2*b1)。
Vector_CrossProduct[1] := Vector_A[2]*Vector_B[3] - Vector_A[3]*Vector_B[2];
Vector_CrossProduct[2] := Vector_A[3]*Vector_B[1] - Vector_A[1]*Vector_B[3];
Vector_CrossProduct[3] := Vector_A[1]*Vector_B[2] - Vector_A[2]*Vector_B[1];
bDone := TRUE;

生成一个适合你的列表

  • 项目
    • 项目
      • 项目
  1. 项目1
  2. 项目2
  3. 项目3
  • 计划任务
  • 完成任务

创建一个表格

一个简单的表格是这么创建的：

项目Value
电脑$1600
手机$12
导管$1

设定内容居中、居左、居右

使用:---------:居中
使用:----------居左
使用----------:居右

第一列第二列第三列
第一列文本居中第二列文本居右第三列文本居左

SmartyPants

SmartyPants将ASCII标点字符转换为“智能”印刷标点HTML实体。例如：

TYPEASCIIHTML
Single backticks'Isn't this fun?'‘Isn’t this fun?’
Quotes"Isn't this fun?"“Isn’t this fun?”
Dashes-- is en-dash, --- is em-dash– is en-dash, — is em-dash

创建一个自定义列表

Markdown
Text-to- HTML conversion tool
Authors
John
Luke

如何创建一个注脚

一个具有注脚的文本。1

注释也是必不可少的

Markdown将文本转换为 HTML。

KaTeX数学公式

您可以使用渲染LaTeX数学表达式 KaTeX:

Gamma公式展示 Γ ( n ) = ( n − 1 ) ! ∀ n ∈ N \Gamma(n) = (n-1)!\quad\forall n\in\mathbb N Γ(n)=(n−1)!∀n∈N 是通过欧拉积分

Γ ( z ) = ∫ 0 ∞ t z − 1 e − t d t   . \Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,. Γ(z)=∫0∞​tz−1e−tdt.

你可以找到更多关于的信息 LaTeX 数学表达式here.

新的甘特图功能，丰富你的文章

Mon 06 Mon 13 Mon 20 已完成 进行中 计划一 计划二 现有任务 Adding GANTT diagram functionality to mermaid
  • 关于 甘特图 语法，参考 这儿,

UML 图表

可以使用UML图表进行渲染。 Mermaid. 例如下面产生的一个序列图：

张三 李四 王五 你好！李四, 最近怎么样? 你最近怎么样，王五？ 我很好，谢谢! 我很好，谢谢! 李四想了很长时间, 文字太长了 不适合放在一行. 打量着王五... 很好... 王五, 你怎么样? 张三 李四 王五

这将产生一个流程图。:

链接
长方形
圆
圆角长方形
菱形
  • 关于 Mermaid 语法，参考 这儿,

FLowchart流程图

我们依旧会支持flowchart的流程图：

Created with Raphaël 2.3.0 开始 我的操作 确认？ 结束 yes no
  • 关于 Flowchart流程图 语法，参考 这儿.

导出与导入

导出

如果你想尝试使用此编辑器, 你可以在此篇文章任意编辑。当你完成了一篇文章的写作, 在上方工具栏找到 文章导出 ，生成一个.md文件或者.html文件进行本地保存。

导入

如果你想加载一篇你写过的.md文件，在上方工具栏可以选择导入功能进行对应扩展名的文件导入，
继续你的创作。

  1. 注脚的解释 ↩︎

Logo
CSDN学习社区

CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！

更多推荐

cover

1 小时解读鸿蒙 10 大热点问题

avatar CSDN学习社区
cover

用 OpenAI Assistants 做大模型应用开发

avatar CSDN学习社区
cover

1 小时解读鸿蒙 10 大热点问题

avatar CSDN学习社区