StewartPlatform.h

00001 #ifndef __STEWARTPLATFORM_H
00002 #define __STEWARTPLATFORM_H
00003 
00004 #include <stdio.h>
00005 #include <stdlib.h>
00006 #include <math.h> 
00007 
00008 #define f1(i) (i-1)  
00009 /* 把习惯的一阶矩阵的下标转化为C语言数组下标*/
00010 
00011 #define f2(i,j,n) ((i-1)*(n)+j-1)
00012 /* 把习惯的二阶矩阵的下标转化为C语言数组下标*/
00013 
00014 //********************************************
00015 //矩阵数据结构体 
00016 //********************************************
00017 typedef struct
00018 {
00019     float Elements[50];                                         //矩阵元素得存储空间 
00020     int Size[2];                                        //矩阵的行列数 
00021 } MatrixType;                                                   //最大支持有50个元素的矩阵 
00022 
00023 //********************************************
00024 //动感平台数据结构体 
00025 //********************************************
00026 typedef struct
00027 {
00028     float topRadius;                                                //平台结构尺寸参数 
00029     float topInterval;
00030     float bottomRadius;
00031     float bottomInterval;
00032     float lengthOfSteelWheel;
00033     float lengthOfCardan;
00034     float lengthOfBar;          
00035     float x;                                                        //上平台姿态参数 
00036     float y;
00037     float z;
00038     float a;
00039     float b;
00040     float c;
00041     float theta[6];                                                 //角度
00042     float theta_servo[6];                                           //舵机角度
00043     float BarLength[6];                                             //上下平面对应的顶点之间的距离 
00044                         
00045 } StewartPlatformType;
00046 
00047 //********************************************
00048 //功能：计算矩阵乘法 C=A*B 
00049 //输入参数：A、B：参加运算的矩阵 
00050 //输出参数：C：运算结果
00051 //返回值：计算是否成功 成功返回0 否则返回1 
00052 //调用外部函数：无 
00053 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00054 //********************************************
00055 int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C);
00056 
00057 //********************************************
00058 //功能：计算矩阵转置 
00059 //输入参数：A：被转置的矩阵 
00060 //输出参数：B：转置后的矩阵 
00061 //返回值：无 
00062 //调用外部函数：无 
00063 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00064 //********************************************
00065 void MatrixTransposition(MatrixType* A, MatrixType* B);
00066 
00067 //********************************************
00068 //功能：获得矩阵的子阵 
00069 //输入参数：A：原矩阵 StartRow、StartColumn、EndRow、EndColumn：子阵起始元素 子阵终了元素 
00070 //输出参数：B：子阵 
00071 //返回值：无 
00072 //调用外部函数：无 
00073 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00074 //********************************************
00075 void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B);
00076 
00077 //********************************************
00078 //功能：填充矩阵 将一个矩阵填充到另一个矩阵中 
00079 //输入参数：A：被填充的矩阵 Row、Column：矩阵填充的位置 B：要填充到被填充矩阵的矩阵 
00080 //输出参数：A：被填充的矩阵
00081 //返回值：0 代表成功 1代表失败 
00082 //调用外部函数：无 
00083 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00084 //********************************************
00085 int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B);
00086 
00087 //********************************************
00088 //功能：指定动平台变换矩阵参数x,y,z,a,b,c，计算动平台上的点A在绝对坐标系下的坐标B  A可以是多个点 一行一个点 
00089 //输入参数：x,y,z,a,b,c：动平台变换矩阵参数  A：动平台上点的相对坐标 
00090 //输出参数：B：点在绝对坐标系下的坐标
00091 //返回值：无
00092 //调用外部函数：无 
00093 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00094 //********************************************
00095 void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B);
00096 
00097 //********************************************
00098 //功能：计算矩阵行向量所表示的坐标点之间的距离 
00099 //输入参数：A, B：要计算距离的矩阵  A,B必须均为n*4的矩阵，维度相同 
00100 //输出参数：C：包含距离值信息的列向量 
00101 //返回值：0：计算成功 1：出错 
00102 //调用外部函数：无 
00103 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00104 //********************************************
00105 int  Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C);
00106 
00107 
00108 //********************************************
00109 //功能：解析动感平台 
00110 //输入参数：Platform：动感平台数据结构 包含各种输入输出 
00111 //输出参数：无 
00112 //返回值：无 
00113 //调用外部函数：无 
00114 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00115 //********************************************
00116 void CalStewartPlatform(StewartPlatformType* Platform);
00117 
00118 
00119 //********************************************
00120 //功能：角度制的三角函数 余弦 
00121 //输入参数：angle：角度 
00122 //输出参数：无 
00123 //返回值：三角函数值 
00124 //调用外部函数：无 
00125 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00126 //********************************************
00127 float cosd(float angle);
00128 
00129 //********************************************
00130 //功能：角度制的三角函数 正弦 
00131 //输入参数：angle：角度 
00132 //输出参数：无 
00133 //返回值：三角函数值 
00134 //调用外部函数：无 
00135 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00136 //********************************************
00137 float sind(float angle);
00138 
00139 
00140 //********************************************
00141 //功能：在命令行打印矩阵 
00142 //输入参数：A：要打印的矩阵 
00143 //输出参数：无 
00144 //返回值：无
00145 //调用外部函数：无 
00146 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 
00147 //********************************************
00148 void PrintMatrix(MatrixType* A); 
00149 
00150 
00151 #endif