![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
Diff: main.cpp
- Revision:
- 11:644c13da326d
- Parent:
- 10:328cc5179ffb
- Child:
- 12:1eeea035bf87
--- a/main.cpp Thu Feb 09 17:59:27 2017 +0800 +++ b/main.cpp Fri Feb 10 22:56:11 2017 +0800 @@ -2,37 +2,173 @@ #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 +}; + + -//stanley robot test define// -enum{ - ID_AXIS1=1, - ID_AXIS2, - ID_AXIS3, - ID_AXIS4, - ID_AXIS5, - ID_AXIS6, - ID_AXIS7 -}; - -#define MAX_AXIS_NUM ID_AXIS7 +//==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 -#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 +//==morot max pulse in joint mode==// +#define DEF_JOINT_MODE_MAX_PULSE 4095 +#define DEF_JOINT_MODE_MIN_PULSE 0 -#define DEF_PI (3.1415) //==Pin define==// @@ -40,22 +176,80 @@ Serial pc(SERIAL_TX, SERIAL_RX); DigitalIn mybutton(USER_BUTTON); -int ROM_Setting() +int ROM_Setting() //new { - - int i=0; + //==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 + }; + - for(i=ID_AXIS1;i<=ID_AXIS7;i++) + //==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]= { - //Set MAX_torgue - dxl_write_word(i,MAX_TORQUE,55);// more safe + 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}; - //Set all to multi turn mode==// - dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff); - dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff); + + 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; + } + + - //Set all to multi turn mode==// - dxl_write_word(i,MULTITURN_OFFSET,-1024); + //==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); } @@ -64,36 +258,36 @@ short int max_torque=0; short int cw_angel_lim=0,ccw_angle_lim=0; short int multi_turn_offset=0; - for(i=ID_AXIS1;i<=ID_AXIS7;i++) + for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { - pc.printf("===AXIS_%d===\n",i); + pc.printf("===AXIS_%d===\n",gMapAxisNO[i]); - //MAX_torgue - max_torque = dxl_read_word(i, MAX_TORQUE); + //==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(i,CW_ANGLE_LIMIT_L); + //==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=%x\n",cw_angel_lim); + 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(i,CCW_ANGLE_LIMIT_L); + 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=%x\n",ccw_angle_lim); + 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(i,MULTITURN_OFFSET); + //==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); @@ -104,7 +298,148 @@ return 0; } -int Output_to_Dynamixel(float *Ang_rad) + + + +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==// @@ -173,53 +508,65 @@ return 0; } +*/ int main() { int rt =0; - + unsigned char i=0; + //==================// //==dxl_initialize==// //===================// - pc.printf("start\n",rt); + DBGMSG(("start\n")) + //pc.printf("start\n",rt); rt=dxl_initialize( 1, 1); - pc.printf("dxl_initialize rt=%d\n",rt); + 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[4]=90*DEF_PI/180; - Ang_rad[5]=90*DEF_PI/180; - Ang_rad[6]=30*DEF_PI/180; + //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); - rt=Output_to_Dynamixel(Ang_rad); - pc.printf("Output_to_Dynamixel rt=%d",rt); - - while(mybutton); + //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); - Ang_rad[4]=0*DEF_PI/180; - Ang_rad[5]=0*DEF_PI/180; - Ang_rad[6]=0*DEF_PI/180; - - rt=Output_to_Dynamixel(Ang_rad); - pc.printf("Output_to_Dynamixel rt=%d",rt); + //while(mybutton); + + - 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); - //pc.printf("before=%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);