Chen Huan
/
StewartOlatform
StewartOlatform
StewartPlatform.h
- Committer:
- heroistired
- Date:
- 2017-10-11
- Revision:
- 0:2b80f11eb1d3
File content as of revision 0:2b80f11eb1d3:
#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