hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@12:1eeea035bf87, 2017-02-11 (annotated)

Committer:
stanley1228
Date:
Sat Feb 11 00:23:03 2017 +0800
Revision:
12:1eeea035bf87
Parent:
11:644c13da326d
Child:
13:f0f52287352a
1.no

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 0:1780ffc33286 1 #include "mbed.h"
stanley1228 0:1780ffc33286 2 #include "dynamixel.h"
stanley1228 12:1eeea035bf87 3 #include "RobotControl_7Axis.h"
stanley1228 12:1eeea035bf87 4
stanley1228 0:1780ffc33286 5
stanley1228 4:53ef39fbf9d9 6
stanley1228 11:644c13da326d 7 #define DEBUG 1
stanley1228 11:644c13da326d 8
stanley1228 11:644c13da326d 9
stanley1228 11:644c13da326d 10 #if (DEBUG)
stanley1228 11:644c13da326d 11 #define DBGMSG(x) pc.printf x;
stanley1228 11:644c13da326d 12 #else
stanley1228 11:644c13da326d 13 #define DBGMSG(x)
stanley1228 11:644c13da326d 14 #endif
stanley1228 11:644c13da326d 15
stanley1228 11:644c13da326d 16
stanley1228 11:644c13da326d 17
stanley1228 9:54710454ce60 18 //==Pin define==//
stanley1228 0:1780ffc33286 19 DigitalOut myled(LED1);
stanley1228 0:1780ffc33286 20 Serial pc(SERIAL_TX, SERIAL_RX);
stanley1228 10:328cc5179ffb 21 DigitalIn mybutton(USER_BUTTON);
stanley1228 0:1780ffc33286 22
stanley1228 0:1780ffc33286 23 int main()
stanley1228 0:1780ffc33286 24 {
stanley1228 9:54710454ce60 25 int rt =0;
stanley1228 11:644c13da326d 26 unsigned char i=0;
stanley1228 11:644c13da326d 27
stanley1228 12:1eeea035bf87 28 //==================
stanley1228 12:1eeea035bf87 29 //==dxl_initialize
stanley1228 12:1eeea035bf87 30 //===================
stanley1228 0:1780ffc33286 31 rt=dxl_initialize( 1, 1);
stanley1228 11:644c13da326d 32 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 12:1eeea035bf87 33
stanley1228 9:54710454ce60 34 //=========================//
stanley1228 8:37f5a7219fe6 35 //==ROM parameter setting==//
stanley1228 9:54710454ce60 36 //=========================//
stanley1228 8:37f5a7219fe6 37 //rt=ROM_Setting();
stanley1228 11:644c13da326d 38 //while(1);
stanley1228 8:37f5a7219fe6 39
stanley1228 12:1eeea035bf87 40
stanley1228 9:54710454ce60 41 float Ang_rad[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 42 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 0:1780ffc33286 43 while(1)
stanley1228 0:1780ffc33286 44 {
stanley1228 10:328cc5179ffb 45 //==Test Output_to_Dynamixel==//
stanley1228 11:644c13da326d 46 //Ang_rad[Index_AXIS5]=160*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 47 //Ang_rad[Index_AXIS6]=100*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 48 //Ang_rad[Index_AXIS7]=90*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 49 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 11:644c13da326d 50 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 51
stanley1228 11:644c13da326d 52 //while(mybutton);
stanley1228 11:644c13da326d 53
stanley1228 11:644c13da326d 54 //
stanley1228 11:644c13da326d 55 //Ang_rad[Index_AXIS5]=-180*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 56 //Ang_rad[Index_AXIS6]=-70*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 57 //Ang_rad[Index_AXIS7]=-90*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 58 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 11:644c13da326d 59 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 60
stanley1228 11:644c13da326d 61 //while(mybutton);
stanley1228 11:644c13da326d 62
stanley1228 11:644c13da326d 63
stanley1228 10:328cc5179ffb 64
stanley1228 11:644c13da326d 65 //==Read robot pos by pos_deg==//
stanley1228 11:644c13da326d 66 float pos_deg[MAX_AXIS_NUM]={0};
stanley1228 12:1eeea035bf87 67 rt=Read_pos(pos_deg,DEF_UNIT_DEG);
stanley1228 12:1eeea035bf87 68 DBGMSG(("Read_pos rt==%d\n",rt));
stanley1228 10:328cc5179ffb 69
stanley1228 11:644c13da326d 70 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 71 {
stanley1228 12:1eeea035bf87 72 DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
stanley1228 11:644c13da326d 73 }
stanley1228 12:1eeea035bf87 74
stanley1228 7:15f0494813f7 75
stanley1228 7:15f0494813f7 76 }
stanley1228 4:53ef39fbf9d9 77 }
stanley1228 12:1eeea035bf87 78
stanley1228 12:1eeea035bf87 79
stanley1228 12:1eeea035bf87 80 //===Test move==//
stanley1228 12:1eeea035bf87 81 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 12:1eeea035bf87 82 //setPosition(3,2048,10);
stanley1228 12:1eeea035bf87 83 //pc.printf("after=%d\n",rt);
stanley1228 12:1eeea035bf87 84 //myled = 1; // LED is ON
stanley1228 12:1eeea035bf87 85 //wait(4);
stanley1228 12:1eeea035bf87 86 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 12:1eeea035bf87 87 //setPosition(3,-2048,10);
stanley1228 12:1eeea035bf87 88 //myled = 0; // LED is ON
stanley1228 12:1eeea035bf87 89 //wait(4);
stanley1228 12:1eeea035bf87 90
stanley1228 12:1eeea035bf87 91
stanley1228 12:1eeea035bf87 92 //===Test read pos====//
stanley1228 12:1eeea035bf87 93 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 12:1eeea035bf87 94 pc.printf("pos=%d\n",pos);
stanley1228 12:1eeea035bf87 95 wait_ms(200); */
stanley1228 12:1eeea035bf87 96
stanley1228 12:1eeea035bf87 97 //====Test read all pos===
stanley1228 12:1eeea035bf87 98 //static short int pos=0;
stanley1228 12:1eeea035bf87 99 //int i=0;
stanley1228 12:1eeea035bf87 100 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 12:1eeea035bf87 101 //{
stanley1228 12:1eeea035bf87 102 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 12:1eeea035bf87 103 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 12:1eeea035bf87 104 // pos=-1;
stanley1228 12:1eeea035bf87 105
stanley1228 12:1eeea035bf87 106 // pc.printf("X%d=%d,",i,pos);
stanley1228 12:1eeea035bf87 107 //}
stanley1228 12:1eeea035bf87 108 //pc.printf("\n");