![](/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-16
- Revision:
- 14:235c64640200
- Parent:
- 13:f0f52287352a
- Child:
- 17:2b67b218da4d
File content as of revision 14:235c64640200:
#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; //================== //== 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}; // // 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")) //================== //==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]=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); // 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); //==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");