机器人学基础-ST_code

机器人学基础-ST_code向量运算Vector_PlusVector_minusVector_dotProductVector_CrossProduct矩阵基本运算RotX如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注脚注释也是必不可少的KaTeX数学公式新的甘特图功能，丰富你

cyb_cqu

449人浏览 · 2021-07-15 11:02:07

cyb_cqu · 2021-07-15 11:02:07 发布

向量运算

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

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语法后生成一个完美的目录。

如何改变文本的样式

强调文本 强调文本

加粗文本 加粗文本

标记文本

~~删除文本~~

引用文本

H₂O is是液体。

2¹⁰ 运算结果是 1024.

插入链接与图片

链接: link.

图片: 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
项目2
项目3

计划任务
完成任务

创建一个表格

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

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

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

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

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

SmartyPants

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

TYPE	ASCII	HTML
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

如何创建一个注脚

一个具有注脚的文本。¹

注释也是必不可少的

Markdown将文本转换为 HTML。

KaTeX数学公式

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

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

$\Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,.$

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

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

关于 甘特图 语法，参考这儿,

UML 图表

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

这将产生一个流程图。:

关于 Mermaid 语法，参考这儿,

FLowchart流程图

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

关于 Flowchart流程图 语法，参考这儿.

导出与导入

导出

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

导入

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

注脚的解释 ↩︎

CSDN学习社区

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

更多推荐

嵌入式作业（七）：基于Ardunio的STM32串口通信

嵌入式作业（七）0作业要求1Ardunio 完成STM32的串口通信（1）安装Ardunio IDE（2）stm32串口通信2关于 stduino IDE0作业要求安装 Ardunio IDE 和相关软件支持库，在Ardunio 完成STM32板子的串口通信程序：（1）持续向串口输出“Hello world！”；（2）当接收到“stop!”时，停止输出。网上有一个国人版的MCU集成开发平台， st

CSDN学习社区

JDBC详解

JDBC文章目录JDBC什么是JDBC?JDBC驱动程序:Java使用JDBC访问数据库的步骤:设置classpath:Oracle连接字符串的书写格式:简单的例子:常用数据库的驱动程序及JDBC URL:Oracle数据库:SQL Server数据库MySQL数据库Access数据库PreparedStatement接口:JNDI-数据源（Data Source）与连接池（Connection

CSDN学习社区

“模式识别与机器学习”学习笔记no2.再谈感知机

接**上篇：上篇主要进行了PLA，Pocket算法的理论过程分析和在给定数据集上利用pocket算法对数据集进行分类学习，得到错分数量最少的分类面。上篇中pocket算法的过程已经进行了编程和测试，框架已经建立了起来，这一篇主要上篇中没有提到或涉及不深的几个问题。1.数据集的构造。上篇是直接使用了题目给的向量，这次来根据正态分布来产生数据集。np.random.normal函数可以根据均值和方差生