hsu han-lin
RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
RobotControl_7Axis.cpp
- Committer:
- stanley1228
- Date:
- 2017-02-11
- Revision:
- 2:71ed7da9ea36
- Parent:
- 1:584f36ed8717
- Child:
- 3:e380c7db9133
File content as of revision 2:71ed7da9ea36:
#include "mbed.h"
#include "dynamixel.h"
#include "RobotControl_7Axis.h"
#include <vector>
#include "Matrix.h"
#include "MatrixMath.h"
#include <math.h>
extern Serial pc;
#if (DEBUG)
#define DBGMSG(x) pc.printf x;
#else
#define DBGMSG(x)
#endif
unsigned char getMapAxisNO(unsigned char index)
{
return gMapAxisNO[index];
}
unsigned char getMapAxisID(unsigned char index)
{
return gMapAxisID[index];
}
int ROM_Setting() //new
{
//==calculate Max torque to set in rom
const short int Max_torque[MAX_AXIS_NUM]=
{
AXIS1_MAX_TORQUE,
AXIS2_MAX_TORQUE,
AXIS3_MAX_TORQUE,
AXIS4_MAX_TORQUE,
AXIS5_MAX_TORQUE,
AXIS6_MAX_TORQUE,
AXIS7_MAX_TORQUE
};
//==Calculate angle limit==//
const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]=
{
AXIS1_R2M_OFFSET_DEG,
AXIS2_R2M_OFFSET_DEG,
AXIS3_R2M_OFFSET_DEG,
AXIS4_R2M_OFFSET_DEG,
AXIS5_R2M_OFFSET_DEG,
AXIS6_R2M_OFFSET_DEG,
AXIS7_R2M_OFFSET_DEG
};
const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]=
{
AXIS1_ROBOT_LIM_DEG_L,
AXIS2_ROBOT_LIM_DEG_L,
AXIS3_ROBOT_LIM_DEG_L,
AXIS4_ROBOT_LIM_DEG_L,
AXIS5_ROBOT_LIM_DEG_L,
AXIS6_ROBOT_LIM_DEG_L,
AXIS7_ROBOT_LIM_DEG_L
};
const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]=
{
AXIS1_ROBOT_LIM_DEG_H,
AXIS2_ROBOT_LIM_DEG_H,
AXIS3_ROBOT_LIM_DEG_H,
AXIS4_ROBOT_LIM_DEG_H,
AXIS5_ROBOT_LIM_DEG_H,
AXIS6_ROBOT_LIM_DEG_H,
AXIS7_ROBOT_LIM_DEG_H
};
unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0};
unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0};
int i=0;
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
}
//==writing to ROM==//
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
//==Set MAX_torgue==//
dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);// more safe
//==Set angel limit==//
dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]);
dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]);
//==Set MULTITURN_OFFSET to 0==//
dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0);
}
//==read and check ==//
int txrx_result=0;
short int max_torque=0;
short int cw_angel_lim=0,ccw_angle_lim=0;
short int multi_turn_offset=0;
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);
//==MAX_torgue==//
max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE);
txrx_result = dxl_get_result();
if(txrx_result!=COMM_RXSUCCESS)
pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result);
else
pc.printf("MAX_TORQUE=%d\n",max_torque);
//==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==//
cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L);
txrx_result = dxl_get_result();
if(txrx_result!=COMM_RXSUCCESS)
pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result);
else
pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG);
ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L);
txrx_result = dxl_get_result();
if(txrx_result!=COMM_RXSUCCESS)
pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result);
else
pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG);
//==multi turn offset==//
multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET);
txrx_result = dxl_get_result();
if(txrx_result!=COMM_RXSUCCESS)
pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result);
else
pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
}
return 0;
}
int Read_pos(float *pos,unsigned char unit)
{
int i=0;
short int pulse=0;
int rt=0;
if(unit==DEF_UNIT_RAD)
{
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
if(dxl_get_result()!=COMM_RXSUCCESS)
{
rt=-gMapAxisNO[i];
pos[i]=0xffff;
}
else
{
pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset
pos[i]=pulse*DEF_RATIO_PUS_TO_RAD;
}
}
}
else if(unit==DEF_UNIT_DEG)
{
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
if(dxl_get_result()!=COMM_RXSUCCESS)
{
rt=-gMapAxisNO[i];
pos[i]=0xffff;
}
else
{
pulse-=gr2m_offset_pulse[i];
pos[i]=pulse*DEF_RATIO_PUS_TO_DEG;
}
}
}
else if(unit==DEF_UNIT_PUS)
{
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
if(dxl_get_result()!=COMM_RXSUCCESS)
{
rt=-gMapAxisNO[i];
pos[i]=0xffff;
}
else
{
pulse-=gr2m_offset_pulse[i];
pos[i]=pulse;
}
}
}
else
{
//non offset value
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS);
if(dxl_get_result()!=COMM_RXSUCCESS)
{
rt=-gMapAxisNO[i];
pos[i]=0xffff;
}
}
}
return rt;
}
int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity)
{
unsigned char i=0;
//===================================================================//
//==limit axis if not zero ,the return value is the overlimit axis==//
//===================================================================//
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) )
{
DBGMSG(("AXIS%d over limit Ang_rad=%f,grobot_lim_rad_L=%f,grobot_lim_rad_H=%f\n",gMapAxisNO[i],Ang_rad[i],grobot_lim_rad_L[i],grobot_lim_rad_H[i]))
return -gMapAxisNO[i];
}
}
//===================================================//
//==trnasformat to pulse and offset to motor domain==//
//====================================================//
short int Ang_pulse[MAX_AXIS_NUM]={0};
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS);
Ang_pulse[i]+=gr2m_offset_pulse[i];
if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )// 0~4095
{
DBGMSG(("AXIS%d over range of mortor Ang_pulse=%d,JOINT_MODE_MIN_PULSE=%d,JOINT_MODE_MAX_PULSE=%d\n",gMapAxisNO[i],Ang_pulse[i],DEF_JOINT_MODE_MIN_PULSE,DEF_JOINT_MODE_MAX_PULSE))
return -gMapAxisNO[i];
}
}
//================================//
//==output to motor by syncpage===//
//===============================//
unsigned short int SyncPage1[21]=
{
ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2],
ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3],
ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4],
ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5],
ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6],
ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7],
};
#if (DEBUG)
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]);
#endif
syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
return 0;
}
//歐拉 Z1X2Y3 Intrinsic Rotaions相對於當前坐標系的的旋轉
Matrix R_z1x2y3(float alpha,float beta,float gamma)
{
Matrix ans(3,3);
ans << cos(alpha)*cos(gamma)-sin(alpha)*sin(beta)*sin(gamma) << -cos(beta)*sin(alpha) << cos(alpha)*sin(gamma)+cos(gamma)*sin(alpha)*sin(beta)
<< cos(gamma)*sin(alpha)+cos(alpha)*sin(beta)*sin(gamma) << cos(alpha)*cos(beta) << sin(alpha)*sin(gamma)-cos(alpha)*cos(gamma)*sin(beta)
<< -cos(beta)*sin(gamma) << sin(beta) << cos(beta)*cos(gamma);
return ans;
}
float norm(const Matrix& v)
{
int i=0;
float ans=0;
for(i=1;i<=v.getRows();i++)
{
ans+=pow(v(i,1),2);
}
ans=sqrt(ans);
return ans;
}
Matrix Rogridues(float theta,const Matrix& V_A)
{
Matrix R_a(4,4);
float cs = cos( theta );
float sn = sin( theta );
R_a << cs + pow(V_A.getNumber(1,1),2)*(1-cs) << V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)-V_A.getNumber(3,1)*sn << V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(2,1)*sn << 0
<< V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)+V_A.getNumber(3,1)*sn << cos(theta)+pow(V_A.getNumber(2,1),2)*(1-cs) << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(1,1)*sn << 0
<< V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(2,1)*sn << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(1,1)*sn << cs+pow(V_A.getNumber(3,1),2)*(1-cs) << 0
<< 0 << 0 << 0 << 1;
return R_a;
}
//enum{
// AXIS1=0,
// AXIS2,
// AXIS3,
// AXIS4,
// AXIS5,
// AXIS6,
// AXIS7
//};
int IK_7DOF(const float l1,const float l2,const float l3,const float x_base,const float y_base,const float z_base,const float x_end,const float y_end,const float z_end,const float alpha,const float beta,const float gamma,const float Rednt_alpha,float* theta)
{
int i=0;
//Out put initial to zero
for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
theta[i]=0;
}
Matrix R(3,3);
R=R_z1x2y3(alpha,beta,gamma);
Matrix V_H_hat_x(3,1);
V_H_hat_x=Matrix::ExportCol(R,1);//取出歐拉角轉換的旋轉矩陣，取出第1行為X軸旋轉後向量
V_H_hat_x*=1/norm(V_H_hat_x);
Matrix V_H_hat_y(3,1);
V_H_hat_y=Matrix::ExportCol(R,2);//取出歐拉角轉換的旋轉矩陣，取出第2行為Y軸旋轉後向量
V_H_hat_y*=1/norm(V_H_hat_y);
Matrix V_r_end(3,1);
V_r_end <<x_end-x_base
<<y_end-y_base
<<z_end-z_base;
Matrix V_r_h(3,1);
V_r_h=V_H_hat_x*L3;
Matrix V_r_wst(3,1);
V_r_wst=V_r_end-V_r_h;
//theat 4
theta[Index_AXIS4]=-(float)(DEF_PI-acos((pow(l1,2)+pow(l2,2)-pow(norm(V_r_wst),2))/(2*l1*l2)));
Matrix V_r_m(3,1);
V_r_m=(pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*pow(norm(V_r_wst),2))*V_r_wst;
//Redundant circle 半徑R
float Rednt_cir_R = pow(l1,2)- pow( (pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*norm(V_r_wst)) , 2);
Rednt_cir_R=sqrt(Rednt_cir_R);
//圓中心點到Elbow向量 V_r_u
Matrix V_shz(3,1);
V_shz <<0
<<0
<<1;
Matrix V_alpha_hat(3,1);//V_alpha_hat=cross(V_r_wst,V_shz)/norm(cross(V_r_wst,V_shz));
Matrix temp_cross(3,1);
temp_cross=MatrixMath::cross(V_r_wst,V_shz); //錯誤
V_alpha_hat=temp_cross*(1/norm(temp_cross));
Matrix V_beta_hat(3,1);//V_beta_hat=cross(V_r_wst,V_alpha_hat)/norm(cross(V_r_wst,V_alpha_hat));
temp_cross=MatrixMath::cross(V_r_wst,V_alpha_hat);
V_beta_hat=temp_cross*(1/norm(temp_cross));
Matrix temp(4,4);//temp=Rogridues(Rednt_alpha,V_r_wst/norm(V_r_wst)) *[Rednt_cir_R*V_beta_hat;1]; %Rednt_alpha的方向和論文上的方向性相反
Matrix V_r_wst_unit =V_r_wst*(1/norm(V_r_wst));
Matrix V_temp3x1(3,1);//需要寫一個可以補1的函試
Matrix V_temp4x1(4,1);
V_temp3x1=V_beta_hat*Rednt_cir_R;
V_temp4x1.Vec_ext_1_row(V_temp3x1,1); //3x1 extend to 4x1
temp=Rogridues(Rednt_alpha,V_r_wst_unit)*V_temp4x1;
Matrix V_R_u(3,1);
V_R_u.Vec_export_3_row(temp);
Matrix V_r_u(3,1);
V_r_u=V_r_m+V_R_u;
theta[Index_AXIS1]=atan2(-V_r_u(1,1),-V_r_u(3,1));//theta(1)=atan2(-V_r_u(1),-V_r_u(3));
if (theta[Index_AXIS1] !=0)
theta[Index_AXIS2]=atan2(V_r_u(2,1),-V_r_u(1,1)/sin(theta[Index_AXIS1]));
else
theta[Index_AXIS2]=atan2(-V_r_u(2,1),V_r_u(3,1));
//theat 3
//theta(3)=atan2( sin(theta(2))*sin(theta(1))*V_r_wst(1)+cos(theta(2))*V_r_wst(2)+sin(theta(2))*cos(theta(1))*V_r_wst(3),cos(theta(1))*V_r_wst(1)-sin(theta(1))*V_r_wst(3));
theta[Index_AXIS3]=atan2( sin(theta[Index_AXIS2])*sin(theta[Index_AXIS1])*V_r_wst(1,1)+cos(theta[Index_AXIS2])*V_r_wst(2,1)+sin(theta[Index_AXIS2])*cos(theta[Index_AXIS1])*V_r_wst(3,1),cos(theta[Index_AXIS1])*V_r_wst(1,1)-sin(theta[Index_AXIS1])*V_r_wst(3,1));
//theat 5
Matrix V_r_f(3,1);
V_r_f=V_r_wst-V_r_u;
Matrix V_Axis6(3,1);
V_Axis6=MatrixMath::cross(V_H_hat_y,-V_r_f)*(1/norm(MatrixMath::cross(V_H_hat_y,-V_r_f)));//V_Axis6=cross(V_H_hat_y,-V_r_f)/norm(cross(V_H_hat_y,-V_r_f));
Matrix V_r_wst_u(3,1);
V_r_wst_u=V_r_wst+V_Axis6;
Matrix A1_4(4,4);
A1_4=MatrixMath::RotY(theta[Index_AXIS1])*MatrixMath::RotX(theta[Index_AXIS2])*MatrixMath::RotZ(theta[Index_AXIS3])*MatrixMath::Tz(-l1)*MatrixMath::RotY(theta[Index_AXIS4]);//A1_4=Ry(theta(1))*Rx(theta(2))*Rz(theta(3))*Tz(-L1)*Ry(theta(4));
Matrix V_temp_f(4,1);
Matrix V_r_wst_u_4x1(4,1);
V_r_wst_u_4x1.Vec_ext_1_row(V_r_wst_u,1);
V_temp_f=MatrixMath::Inv(A1_4)*V_r_wst_u_4x1;//V_temp_f=inv(A1_4)*[V_r_wst_u;1]; %(3.31) 這個是補一列1上去的意思,need fix
theta[Index_AXIS5]=atan2(V_temp_f(2,1),V_temp_f(1,1));//theta(5)=atan2(V_temp_f(2),V_temp_f(1));
//theat 6
Matrix V_r_wst_r(3,1);
V_r_wst_r=V_r_wst+V_H_hat_y;
Matrix A1_5(4,4);
A1_5=A1_4*MatrixMath::RotZ(theta[Index_AXIS5])*MatrixMath::Tz(-l2);//A1_5=A1_4*Rz(theta(5))*Tz(-L2);
Matrix V_temp_g(4,4);
Matrix V_r_wst_r_4x1(4,1);
V_r_wst_r_4x1.Vec_ext_1_row(V_r_wst_r,1);
V_temp_g=MatrixMath::Inv(A1_5)*V_r_wst_r_4x1; //V_temp_g=inv(A1_5)*[V_r_wst_r;1]; %(3.38) 這個是補一列1上去的意思,need fix
theta[Index_AXIS6]=atan2(V_temp_g(3,1),V_temp_g(2,1));
//theat 7
Matrix V_r_wst_f(3,1);
V_r_wst_f=V_r_wst+V_H_hat_x;
Matrix A1_6(4,4);
A1_6=A1_5*MatrixMath::RotX(theta[Index_AXIS6]);
Matrix V_temp_h(3,1);
Matrix V_r_wst_f_4x1(4,1);
V_r_wst_f_4x1.Vec_ext_1_row(V_r_wst_f,1);
V_temp_h=MatrixMath::Inv(A1_6)*V_r_wst_f_4x1; //V_temp_h=inv(A1_6)*[V_r_wst_f;1];
theta[Index_AXIS7]=atan2(-V_temp_h(1,1),-V_temp_h(3,1));//theta(7)=atan2(-V_temp_h(1),-V_temp_h(3));
return 0;
}
//int keepitforawhile_deleteitafter()
//{
//
// //DigitalOut myled(LED1);
// //Timer t,t2;
//
// //t.start();
// float theta[7]={0};
// int rt = IK_7DOF_stanley(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-M_PI*0.5,theta);
////---
// Matrix myMatrix(3,3);
// Matrix anotherMatrix;
//
// // Fill Matrix with data.
// myMatrix << 1 << 2 << 3
// << 4 << 5 << 6
// << 7 << 8 << 9;
//
// printf( "\nmyMatrix:\n\n");
// myMatrix.print();
// printf( "\n" );
//
// /*Matrix myMatrix2(3,3);
// myMatrix2 << 1 << 0 << 0
// << 0 << 1 << 0
// << 0 << 0 << 1;*/
// float mydet = MatrixMath::det( myMatrix );
//
// printf( "det mymatrix=%f:\n\n",mydet);
//
//
//
// // Matrix operations //
//
// // Add 5 to negative Matrix
// anotherMatrix = - myMatrix + 5;
//
// printf( "Result Matrix: anotherMatrix = - myMatrix + 5\n\n" );
// anotherMatrix.print();
// printf( "\n" );
//
// // Matrix Multiplication *
// anotherMatrix *= myMatrix;
//
// printf( "\nanotherMatrix = anotherMatrix * myMatrix\n\n" );
// anotherMatrix.print();
// printf( "\n" );
//
// // Scalar Matrix Multiplication anotherMatrix *= 0.5
// anotherMatrix *= 0.5;
//
// printf( "\nResult Matrix *= 0.5:\n\n" );
// anotherMatrix.print();
// printf( " _______________________________ \n" );
//
//
// printf("\n\n *** MEMBER OPERATIONS *** \n\n");
//
// //Copy myMatrix
// Matrix temp( myMatrix );
//
// // Resize Matrix
// temp.Resize(4,4);
// printf("\nAdded one Column, one Row to the limits of myMatrix saved in temp Matrix\n");
// temp.print();
//
// //Delete those new elements, we don't need them anyway.
// Matrix::DeleteRow( temp, 4 );
// Matrix::DeleteCol( temp, 4 );
//
// printf("\nBack to normal\n");
// temp.print();
//
//
// // Make room at the begining of Matrix
// Matrix::AddRow( temp, 1 );
// Matrix::AddCol( temp, 1 );
//
// printf("\nAdded Just one Row and column to the beginning\n");
// temp.print();
//
// // Take the second Row as a new Matrix
// anotherMatrix = Matrix::ExportRow( temp, 2 );
// printf("\nExport Second Row \n");
// anotherMatrix.print();
//
// // The second Column as a new Matrix, then transpose it to make it a Row
// anotherMatrix = Matrix::ExportCol( temp, 2 );
// anotherMatrix = MatrixMath::Transpose( anotherMatrix );
// printf("\nAnd Export Second Column and Transpose it \n");
// anotherMatrix.print();
//
// // This will Check to see if your are reduce to a single Row or Column
// temp = Matrix::ToPackedVector( myMatrix );
// printf("\nInitial Matrix turned into a single Row\n");
// temp.print();
//
// // Matrix Math //
// printf("\n\n *** Matrix Inverse and Determinant ***\n");
//
// //Matrix BigMat( 8, 8 );
// //
// //BigMat << 1 << 0.3 << 1.0 << 1 << 3 << 0.5 << 7.12 << 899
// // << 2 << 3.2 << 4.1 << 0 << 4 << 0.8 << 9.26 << 321
// // << 5 << 6.0 << 1 << 1 << 2 << 7.4 << 3.87 << 562
// // << 1 << 0.0 << 2.7 << 1 << 1 << 4.6 << 1.21 << 478
// // << 2 << 3.7 << 48 << 2 << 0 << 77 << 0.19 << 147
// // << 1 << 1.0 << 3.8 << 7 << 1 << 9.9 << 7.25 << 365
// // << 9 << 0.9 << 2.7 << 8 << 0 << 13 << 4.16 << 145
// // << 7 << 23 << 28 << 9 << 9 << 1.7 << 9.16 << 156;
//
// //printf( "\nBigMat:\n");
// //BigMat.print();
// //printf( "\n" );
//
// //t2.start();
// //float determ = MatrixMath::det( BigMat );//有問題
//
// //Matrix myInv = MatrixMath::Inv( BigMat );
// ////t2.stop();
//
// //printf( "\nBigMat's Determinant is: %f \n", determ);
// //printf( "\n" );
// //
// //printf( "\nBigMat's Inverse is:\n");
// //myInv.print();
// //printf( "\n" );
//
// //*** Homogenous Transformations **//
//
// printf( "\n\n *** TRANSFORMATIONS *** \n\n");
//
// Matrix rot;
//
// printf( " RotX 0.5 rad \n" );
// rot = MatrixMath::RotX(0.5);
// rot.print();
// printf( " _______________________________ \n\n" );
//
// printf( " RotY 0.5 rad \n" );
// rot = MatrixMath::RotY(0.5);
// rot.print();
// printf( " _______________________________ \n\n" );
//
// printf( " RotZ 0.5 rad \n" );
// rot = MatrixMath::RotZ(0.5);
// rot.print();
// printf( " _______________________________ \n\n" );
//
// //printf( " Transl 5x 3y 4z\n" ); //有問題
// //rot = MatrixMath::Transl( 5, 3, 4 );
// //rot.print();
// //printf( " _______________________________ \n\n" );
//
// //---
//
// //t.stop();
//
// //float bigtime = t2.read();
// //float average = 12.149647 - bigtime;
//
// //printf( "\n\nThe time for all those operations in mbed was : %f seconds\n", t.read() );
// //printf( "\nOnly operations witout any print takes: 12.149647 seconds\n" );
// //printf( "\nDue to the 8x8 matrix alone takes: %f \n",bigtime );
// //printf( "\nSo normal 4x4 matrix ops: %f\n", average );
//
// //while(1) {
// // myled = 1;
// // wait(0.2);
// // myled = 0;
// // wait(0.2);
// //}
//}