![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
Diff: main.cpp
- Revision:
- 14:235c64640200
- Parent:
- 13:f0f52287352a
- Child:
- 17:2b67b218da4d
--- a/main.cpp Sat Feb 11 13:07:35 2017 +0000 +++ b/main.cpp Thu Feb 16 11:38:26 2017 +0000 @@ -22,29 +22,44 @@ Serial pc(SERIAL_TX, SERIAL_RX); DigitalIn mybutton(USER_BUTTON); + int main() { int rt =0; unsigned char i=0; - + + //================== + //== Receive from PC by uart + //=================== + //char buffer[128]; +// +// pc.gets(buffer, 10); +// for(i=0;i<10;i++) +// { +// DBGMSG(("buffer[%d]=%d,",i,buffer[i])) +// } + + + +// while(1); //================== //== Test IK it need 120ms to calculate //=================== - DBGMSG(("start")) - float theta[7]={0}; + //DBGMSG(("start")) +// float theta[7]={0}; +// +// myled=1; +// rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta); +// myled=0; +// +// +// for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) +// { +// DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i])) +// } +// DBGMSG(("\n")) - myled=1; - rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta); - myled=0; - - - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i])) - } - DBGMSG(("\n")) - while(1); //================== //==dxl_initialize @@ -64,40 +79,41 @@ 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); + Ang_rad[Index_AXIS5]=140*DEF_RATIO_DEG_TO_RAD; + Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD; + Ang_rad[Index_AXIS7]=10*DEF_RATIO_DEG_TO_RAD; + rt=Output_to_Dynamixel(Ang_rad,velocity); + pc.printf("Output_to_Dynamixel rt=%d\n",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[Index_AXIS5]=0*DEF_RATIO_DEG_TO_RAD; + Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD; + Ang_rad[Index_AXIS7]=-10*DEF_RATIO_DEG_TO_RAD; + rt=Output_to_Dynamixel(Ang_rad,velocity); + pc.printf("Output_to_Dynamixel rt=%d\n",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_DEG); - DBGMSG(("Read_pos rt==%d\n",rt)); - - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i])); - } + //float pos_deg[MAX_AXIS_NUM]={0}; +// rt=Read_pos(pos_deg,DEF_UNIT_DEG); +// DBGMSG(("Read_pos rt==%d\n",rt)); +// +// for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) +// { +// DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i])); +// } } } + //===Test move==// //dxl_write_word(3,GOAL_POSITION,400); //setPosition(3,2048,10);