RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
RobotControl_7Axis.cpp
- Committer:
- stanley1228
- Date:
- 2017-03-31
- Revision:
- 3:e380c7db9133
- Parent:
- 2:71ed7da9ea36
File content as of revision 3:e380c7db9133:
#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; } int Output_to_Dynamixel_pulse(const unsigned short int *Ang_pulse,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_pulse[i] > grobot_lim_pus_H[i]) || (Ang_pulse[i] < grobot_lim_pus_L[i]) ) { DBGMSG(("AXIS%d over limit Ang_pus=%d,grobot_lim_pus_L=%d,grobot_lim_pus_H=%d\n",gMapAxisNO[i],Ang_pulse[i],grobot_lim_pus_L[i],grobot_lim_pus_H[i])) return -gMapAxisNO[i]; } } //====================================================// //==trnasformat to pulse and offset to motor domain===// //====================================================// unsigned short int Ang_pulse_with_offset[MAX_AXIS_NUM]={0}; for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { Ang_pulse_with_offset[i]=Ang_pulse[i]+gr2m_offset_pulse[i]; if( Ang_pulse_with_offset[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_with_offset[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,Ang_pulse_with_offset[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity ID_AXIS2,Ang_pulse_with_offset[Index_AXIS2],velocity[Index_AXIS2], ID_AXIS3,Ang_pulse_with_offset[Index_AXIS3],velocity[Index_AXIS3], ID_AXIS4,Ang_pulse_with_offset[Index_AXIS4],velocity[Index_AXIS4], ID_AXIS5,Ang_pulse_with_offset[Index_AXIS5],velocity[Index_AXIS5], ID_AXIS6,Ang_pulse_with_offset[Index_AXIS6],velocity[Index_AXIS6], ID_AXIS7,Ang_pulse_with_offset[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); // //} //}