Diff: StewartPlatform.h

Revision:

0:2b80f11eb1d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StewartPlatform.h	Wed Oct 11 07:05:25 2017 +0000
@@ -0,0 +1,151 @@
+#ifndef __STEWARTPLATFORM_H
+#define __STEWARTPLATFORM_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h> 
+
+#define f1(i) (i-1)  
+/* 把习惯的一阶矩阵的下标转化为C语言数组下标*/
+
+#define f2(i,j,n) ((i-1)*(n)+j-1)
+/* 把习惯的二阶矩阵的下标转化为C语言数组下标*/
+
+//********************************************
+//矩阵数据结构体 
+//********************************************
+typedef struct
+{
+    float Elements[50];                                         //矩阵元素得存储空间 
+    int Size[2];                                        //矩阵的行列数 
+} MatrixType;                                                   //最大支持有50个元素的矩阵 
+
+//********************************************
+//动感平台数据结构体 
+//********************************************
+typedef struct
+{
+    float topRadius;                                                //平台结构尺寸参数 
+    float topInterval;
+    float bottomRadius;
+    float bottomInterval;
+    float lengthOfSteelWheel;
+    float lengthOfCardan;
+    float lengthOfBar;          
+    float x;                                                        //上平台姿态参数 
+    float y;
+    float z;
+    float a;
+    float b;
+    float c;
+    float theta[6];                                                 //角度
+    float theta_servo[6];                                           //舵机角度
+    float BarLength[6];                                             //上下平面对应的顶点之间的距离 
+                        
+} StewartPlatformType;
+
+//********************************************
+//功能：计算矩阵乘法 C=A*B 
+//输入参数：A、B：参加运算的矩阵 
+//输出参数：C：运算结果
+//返回值：计算是否成功 成功返回0 否则返回1 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C);
+
+//********************************************
+//功能：计算矩阵转置 
+//输入参数：A：被转置的矩阵 
+//输出参数：B：转置后的矩阵 
+//返回值：无 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+void MatrixTransposition(MatrixType* A, MatrixType* B);
+
+//********************************************
+//功能：获得矩阵的子阵 
+//输入参数：A：原矩阵 StartRow、StartColumn、EndRow、EndColumn：子阵起始元素 子阵终了元素 
+//输出参数：B：子阵 
+//返回值：无 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B);
+
+//********************************************
+//功能：填充矩阵 将一个矩阵填充到另一个矩阵中 
+//输入参数：A：被填充的矩阵 Row、Column：矩阵填充的位置 B：要填充到被填充矩阵的矩阵 
+//输出参数：A：被填充的矩阵
+//返回值：0 代表成功 1代表失败 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B);
+
+//********************************************
+//功能：指定动平台变换矩阵参数x,y,z,a,b,c，计算动平台上的点A在绝对坐标系下的坐标B  A可以是多个点 一行一个点 
+//输入参数：x,y,z,a,b,c：动平台变换矩阵参数  A：动平台上点的相对坐标 
+//输出参数：B：点在绝对坐标系下的坐标
+//返回值：无
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B);
+
+//********************************************
+//功能：计算矩阵行向量所表示的坐标点之间的距离 
+//输入参数：A, B：要计算距离的矩阵  A,B必须均为n*4的矩阵，维度相同 
+//输出参数：C：包含距离值信息的列向量 
+//返回值：0：计算成功 1：出错 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+int  Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C);
+
+
+//********************************************
+//功能：解析动感平台 
+//输入参数：Platform：动感平台数据结构 包含各种输入输出 
+//输出参数：无 
+//返回值：无 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+void CalStewartPlatform(StewartPlatformType* Platform);
+
+
+//********************************************
+//功能：角度制的三角函数 余弦 
+//输入参数：angle：角度 
+//输出参数：无 
+//返回值：三角函数值 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+float cosd(float angle);
+
+//********************************************
+//功能：角度制的三角函数 正弦 
+//输入参数：angle：角度 
+//输出参数：无 
+//返回值：三角函数值 
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+float sind(float angle);
+
+
+//********************************************
+//功能：在命令行打印矩阵 
+//输入参数：A：要打印的矩阵 
+//输出参数：无 
+//返回值：无
+//调用外部函数：无 
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn 
+//********************************************
+void PrintMatrix(MatrixType* A); 
+
+
+#endif