Chen Huan
/
StewartOlatform
StewartOlatform
StewartPlatform.h@0:2b80f11eb1d3, 2017-10-11 (annotated)
- Committer:
- heroistired
- Date:
- Wed Oct 11 07:05:25 2017 +0000
- Revision:
- 0:2b80f11eb1d3
stewart platform
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
heroistired | 0:2b80f11eb1d3 | 1 | #ifndef __STEWARTPLATFORM_H |
heroistired | 0:2b80f11eb1d3 | 2 | #define __STEWARTPLATFORM_H |
heroistired | 0:2b80f11eb1d3 | 3 | |
heroistired | 0:2b80f11eb1d3 | 4 | #include <stdio.h> |
heroistired | 0:2b80f11eb1d3 | 5 | #include <stdlib.h> |
heroistired | 0:2b80f11eb1d3 | 6 | #include <math.h> |
heroistired | 0:2b80f11eb1d3 | 7 | |
heroistired | 0:2b80f11eb1d3 | 8 | #define f1(i) (i-1) |
heroistired | 0:2b80f11eb1d3 | 9 | /* 把习惯的一阶矩阵的下标转化为C语言数组下标*/ |
heroistired | 0:2b80f11eb1d3 | 10 | |
heroistired | 0:2b80f11eb1d3 | 11 | #define f2(i,j,n) ((i-1)*(n)+j-1) |
heroistired | 0:2b80f11eb1d3 | 12 | /* 把习惯的二阶矩阵的下标转化为C语言数组下标*/ |
heroistired | 0:2b80f11eb1d3 | 13 | |
heroistired | 0:2b80f11eb1d3 | 14 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 15 | //矩阵数据结构体 |
heroistired | 0:2b80f11eb1d3 | 16 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 17 | typedef struct |
heroistired | 0:2b80f11eb1d3 | 18 | { |
heroistired | 0:2b80f11eb1d3 | 19 | float Elements[50]; //矩阵元素得存储空间 |
heroistired | 0:2b80f11eb1d3 | 20 | int Size[2]; //矩阵的行列数 |
heroistired | 0:2b80f11eb1d3 | 21 | } MatrixType; //最大支持有50个元素的矩阵 |
heroistired | 0:2b80f11eb1d3 | 22 | |
heroistired | 0:2b80f11eb1d3 | 23 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 24 | //动感平台数据结构体 |
heroistired | 0:2b80f11eb1d3 | 25 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 26 | typedef struct |
heroistired | 0:2b80f11eb1d3 | 27 | { |
heroistired | 0:2b80f11eb1d3 | 28 | float topRadius; //平台结构尺寸参数 |
heroistired | 0:2b80f11eb1d3 | 29 | float topInterval; |
heroistired | 0:2b80f11eb1d3 | 30 | float bottomRadius; |
heroistired | 0:2b80f11eb1d3 | 31 | float bottomInterval; |
heroistired | 0:2b80f11eb1d3 | 32 | float lengthOfSteelWheel; |
heroistired | 0:2b80f11eb1d3 | 33 | float lengthOfCardan; |
heroistired | 0:2b80f11eb1d3 | 34 | float lengthOfBar; |
heroistired | 0:2b80f11eb1d3 | 35 | float x; //上平台姿态参数 |
heroistired | 0:2b80f11eb1d3 | 36 | float y; |
heroistired | 0:2b80f11eb1d3 | 37 | float z; |
heroistired | 0:2b80f11eb1d3 | 38 | float a; |
heroistired | 0:2b80f11eb1d3 | 39 | float b; |
heroistired | 0:2b80f11eb1d3 | 40 | float c; |
heroistired | 0:2b80f11eb1d3 | 41 | float theta[6]; //角度 |
heroistired | 0:2b80f11eb1d3 | 42 | float theta_servo[6]; //舵机角度 |
heroistired | 0:2b80f11eb1d3 | 43 | float BarLength[6]; //上下平面对应的顶点之间的距离 |
heroistired | 0:2b80f11eb1d3 | 44 | |
heroistired | 0:2b80f11eb1d3 | 45 | } StewartPlatformType; |
heroistired | 0:2b80f11eb1d3 | 46 | |
heroistired | 0:2b80f11eb1d3 | 47 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 48 | //功能:计算矩阵乘法 C=A*B |
heroistired | 0:2b80f11eb1d3 | 49 | //输入参数:A、B:参加运算的矩阵 |
heroistired | 0:2b80f11eb1d3 | 50 | //输出参数:C:运算结果 |
heroistired | 0:2b80f11eb1d3 | 51 | //返回值:计算是否成功 成功返回0 否则返回1 |
heroistired | 0:2b80f11eb1d3 | 52 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 53 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 54 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 55 | int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C); |
heroistired | 0:2b80f11eb1d3 | 56 | |
heroistired | 0:2b80f11eb1d3 | 57 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 58 | //功能:计算矩阵转置 |
heroistired | 0:2b80f11eb1d3 | 59 | //输入参数:A:被转置的矩阵 |
heroistired | 0:2b80f11eb1d3 | 60 | //输出参数:B:转置后的矩阵 |
heroistired | 0:2b80f11eb1d3 | 61 | //返回值:无 |
heroistired | 0:2b80f11eb1d3 | 62 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 63 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 64 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 65 | void MatrixTransposition(MatrixType* A, MatrixType* B); |
heroistired | 0:2b80f11eb1d3 | 66 | |
heroistired | 0:2b80f11eb1d3 | 67 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 68 | //功能:获得矩阵的子阵 |
heroistired | 0:2b80f11eb1d3 | 69 | //输入参数:A:原矩阵 StartRow、StartColumn、EndRow、EndColumn:子阵起始元素 子阵终了元素 |
heroistired | 0:2b80f11eb1d3 | 70 | //输出参数:B:子阵 |
heroistired | 0:2b80f11eb1d3 | 71 | //返回值:无 |
heroistired | 0:2b80f11eb1d3 | 72 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 73 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 74 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 75 | void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B); |
heroistired | 0:2b80f11eb1d3 | 76 | |
heroistired | 0:2b80f11eb1d3 | 77 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 78 | //功能:填充矩阵 将一个矩阵填充到另一个矩阵中 |
heroistired | 0:2b80f11eb1d3 | 79 | //输入参数:A:被填充的矩阵 Row、Column:矩阵填充的位置 B:要填充到被填充矩阵的矩阵 |
heroistired | 0:2b80f11eb1d3 | 80 | //输出参数:A:被填充的矩阵 |
heroistired | 0:2b80f11eb1d3 | 81 | //返回值:0 代表成功 1代表失败 |
heroistired | 0:2b80f11eb1d3 | 82 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 83 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 84 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 85 | int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B); |
heroistired | 0:2b80f11eb1d3 | 86 | |
heroistired | 0:2b80f11eb1d3 | 87 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 88 | //功能:指定动平台变换矩阵参数x,y,z,a,b,c,计算动平台上的点A在绝对坐标系下的坐标B A可以是多个点 一行一个点 |
heroistired | 0:2b80f11eb1d3 | 89 | //输入参数:x,y,z,a,b,c:动平台变换矩阵参数 A:动平台上点的相对坐标 |
heroistired | 0:2b80f11eb1d3 | 90 | //输出参数:B:点在绝对坐标系下的坐标 |
heroistired | 0:2b80f11eb1d3 | 91 | //返回值:无 |
heroistired | 0:2b80f11eb1d3 | 92 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 93 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 94 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 95 | void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B); |
heroistired | 0:2b80f11eb1d3 | 96 | |
heroistired | 0:2b80f11eb1d3 | 97 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 98 | //功能:计算矩阵行向量所表示的坐标点之间的距离 |
heroistired | 0:2b80f11eb1d3 | 99 | //输入参数:A, B:要计算距离的矩阵 A,B必须均为n*4的矩阵,维度相同 |
heroistired | 0:2b80f11eb1d3 | 100 | //输出参数:C:包含距离值信息的列向量 |
heroistired | 0:2b80f11eb1d3 | 101 | //返回值:0:计算成功 1:出错 |
heroistired | 0:2b80f11eb1d3 | 102 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 103 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 104 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 105 | int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C); |
heroistired | 0:2b80f11eb1d3 | 106 | |
heroistired | 0:2b80f11eb1d3 | 107 | |
heroistired | 0:2b80f11eb1d3 | 108 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 109 | //功能:解析动感平台 |
heroistired | 0:2b80f11eb1d3 | 110 | //输入参数:Platform:动感平台数据结构 包含各种输入输出 |
heroistired | 0:2b80f11eb1d3 | 111 | //输出参数:无 |
heroistired | 0:2b80f11eb1d3 | 112 | //返回值:无 |
heroistired | 0:2b80f11eb1d3 | 113 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 114 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 115 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 116 | void CalStewartPlatform(StewartPlatformType* Platform); |
heroistired | 0:2b80f11eb1d3 | 117 | |
heroistired | 0:2b80f11eb1d3 | 118 | |
heroistired | 0:2b80f11eb1d3 | 119 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 120 | //功能:角度制的三角函数 余弦 |
heroistired | 0:2b80f11eb1d3 | 121 | //输入参数:angle:角度 |
heroistired | 0:2b80f11eb1d3 | 122 | //输出参数:无 |
heroistired | 0:2b80f11eb1d3 | 123 | //返回值:三角函数值 |
heroistired | 0:2b80f11eb1d3 | 124 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 125 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 126 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 127 | float cosd(float angle); |
heroistired | 0:2b80f11eb1d3 | 128 | |
heroistired | 0:2b80f11eb1d3 | 129 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 130 | //功能:角度制的三角函数 正弦 |
heroistired | 0:2b80f11eb1d3 | 131 | //输入参数:angle:角度 |
heroistired | 0:2b80f11eb1d3 | 132 | //输出参数:无 |
heroistired | 0:2b80f11eb1d3 | 133 | //返回值:三角函数值 |
heroistired | 0:2b80f11eb1d3 | 134 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 135 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 136 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 137 | float sind(float angle); |
heroistired | 0:2b80f11eb1d3 | 138 | |
heroistired | 0:2b80f11eb1d3 | 139 | |
heroistired | 0:2b80f11eb1d3 | 140 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 141 | //功能:在命令行打印矩阵 |
heroistired | 0:2b80f11eb1d3 | 142 | //输入参数:A:要打印的矩阵 |
heroistired | 0:2b80f11eb1d3 | 143 | //输出参数:无 |
heroistired | 0:2b80f11eb1d3 | 144 | //返回值:无 |
heroistired | 0:2b80f11eb1d3 | 145 | //调用外部函数:无 |
heroistired | 0:2b80f11eb1d3 | 146 | //作者:陈欢 h-che14@mails.tsinghua.edu.cn |
heroistired | 0:2b80f11eb1d3 | 147 | //******************************************** |
heroistired | 0:2b80f11eb1d3 | 148 | void PrintMatrix(MatrixType* A); |
heroistired | 0:2b80f11eb1d3 | 149 | |
heroistired | 0:2b80f11eb1d3 | 150 | |
heroistired | 0:2b80f11eb1d3 | 151 | #endif |