StewartOlatform

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
heroistired
Date:
Wed Oct 11 07:05:25 2017 +0000
Commit message:
stewart platform

Changed in this revision

StewartPlatform.cpp Show annotated file Show diff for this revision Revisions of this file
StewartPlatform.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 2b80f11eb1d3 StewartPlatform.cpp
--- /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");
+    }
+}
+
diff -r 000000000000 -r 2b80f11eb1d3 StewartPlatform.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StewartPlatform.h	Wed Oct 11 07:05:25 2017 +0000
@@ -0,0 +1,151 @@
+#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  
diff -r 000000000000 -r 2b80f11eb1d3 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 11 07:05:25 2017 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h> 
+#include "StewartPlatform.h"
+DigitalOut myled(LED1);
+Serial pc(SERIAL_TX, SERIAL_RX);
+StewartPlatformType Platform;                                           //动感平台数据结构体 
+
+int main() 
+{
+    while(1) 
+    {
+        myled = 1; // LED is ON
+        wait(0.2); // 200 ms
+        myled = 0; // LED is OFF
+        wait(1.0); // 1 sec
+        
+        
+        Platform.topRadius = 244.95;                                            //平台参数初始化 
+        Platform.topInterval = 100;
+        Platform.bottomRadius = 332.54;
+        Platform.bottomInterval = 340;
+        Platform.lengthOfSteelWheel = 150;
+        Platform.lengthOfCardan = 150;
+        Platform.lengthOfBar = 368;
+        Platform.x = 25;                                                        //设定上平台姿态 
+        Platform.y = 31;
+        Platform.z = 278;
+        Platform.a = 15;
+        Platform.b = 12;
+        Platform.c = 22;
+        CalStewartPlatform(&Platform);                                          //解析平台数据 
+        pc.printf("Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta[0], Platform.theta[1], Platform.theta[2], Platform.theta[3], Platform.theta[4], Platform.theta[5]);
+        pc.printf("Servo Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta_servo[0], Platform.theta_servo[1], Platform.theta_servo[2], Platform.theta_servo[3], Platform.theta_servo[4], Platform.theta_servo[5]);
+        pc.printf("BarLength: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.BarLength[0], Platform.BarLength[1], Platform.BarLength[2], Platform.BarLength[3], Platform.BarLength[4], Platform.BarLength[5]);
+    }
+}
diff -r 000000000000 -r 2b80f11eb1d3 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 11 07:05:25 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/235179ab3f27
\ No newline at end of file