![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp
- Committer:
- stanley1228
- Date:
- 2017-02-10
- Revision:
- 11:644c13da326d
- Parent:
- 10:328cc5179ffb
- Child:
- 12:1eeea035bf87
File content as of revision 11:644c13da326d:
#include "mbed.h" #include "dynamixel.h" #define DEBUG 1 #if (DEBUG) #define DBGMSG(x) pc.printf x; #else #define DBGMSG(x) #endif #define MAX_AXIS_NUM 7 enum{ Index_AXIS1=0, Index_AXIS2, Index_AXIS3, Index_AXIS4, Index_AXIS5, Index_AXIS6, Index_AXIS7 }; enum{ ID_AXIS1=1, ID_AXIS2, ID_AXIS3, ID_AXIS4, ID_AXIS5, ID_AXIS6, ID_AXIS7 }; enum{ NO_AXIS1=1, NO_AXIS2, NO_AXIS3, NO_AXIS4, NO_AXIS5, NO_AXIS6, NO_AXIS7 }; static const unsigned char gMapAxisNO[MAX_AXIS_NUM]= { NO_AXIS1, NO_AXIS2, NO_AXIS3, NO_AXIS4, NO_AXIS5, NO_AXIS6, NO_AXIS7 }; static const unsigned char gMapAxisID[MAX_AXIS_NUM]= { ID_AXIS1, ID_AXIS2, ID_AXIS3, ID_AXIS4, ID_AXIS5, ID_AXIS6, ID_AXIS7 }; //#define AXIS1_PULSE_LIM_L (-910) //#define AXIS1_PULSE_LIM_H 2048 //#define AXIS2_PULSE_LIM_L (-205) //#define AXIS2_PULSE_LIM_H 2048 //#define AXIS3_PULSE_LIM_L (-2840) //#define AXIS3_PULSE_LIM_H 1190 //#define AXIS4_PULSE_LIM_L 0//need to test //#define AXIS4_PULSE_LIM_H 0//need to test //#define AXIS5_PULSE_LIM_L (-2048) //#define AXIS5_PULSE_LIM_H 1680 //#define AXIS6_PULSE_LIM_L (-680) //#define AXIS6_PULSE_LIM_H 1024 //#define AXIS7_PULSE_LIM_L (-420) //#define AXIS7_PULSE_LIM_H 420 enum{ DEF_UNIT_RAD=1, DEF_UNIT_DEG, DEF_UNIT_PUS }; #define DEF_PI (3.1415F) #define DEF_RATIO_PUS_TO_DEG (0.0879F) //360/4096 #define DEF_RATIO_PUS_TO_RAD (0.0015F) //2pi/4096 0.00153398078788564122971808758949 #define DEF_RATIO_DEG_TO_PUS (11.3778F) //4096/360 #define DEF_RATIO_DEG_TO_RAD (DEF_PI/180) //pi/180 0.01745329251994329576923690768489 (0.0175F) #define DEF_RATIO_RAD_TO_PUS (651.8986F) //4096/2pi 651.89864690440329530934789477382 //==robot to Motor offset==// robot pos=motor position -M2R_OFFSET #define AXIS1_R2M_OFFSET_DEG 90 #define AXIS2_R2M_OFFSET_DEG 90 #define AXIS3_R2M_OFFSET_DEG 225 #define AXIS4_R2M_OFFSET_DEG 90 #define AXIS5_R2M_OFFSET_DEG 180 #define AXIS6_R2M_OFFSET_DEG 90 #define AXIS7_R2M_OFFSET_DEG 90 static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]= { (unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), (unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS) }; //==robot angle limit==// #define AXIS1_ROBOT_LIM_DEG_L (-80) #define AXIS1_ROBOT_LIM_DEG_H 180 #define AXIS2_ROBOT_LIM_DEG_L (-18) #define AXIS2_ROBOT_LIM_DEG_H 180 #define AXIS3_ROBOT_LIM_DEG_L (-225) #define AXIS3_ROBOT_LIM_DEG_H 105 #define AXIS4_ROBOT_LIM_DEG_L 0 //need to test #define AXIS4_ROBOT_LIM_DEG_H 114 //need to test #define AXIS5_ROBOT_LIM_DEG_L (-180) #define AXIS5_ROBOT_LIM_DEG_H 148 #define AXIS6_ROBOT_LIM_DEG_L (-60) #define AXIS6_ROBOT_LIM_DEG_H 90 #define AXIS7_ROBOT_LIM_DEG_L (-30) #define AXIS7_ROBOT_LIM_DEG_H 30 static const float grobot_lim_rad_L[MAX_AXIS_NUM]= { AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD }; static const float grobot_lim_rad_H[MAX_AXIS_NUM]= { AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD }; //==robot angle limit==// #define AXIS1_MAX_TORQUE 55 #define AXIS2_MAX_TORQUE 55 #define AXIS3_MAX_TORQUE 55 #define AXIS4_MAX_TORQUE 55 #define AXIS5_MAX_TORQUE 55 #define AXIS6_MAX_TORQUE 55 #define AXIS7_MAX_TORQUE 55 //==morot max pulse in joint mode==// #define DEF_JOINT_MODE_MAX_PULSE 4095 #define DEF_JOINT_MODE_MIN_PULSE 0 //==Pin define==// DigitalOut myled(LED1); Serial pc(SERIAL_TX, SERIAL_RX); DigitalIn mybutton(USER_BUTTON); 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) //new { 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(float *Ang_rad) //old { //========================// //==trnasformat to pulse==// //========================// short int Ang_pulse[MAX_AXIS_NUM]={0}; short int i=0; for(i=0;i<MAX_AXIS_NUM;i++) { Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse } //===================================================================// //==limit axis if not zero ,the return value is the overlimit axis==// //===================================================================// static int Axis_Pulse_lim_H[MAX_AXIS_NUM]= { AXIS1_PULSE_LIM_H, AXIS2_PULSE_LIM_H, AXIS3_PULSE_LIM_H, AXIS4_PULSE_LIM_H, AXIS5_PULSE_LIM_H, AXIS6_PULSE_LIM_H, AXIS7_PULSE_LIM_H}; static int Axis_Pulse_lim_L[MAX_AXIS_NUM]= { AXIS1_PULSE_LIM_L, AXIS2_PULSE_LIM_L, AXIS3_PULSE_LIM_L, AXIS4_PULSE_LIM_L, AXIS5_PULSE_LIM_L, AXIS6_PULSE_LIM_L, AXIS7_PULSE_LIM_L}; for(i=0;i<MAX_AXIS_NUM;i++) { if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) ) { pc.printf("over limit Ang_pulse[%d]=%d,Axis_Pulse_lim_H[%d]=%d,Axis_Pulse_lim_L[%d]=%d\n",i,Ang_pulse[i],i,Axis_Pulse_lim_H[i],i,Axis_Pulse_lim_L[i]); return i+1; } } //================================// //==output to motor by syncpage===// //===============================// short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30}; short int SyncPage1[21]= { ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity ID_AXIS2,Ang_pulse[1],velocity[1], ID_AXIS3,Ang_pulse[2],velocity[2], ID_AXIS4,Ang_pulse[3],velocity[3], ID_AXIS5,Ang_pulse[4],velocity[4], ID_AXIS6,Ang_pulse[5],velocity[5], ID_AXIS7,Ang_pulse[6],velocity[6], }; for(i=0;i<MAX_AXIS_NUM;i++) pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]); syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length); return 0; } */ int main() { int rt =0; unsigned char i=0; //==================// //==dxl_initialize==// //===================// DBGMSG(("start\n")) //pc.printf("start\n",rt); rt=dxl_initialize( 1, 1); DBGMSG(("dxl_initialize rt=%d\n",rt)) //pc.printf("dxl_initialize rt=%d\n",rt); //=========================// //==ROM parameter setting==// //=========================// //rt=ROM_Setting(); //while(1); //myled = 0; // LED is ON float Ang_rad[MAX_AXIS_NUM]={0}; unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10}; while(1) { //==Test Output_to_Dynamixel==// //Ang_rad[Index_AXIS5]=160*DEF_RATIO_DEG_TO_RAD; //Ang_rad[Index_AXIS6]=100*DEF_RATIO_DEG_TO_RAD; //Ang_rad[Index_AXIS7]=90*DEF_RATIO_DEG_TO_RAD; //rt=Output_to_Dynamixel(Ang_rad,velocity); //pc.printf("Output_to_Dynamixel rt=%d\n",rt); //while(mybutton); // //Ang_rad[Index_AXIS5]=-180*DEF_RATIO_DEG_TO_RAD; //Ang_rad[Index_AXIS6]=-70*DEF_RATIO_DEG_TO_RAD; //Ang_rad[Index_AXIS7]=-90*DEF_RATIO_DEG_TO_RAD; //rt=Output_to_Dynamixel(Ang_rad,velocity); //pc.printf("Output_to_Dynamixel rt=%d\n",rt); //while(mybutton); //==Read robot pos by pos_deg==// float pos_deg[MAX_AXIS_NUM]={0}; rt=Read_pos(pos_deg,DEF_UNIT_RAD); pc.printf("Read_pos rt==%d\n",rt); for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { pc.printf("X%d=%.2f,",gMapAxisNO[i],pos_deg[i]); } //===Test move==// //dxl_write_word(3,GOAL_POSITION,400); //setPosition(3,2048,10); //pc.printf("after=%d\n",rt); //myled = 1; // LED is ON //wait(4); //dxl_write_word(3,GOAL_POSITION,-400); //setPosition(3,-2048,10); //myled = 0; // LED is ON //wait(4); //===Test read pos====// /*pos=dxl_read_word(3,PRESENT_POS); pc.printf("pos=%d\n",pos); wait_ms(200); */ //====Test read all pos=== //static short int pos=0; //int i=0; //for(i=ID_AXIS1;i<=ID_AXIS7;i++) //{ // pos = dxl_read_word(i, PRESENT_POS); // if(dxl_get_result()!=COMM_RXSUCCESS) // pos=-1; // pc.printf("X%d=%d,",i,pos); //} //pc.printf("\n"); //====Test syncWrite_u16base=== /*static short int pos3=-100; pos3=(pos3==-100)? 100 :-100; short int SyncPage1[21]= { ID_AXIS1,100,5, ID_AXIS2,100,5, ID_AXIS3,100,5, ID_AXIS4,100,5, ID_AXIS5,100,5, ID_AXIS6,100,5, ID_AXIS7,100,5, }; wait(0.5); */ //short int SyncPage1[21]=//Test use //{ // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 // 0x01,0x020,0x360, // 0x02,0x030,0x170, // 0x03,0x220,0x380, // 0x05,0x123,0x121, // 0x06,0x234,0x143, // 0x07,0x145,0x167 //}; //syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; } }