RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
Diff: RobotControl_7Axis.cpp
- Revision:
- 3:e380c7db9133
- Parent:
- 2:71ed7da9ea36
diff -r 71ed7da9ea36 -r e380c7db9133 RobotControl_7Axis.cpp --- a/RobotControl_7Axis.cpp Sat Feb 11 13:06:49 2017 +0000 +++ b/RobotControl_7Axis.cpp Fri Mar 31 16:31:30 2017 +0800 @@ -292,7 +292,61 @@ 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相對於當前坐標系的的旋轉