Chen Huan / Mbed 2 deprecated StewartOlatform

StewartOlatform

Dependencies:   mbed

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?

UserRevisionLine numberNew 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