![](/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-11
- Revision:
- 13:f0f52287352a
- Parent:
- 12:1eeea035bf87
- Child:
- 14:235c64640200
File content as of revision 13:f0f52287352a:
#include "mbed.h" #include "dynamixel.h" #include "RobotControl_7Axis.h" #define DEBUG 1 #if (DEBUG) #define DBGMSG(x) pc.printf x; #else #define DBGMSG(x) #endif //==Pin define==// DigitalOut myled(LED1); DigitalOut Test(D4,PullDown); Serial pc(SERIAL_TX, SERIAL_RX); DigitalIn mybutton(USER_BUTTON); int main() { int rt =0; unsigned char i=0; //================== //== Test IK it need 120ms to calculate //=================== 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")) while(1); //================== //==dxl_initialize //=================== rt=dxl_initialize( 1, 1); DBGMSG(("dxl_initialize rt=%d\n",rt)) //=========================// //==ROM parameter setting==// //=========================// //rt=ROM_Setting(); //while(1); 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_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); //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");