Chen Huan
StewartOlatform
Diff: StewartPlatform.cpp
- Revision:
- 0:2b80f11eb1d3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StewartPlatform.cpp Wed Oct 11 07:05:25 2017 +0000
@@ -0,0 +1,768 @@
+#include "StewartPlatform.h"
+
+//********************************************
+//功能：计算矩阵乘法 C=A*B
+//输入参数：A、B：参加运算的矩阵
+//输出参数：C：运算结果
+//返回值：计算是否成功 成功返回0 否则返回1
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C)
+{
+ int i, j, k; //循环控制变量
+ int posA, posB, posC; //矩阵下标索引
+
+ //判断异常
+ if(A->Size[1] != B->Size[0]) //维度不匹配
+ return 1;
+ if(A->Size[0] * B->Size[1] > 50) //C矩阵规模太大
+ return 1;
+
+ //计算矩阵乘法
+ C->Size[0] = A->Size[0]; //得到C的行列数
+ C->Size[1] = B->Size[1];
+ for(i = 1; i <= C->Size[0]; i++) //行循环
+ {
+ for(j = 1; j <= C->Size[1]; j++) //列循环
+ {
+ posC = f2(i,j,C->Size[1]);
+ C->Elements[posC] = 0;
+ for(k = 1; k <= A->Size[1]; k++) //计算乘法结果
+ {
+ posA = f2(i,k,A->Size[1]);
+ posB = f2(k,j,B->Size[1]);
+ C->Elements[posC] += A->Elements[posA] * B->Elements[posB];
+ }
+ }
+ }
+ return 0;
+}
+
+//********************************************
+//功能：计算矩阵转置
+//输入参数：A：被转置的矩阵
+//输出参数：B：转置后的矩阵
+//返回值：无
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+void MatrixTransposition(MatrixType* A, MatrixType* B)
+{
+ int i, j;
+ int posA, posB;
+
+ B->Size[0] = A->Size[1];
+ B->Size[1] = A->Size[0];
+
+ for(i = 1; i <= A->Size[0]; i++)
+ {
+ for(j = 1; j <= A->Size[1]; j++)
+ {
+ posA = f2(i,j,A->Size[1]);
+ posB = f2(j,i,B->Size[1]);
+ B->Elements[posB] = A->Elements[posA];
+ }
+ }
+}
+
+//********************************************
+//功能：获得矩阵的子阵
+//输入参数：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)
+{
+ int i, j; //循环控制变量
+ int posA, posB; //矩阵索引
+
+ B->Size[0] = EndRow - StartRow + 1; //计算子阵的维度
+ B->Size[1] = EndColumn - StartColumn + 1;
+ for(i = 1; i <= B->Size[0]; i++)
+ {
+ for(j = 1; j <= B->Size[1]; j++)
+ {
+ posA = f2(StartRow + i - 1, StartColumn + j - 1, A->Size[1]);
+ posB = f2(i, j, B->Size[1]);
+ B->Elements[posB] = A->Elements[posA];
+ }
+ }
+}
+
+//********************************************
+//功能：填充矩阵 将一个矩阵填充到另一个矩阵中
+//输入参数：A：被填充的矩阵 Row、Column：矩阵填充的位置 B：要填充到被填充矩阵的矩阵
+//输出参数：A：被填充的矩阵
+//返回值：0 代表成功 1代表失败
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B)
+{
+ int i, j, posA, posB;
+
+ if((Row + B->Size[0] - 1) > A->Size[0])
+ {
+ return 1;
+ }
+ else if((Column + B->Size[1] - 1) > A->Size[1])
+ {
+ return 1;
+ }
+ else
+ {
+ for(i = 1; i <= B->Size[0]; i++)
+ {
+ for(j = 1; j <= B->Size[1]; j++)
+ {
+ posA = f2(Row + i - 1, Column + j - 1, A->Size[1]);
+ posB = f2(i, j, B->Size[1]);
+ A->Elements[posA] = B->Elements[posB];
+ }
+ }
+ return 0;
+ }
+}
+
+//********************************************
+//功能：指定动平台变换矩阵参数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)
+{
+ int i;
+
+ MatrixType T; //原点平移
+ MatrixType Ra; //俯仰 YAW
+ MatrixType Rb; //横滚 ROLL
+ MatrixType Rc; //偏航 PITCH
+ MatrixType temp1, temp2, temp3;
+
+ T.Size[0] = 4;
+ T.Size[1] = 4;
+ T.Elements[f2(1,1,T.Size[1])] = 1;
+ T.Elements[f2(1,2,T.Size[1])] = 0;
+ T.Elements[f2(1,3,T.Size[1])] = 0;
+ T.Elements[f2(1,4,T.Size[1])] = x;
+ T.Elements[f2(2,1,T.Size[1])] = 0;
+ T.Elements[f2(2,2,T.Size[1])] = 1;
+ T.Elements[f2(2,3,T.Size[1])] = 0;
+ T.Elements[f2(2,4,T.Size[1])] = y;
+ T.Elements[f2(3,1,T.Size[1])] = 0;
+ T.Elements[f2(3,2,T.Size[1])] = 0;
+ T.Elements[f2(3,3,T.Size[1])] = 1;
+ T.Elements[f2(3,4,T.Size[1])] = z;
+ T.Elements[f2(4,1,T.Size[1])] = 0;
+ T.Elements[f2(4,2,T.Size[1])] = 0;
+ T.Elements[f2(4,3,T.Size[1])] = 0;
+ T.Elements[f2(4,4,T.Size[1])] = 1;
+
+ Ra.Size[0] = 4;
+ Ra.Size[1] = 4;
+ Ra.Elements[f2(1,1,Ra.Size[1])] = 1;
+ Ra.Elements[f2(1,2,Ra.Size[1])] = 0;
+ Ra.Elements[f2(1,3,Ra.Size[1])] = 0;
+ Ra.Elements[f2(1,4,Ra.Size[1])] = 0;
+ Ra.Elements[f2(2,1,Ra.Size[1])] = 0;
+ Ra.Elements[f2(2,2,Ra.Size[1])] = cosd(a);
+ Ra.Elements[f2(2,3,Ra.Size[1])] = -sind(a);
+ Ra.Elements[f2(2,4,Ra.Size[1])] = 0;
+ Ra.Elements[f2(3,1,Ra.Size[1])] = 0;
+ Ra.Elements[f2(3,2,Ra.Size[1])] = sind(a);
+ Ra.Elements[f2(3,3,Ra.Size[1])] = cosd(a);
+ Ra.Elements[f2(3,4,Ra.Size[1])] = 0;
+ Ra.Elements[f2(4,1,Ra.Size[1])] = 0;
+ Ra.Elements[f2(4,2,Ra.Size[1])] = 0;
+ Ra.Elements[f2(4,3,Ra.Size[1])] = 0;
+ Ra.Elements[f2(4,4,Ra.Size[1])] = 1;
+
+ Rb.Size[0] = 4;
+ Rb.Size[1] = 4;
+ Rb.Elements[f2(1,1,Rb.Size[1])] = cosd(b);
+ Rb.Elements[f2(1,2,Rb.Size[1])] = 0;
+ Rb.Elements[f2(1,3,Rb.Size[1])] = sind(b);
+ Rb.Elements[f2(1,4,Rb.Size[1])] = 0;
+ Rb.Elements[f2(2,1,Rb.Size[1])] = 0;
+ Rb.Elements[f2(2,2,Rb.Size[1])] = 1;
+ Rb.Elements[f2(2,3,Rb.Size[1])] = 0;
+ Rb.Elements[f2(2,4,Rb.Size[1])] = 0;
+ Rb.Elements[f2(3,1,Rb.Size[1])] = -sind(b);
+ Rb.Elements[f2(3,2,Rb.Size[1])] = 0;
+ Rb.Elements[f2(3,3,Rb.Size[1])] = cosd(b);
+ Rb.Elements[f2(3,4,Rb.Size[1])] = 0;
+ Rb.Elements[f2(4,1,Rb.Size[1])] = 0;
+ Rb.Elements[f2(4,2,Rb.Size[1])] = 0;
+ Rb.Elements[f2(4,3,Rb.Size[1])] = 0;
+ Rb.Elements[f2(4,4,Rb.Size[1])] = 1;
+
+ Rc.Size[0] = 4;
+ Rc.Size[1] = 4;
+ Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(c);
+ Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(c);
+ Rc.Elements[f2(1,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(1,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(2,1,Rc.Size[1])] = sind(c);
+ Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(c);
+ Rc.Elements[f2(2,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(2,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,1,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,2,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,3,Rc.Size[1])] = 1;
+ Rc.Elements[f2(3,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,1,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,2,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,4,Rc.Size[1])] = 1;
+
+ B->Size[0] = A->Size[0];
+ B->Size[1] = A->Size[1];
+ MatrixDot(&T, &Rc, &temp1); //相当于T * Rc * Rb * Ra
+ MatrixDot(&temp1, &Rb, &temp2);
+ MatrixDot(&temp2, &Ra, &temp1);
+ for(i = 1; i <= B->Size[0]; i++)
+ {
+ MatrixSub(A, i, 1, i, A->Size[1], &temp2);
+ MatrixTransposition(&temp2, &temp3);
+ MatrixDot(&temp1, &temp3, &temp2);
+ MatrixTransposition(&temp2, &temp3);
+ MatrixFill(B, i, 1, &temp3);
+ }
+}
+
+//********************************************
+//功能：计算矩阵行向量所表示的坐标点之间的距离
+//输入参数：A, B：要计算距离的矩阵
+//输出参数：C：包含距离值信息的列向量
+//返回值：无
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C)
+{
+ float distance = 0;
+ int i = 0, j = 0;
+ if((A->Size[0] != B->Size[0]) || (A->Size[1] != 4) || (B->Size[1] != 4))
+ {
+ return 1;
+ }
+ else
+ {
+ C->Size[0] = A->Size[0];
+ C->Size[1] = 1;
+ for(i = 1; i <= A->Size[0]; i++)
+ {
+ for(j = 1; j <= A->Size[1]; j++)
+ {
+ distance = distance + (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]) * (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]);
+ }
+ C->Elements[f2(i,1,C->Size[1])] = sqrt(distance);
+ distance = 0;
+ }
+ return 0;
+ }
+}
+
+//********************************************
+//功能：解析动感平台
+//输入参数：Platform：动感平台数据结构 包含各种输入输出
+//输出参数：无
+//返回值：无
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+void CalStewartPlatform(StewartPlatformType* Platform)
+{
+ int x, y, z, a, b, c; //动平台变换矩阵参数
+ float xx, yy, zz, r, l, AA, BB, CC, DD, EE, delta, mytheta1, mytheta2, d, e, f, l1, l2, l3, l4; //计算电机角度用的参数
+ float theta1, theta2, theta3, theta4, theta5, theta6;
+ float topRadius, topInterval, bottomRadius, bottomInterval, lengthOfSteelWheel, lengthOfCardan, lengthOfBar;
+ //平台机械尺寸定义
+ float theta0; //舵盘结构的倾角
+ MatrixType temp1, temp2;
+
+ MatrixType topPlatform; //动平台上的6个参考点
+ MatrixType Rc;
+ MatrixType bottomPlatform; //定平台上的6个参考点
+ MatrixType lengthOfBar1;
+
+ topRadius = Platform->topRadius; //平台参数初始化
+ topInterval = Platform->topInterval;
+ bottomRadius = Platform->bottomRadius;
+ bottomInterval = Platform->bottomInterval;
+ lengthOfSteelWheel = Platform->lengthOfSteelWheel;
+ lengthOfCardan = Platform->lengthOfCardan;
+ lengthOfBar = Platform->lengthOfBar;
+
+ topPlatform.Size[0] = 6;
+ topPlatform.Size[1] = 4;
+ topPlatform.Elements[f2(1,1,topPlatform.Size[1])] = -topInterval / 2;
+ topPlatform.Elements[f2(1,2,topPlatform.Size[1])] = -topRadius;
+ topPlatform.Elements[f2(1,3,topPlatform.Size[1])] = 0;
+ topPlatform.Elements[f2(1,4,topPlatform.Size[1])] = 1;
+ topPlatform.Elements[f2(2,1,topPlatform.Size[1])] = topInterval / 2;
+ topPlatform.Elements[f2(2,2,topPlatform.Size[1])] = -topRadius;
+ topPlatform.Elements[f2(2,3,topPlatform.Size[1])] = 0;
+ topPlatform.Elements[f2(2,4,topPlatform.Size[1])] = 1;
+
+ Rc.Size[0] = 4;
+ Rc.Size[1] = 4;
+ Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(120);
+ Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(120);
+ Rc.Elements[f2(1,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(1,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(2,1,Rc.Size[1])] = sind(120);
+ Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(120);
+ Rc.Elements[f2(2,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(2,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,1,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,2,Rc.Size[1])] = 0;
+ Rc.Elements[f2(3,3,Rc.Size[1])] = 1;
+ Rc.Elements[f2(3,4,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,1,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,2,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,3,Rc.Size[1])] = 0;
+ Rc.Elements[f2(4,4,Rc.Size[1])] = 1;
+
+ MatrixSub(&topPlatform, 1, 1, 1, 4, &temp1); //等效于topPlatform(3,:) = (Rc * topPlatform(1, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&topPlatform, 3, 1, &temp2);
+ MatrixSub(&topPlatform, 2, 1, 2, 4, &temp1); //等效于topPlatform(4,:) = (Rc * topPlatform(2, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&topPlatform, 4, 1, &temp2);
+ MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&topPlatform, 5, 1, &temp2);
+ MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&topPlatform, 6, 1, &temp2);
+
+ x = Platform->x;
+ y = Platform->y;
+ z = Platform->z;
+ a = Platform->a;
+ b = Platform->b;
+ c = Platform->c;
+ Inverse(x, y, z, a, b, c, &topPlatform, &temp1); //计算出动平台参考点的实际位置
+ MatrixFill(&topPlatform, 1, 1, &temp1);
+
+ bottomPlatform.Size[0] = 6;
+ bottomPlatform.Size[1] = 4;
+ bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])] = -bottomInterval / 2;
+ bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])] = -bottomRadius;
+ bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])] = 0;
+ bottomPlatform.Elements[f2(1,4,bottomPlatform.Size[1])] = 1;
+ bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])] = bottomInterval / 2;
+ bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])] = -bottomRadius;
+ bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])] = 0;
+ bottomPlatform.Elements[f2(2,4,bottomPlatform.Size[1])] = 1;
+
+ MatrixSub(&bottomPlatform, 1, 1, 1, 4, &temp1); //等效于bottomPlatform(3,:) = (Rc * bottomPlatform(1, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&bottomPlatform, 3, 1, &temp2);
+ MatrixSub(&bottomPlatform, 2, 1, 2, 4, &temp1); //等效于bottomPlatform(4,:) = (Rc * bottomPlatform(2, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&bottomPlatform, 4, 1, &temp2);
+ MatrixSub(&bottomPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(5,:) = (Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&bottomPlatform, 5, 1, &temp2);
+ MatrixSub(&bottomPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ MatrixFill(&bottomPlatform, 6, 1, &temp2);
+
+ Distance2Point(&topPlatform, &bottomPlatform, &lengthOfBar1);
+
+ Platform->BarLength[0] = lengthOfBar1.Elements[f2(1,1,lengthOfBar1.Size[1])]; //赋值计算出的杆长
+ Platform->BarLength[1] = lengthOfBar1.Elements[f2(2,1,lengthOfBar1.Size[1])];
+ Platform->BarLength[2] = lengthOfBar1.Elements[f2(3,1,lengthOfBar1.Size[1])];
+ Platform->BarLength[3] = lengthOfBar1.Elements[f2(4,1,lengthOfBar1.Size[1])];
+ Platform->BarLength[4] = lengthOfBar1.Elements[f2(5,1,lengthOfBar1.Size[1])];
+ Platform->BarLength[5] = lengthOfBar1.Elements[f2(6,1,lengthOfBar1.Size[1])];
+
+ //计算角度
+ r = sqrt(lengthOfCardan * lengthOfCardan + lengthOfSteelWheel * lengthOfSteelWheel);
+ l = lengthOfBar;
+ //点1
+ xx = topPlatform.Elements[f2(1,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
+ yy = topPlatform.Elements[f2(1,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
+ zz = topPlatform.Elements[f2(1,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = 2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta1 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta1 = -mytheta1;
+ }
+ else if(abs(l3 - l) <= 0.0001)
+ {
+ theta1 = mytheta2;
+ }
+ else
+ {
+ theta1 = -mytheta2;
+ }
+ //点2
+ xx = topPlatform.Elements[f2(2,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
+ yy = topPlatform.Elements[f2(2,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
+ zz = topPlatform.Elements[f2(2,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = -2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta2 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta2 = -mytheta1;
+ }
+ else if(abs(l4 - l) <= 0.0001)
+ {
+ theta2 = mytheta2;
+ }
+ else
+ {
+ theta2 = -mytheta2;
+ }
+ //点3
+ MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixDot(&Rc, &temp1, &temp2);
+ MatrixTransposition(&temp2, &temp1);
+ xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
+ yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
+ zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = 2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta3 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta3 = -mytheta1;
+ }
+ else if(abs(l4 - l) <= 0.0001)
+ {
+ theta3 = mytheta2;
+ }
+ else
+ {
+ theta3 = -mytheta2;
+ }
+ //点4
+ MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixDot(&Rc, &temp1, &temp2);
+ MatrixTransposition(&temp2, &temp1);
+ xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
+ yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
+ zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = -2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta4 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta4 = -mytheta1;
+ }
+ else if(abs(l4 - l) <= 0.0001)
+ {
+ theta4 = mytheta2;
+ }
+ else
+ {
+ theta4 = -mytheta2;
+ }
+ //点5
+ MatrixSub(&topPlatform, 5, 1, 5, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])];
+ yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])];
+ zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = 2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta5 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta5 = -mytheta1;
+ }
+ else if(abs(l4 - l) <= 0.0001)
+ {
+ theta5 = mytheta2;
+ }
+ else
+ {
+ theta5 = -mytheta2;
+ }
+ //点6
+ MatrixSub(&topPlatform, 6, 1, 6, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')';
+ MatrixTransposition(&temp1, &temp2);
+ MatrixDot(&Rc, &temp2, &temp1);
+ MatrixTransposition(&temp1, &temp2);
+ xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])];
+ yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])];
+ zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])];
+ AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz);
+ BB = -2 * r * xx / (2 * r * zz);
+ CC = BB * BB + 1;
+ DD = 2 * AA * BB;
+ EE = AA * AA - 1;
+ delta = DD * DD - 4 * CC * EE;
+ mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC);
+ mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1));
+ l1 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1));
+ l2 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2));
+ l3 = sqrt(d + e + f);
+ d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2));
+ e = yy * yy;
+ f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2));
+ l4 = sqrt(d + e + f);
+ mytheta1 = mytheta1 / 3.1415926 * 180;
+ mytheta2 = mytheta2 / 3.1415926 * 180;
+ if(abs(l1 - l) <= 0.0001)
+ {
+ theta6 = mytheta1;
+ }
+ else if(abs(l2 - l) <= 0.0001)
+ {
+ theta6 = -mytheta1;
+ }
+ else if(abs(l4 - l) <= 0.0001)
+ {
+ theta6 = mytheta2;
+ }
+ else
+ {
+ theta6 = -mytheta2;
+ }
+ Platform->theta[0] = theta1;
+ Platform->theta[1] = theta2;
+ Platform->theta[2] = theta3;
+ Platform->theta[3] = theta4;
+ Platform->theta[4] = theta5;
+ Platform->theta[5] = theta6;
+ theta0 = atan(lengthOfCardan / lengthOfSteelWheel) / 3.1415926 * 180;
+ Platform->theta_servo[0] = theta1 - theta0;
+ Platform->theta_servo[1] = theta2 - theta0;
+ Platform->theta_servo[2] = theta3 - theta0;
+ Platform->theta_servo[3] = theta4 - theta0;
+ Platform->theta_servo[4] = theta5 - theta0;
+ Platform->theta_servo[5] = theta6 - theta0;
+}
+
+
+
+//********************************************
+//功能：角度制的三角函数 余弦
+//输入参数：angle：角度
+//输出参数：无
+//返回值：三角函数值
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+float cosd(float angle)
+{
+ return cos(angle/180*3.1415926);
+}
+
+//********************************************
+//功能：角度制的三角函数 正弦
+//输入参数：angle：角度
+//输出参数：无
+//返回值：三角函数值
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+float sind(float angle)
+{
+ return sin(angle/180*3.1415926);
+}
+
+
+//********************************************
+//功能：在命令行打印矩阵
+//输入参数：A：要打印的矩阵
+//输出参数：无
+//返回值：无
+//调用外部函数：无
+//作者：陈欢 h-che14@mails.tsinghua.edu.cn
+//********************************************
+void PrintMatrix(MatrixType* A)
+{
+ int i, j;
+ int pos2 = 0;
+ printf("\r\nThe Matrix is:\r\n");
+ for(i = 1; i <= A->Size[0]; i++)
+ {
+ for(j = 1; j <= A->Size[1]; j++)
+ {
+ pos2 = f2(i,j,A->Size[1]);
+ printf(" %f ", A->Elements[pos2]);
+ }
+ printf("\r\n");
+ }
+}
+