![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
Diff: main.cpp
- Revision:
- 9:54710454ce60
- Parent:
- 8:37f5a7219fe6
- Child:
- 10:328cc5179ffb
--- a/main.cpp Wed Feb 08 15:50:53 2017 +0800 +++ b/main.cpp Thu Feb 09 17:18:40 2017 +0800 @@ -32,7 +32,10 @@ #define AXIS7_PULSE_LIM_L (-420) #define AXIS7_PULSE_LIM_H 420 +#define DEF_PI (3.1415) + +//==Pin define==// DigitalOut myled(LED1); Serial pc(SERIAL_TX, SERIAL_RX); @@ -101,28 +104,108 @@ return 0; } +int Output_to_Dynamixel(float *Ang_rad) +{ + //========================// + //==trnasformat to pulse==// + //========================// + short int Ang_pulse[MAX_AXIS_NUM]={0}; + short int i=0; + for(i=0;i<MAX_AXIS_NUM;i++) + { + Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse + } + + //===================================================================// + //==limit axis if not zero ,the return value is the overlimit axis==// + //===================================================================// + static int Axis_Pulse_lim_H[MAX_AXIS_NUM]= + { + AXIS1_PULSE_LIM_H, + AXIS2_PULSE_LIM_H, + AXIS3_PULSE_LIM_H, + AXIS4_PULSE_LIM_H, + AXIS5_PULSE_LIM_H, + AXIS6_PULSE_LIM_H, + AXIS7_PULSE_LIM_H}; + + static int Axis_Pulse_lim_L[MAX_AXIS_NUM]= + { + AXIS1_PULSE_LIM_L, + AXIS2_PULSE_LIM_L, + AXIS3_PULSE_LIM_L, + AXIS4_PULSE_LIM_L, + AXIS5_PULSE_LIM_L, + AXIS6_PULSE_LIM_L, + AXIS7_PULSE_LIM_L}; + + for(i=0;i<MAX_AXIS_NUM;i++) + { + if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) ) + { + pc.printf("over limit Ang_pulse[%d]=%d,Axis_Pulse_lim_H[%d]=%d,Axis_Pulse_lim_L[%d]=%d\n",i,Ang_pulse[i],i,Axis_Pulse_lim_H[i],i,Axis_Pulse_lim_L[i]); + return i+1; + } + } + + //================================// + //==output to motor by syncpage===// + //===============================// + short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30}; + + short int SyncPage1[21]= + { + ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity + ID_AXIS2,Ang_pulse[1],velocity[1], + ID_AXIS3,Ang_pulse[2],velocity[2], + ID_AXIS4,Ang_pulse[3],velocity[3], + ID_AXIS5,Ang_pulse[4],velocity[4], + ID_AXIS6,Ang_pulse[5],velocity[5], + ID_AXIS7,Ang_pulse[6],velocity[6], + }; + + for(i=0;i<MAX_AXIS_NUM;i++) + pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]); + + + + syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length); + + return 0; +} + int main() { - int rt =0; + int rt =0; + //==================// + //==dxl_initialize==// + //===================// pc.printf("start\n",rt); - rt=dxl_initialize( 1, 1); pc.printf("dxl_initialize rt=%d\n",rt); - short int pos=0; + //=========================// //==ROM parameter setting==// + //=========================// //rt=ROM_Setting(); - myled = 0; // LED is ON - /************************** - *** Check communication *** - **************************/ - //dxl_write_byte(3,ADDRESS_LED,0); - //dxl_ping(3); + //myled = 0; // LED is ON + //==Test Output_to_Dynamixel==// + float Ang_rad[MAX_AXIS_NUM]={0}; + Ang_rad[4]=90*DEF_PI/180; + Ang_rad[5]=90*DEF_PI/180; + Ang_rad[6]=30*DEF_PI/180; + + //dxl_write_word(ID_AXIS5,MAX_TORQUE,55); + //dxl_write_word(ID_AXIS5,TORQUE_LIMIT,512); + + rt=Output_to_Dynamixel(Ang_rad); + pc.printf("Output_to_Dynamixel rt=%d",rt); + while(1) { //pc.printf("before=%d\n",rt); @@ -146,6 +229,7 @@ wait_ms(200); */ //====Test read all pos=== + static short int pos=0; int i=0; for(i=ID_AXIS1;i<=ID_AXIS7;i++) { @@ -158,9 +242,11 @@ pc.printf("\n"); + + //====Test syncWrite_u16base=== - static short int pos3=-100; + /*static short int pos3=-100; pos3=(pos3==-100)? 100 :-100; short int SyncPage1[21]= @@ -174,7 +260,7 @@ ID_AXIS7,100,5, }; - wait(5); + wait(0.5); */ //short int SyncPage1[21]=//Test use //{ // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 @@ -187,7 +273,7 @@ //}; - syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; + //syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; } }