RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
Diff: RobotControl_7Axis.cpp
- Revision:
- 1:584f36ed8717
- Parent:
- 0:c62673155cbf
- Child:
- 2:71ed7da9ea36
--- a/RobotControl_7Axis.cpp Fri Feb 10 15:26:47 2017 +0000 +++ b/RobotControl_7Axis.cpp Sat Feb 11 00:22:11 2017 +0800 @@ -0,0 +1,288 @@ +#include "mbed.h" +#include "dynamixel.h" +#include "RobotControl_7Axis.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; +} +