hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@14:235c64640200, 2017-02-16 (annotated)

Committer:
stanley1228
Date:
Thu Feb 16 11:38:26 2017 +0000
Revision:
14:235c64640200
Parent:
13:f0f52287352a
Child:
17:2b67b218da4d
test 567axis

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 13:f0f52287352a 20
stanley1228 13:f0f52287352a 21 DigitalOut Test(D4,PullDown);
stanley1228 0:1780ffc33286 22 Serial pc(SERIAL_TX, SERIAL_RX);
stanley1228 10:328cc5179ffb 23 DigitalIn mybutton(USER_BUTTON);
stanley1228 0:1780ffc33286 24
stanley1228 14:235c64640200 25
stanley1228 0:1780ffc33286 26 int main()
stanley1228 0:1780ffc33286 27 {
stanley1228 9:54710454ce60 28 int rt =0;
stanley1228 11:644c13da326d 29 unsigned char i=0;
stanley1228 14:235c64640200 30
stanley1228 14:235c64640200 31 //==================
stanley1228 14:235c64640200 32 //== Receive from PC by uart
stanley1228 14:235c64640200 33 //===================
stanley1228 14:235c64640200 34 //char buffer[128];
stanley1228 14:235c64640200 35 //
stanley1228 14:235c64640200 36 // pc.gets(buffer, 10);
stanley1228 14:235c64640200 37 // for(i=0;i<10;i++)
stanley1228 14:235c64640200 38 // {
stanley1228 14:235c64640200 39 // DBGMSG(("buffer[%d]=%d,",i,buffer[i]))
stanley1228 14:235c64640200 40 // }
stanley1228 14:235c64640200 41
stanley1228 14:235c64640200 42
stanley1228 14:235c64640200 43
stanley1228 14:235c64640200 44 // while(1);
stanley1228 12:1eeea035bf87 45 //==================
stanley1228 13:f0f52287352a 46 //== Test IK it need 120ms to calculate
stanley1228 13:f0f52287352a 47 //===================
stanley1228 13:f0f52287352a 48
stanley1228 14:235c64640200 49 //DBGMSG(("start"))
stanley1228 14:235c64640200 50 // float theta[7]={0};
stanley1228 14:235c64640200 51 //
stanley1228 14:235c64640200 52 // myled=1;
stanley1228 14:235c64640200 53 // rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta);
stanley1228 14:235c64640200 54 // myled=0;
stanley1228 14:235c64640200 55 //
stanley1228 14:235c64640200 56 //
stanley1228 14:235c64640200 57 // for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 14:235c64640200 58 // {
stanley1228 14:235c64640200 59 // DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i]))
stanley1228 14:235c64640200 60 // }
stanley1228 14:235c64640200 61 // DBGMSG(("\n"))
stanley1228 13:f0f52287352a 62
stanley1228 13:f0f52287352a 63
stanley1228 13:f0f52287352a 64 //==================
stanley1228 12:1eeea035bf87 65 //==dxl_initialize
stanley1228 12:1eeea035bf87 66 //===================
stanley1228 0:1780ffc33286 67 rt=dxl_initialize( 1, 1);
stanley1228 11:644c13da326d 68 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 12:1eeea035bf87 69
stanley1228 9:54710454ce60 70 //=========================//
stanley1228 8:37f5a7219fe6 71 //==ROM parameter setting==//
stanley1228 9:54710454ce60 72 //=========================//
stanley1228 8:37f5a7219fe6 73 //rt=ROM_Setting();
stanley1228 11:644c13da326d 74 //while(1);
stanley1228 8:37f5a7219fe6 75
stanley1228 12:1eeea035bf87 76
stanley1228 9:54710454ce60 77 float Ang_rad[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 78 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 0:1780ffc33286 79 while(1)
stanley1228 0:1780ffc33286 80 {
stanley1228 10:328cc5179ffb 81 //==Test Output_to_Dynamixel==//
stanley1228 14:235c64640200 82 Ang_rad[Index_AXIS5]=140*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 83 Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 84 Ang_rad[Index_AXIS7]=10*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 85 rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 14:235c64640200 86 pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 87
stanley1228 14:235c64640200 88 while(mybutton);
stanley1228 11:644c13da326d 89
stanley1228 11:644c13da326d 90 //
stanley1228 14:235c64640200 91 Ang_rad[Index_AXIS5]=0*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 92 Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 93 Ang_rad[Index_AXIS7]=-10*DEF_RATIO_DEG_TO_RAD;
stanley1228 14:235c64640200 94 rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 14:235c64640200 95 pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 96
stanley1228 14:235c64640200 97 while(mybutton);
stanley1228 11:644c13da326d 98
stanley1228 11:644c13da326d 99
stanley1228 10:328cc5179ffb 100
stanley1228 11:644c13da326d 101 //==Read robot pos by pos_deg==//
stanley1228 14:235c64640200 102 //float pos_deg[MAX_AXIS_NUM]={0};
stanley1228 14:235c64640200 103 // rt=Read_pos(pos_deg,DEF_UNIT_DEG);
stanley1228 14:235c64640200 104 // DBGMSG(("Read_pos rt==%d\n",rt));
stanley1228 14:235c64640200 105 //
stanley1228 14:235c64640200 106 // for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 14:235c64640200 107 // {
stanley1228 14:235c64640200 108 // DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
stanley1228 14:235c64640200 109 // }
stanley1228 12:1eeea035bf87 110
stanley1228 7:15f0494813f7 111
stanley1228 7:15f0494813f7 112 }
stanley1228 4:53ef39fbf9d9 113 }
stanley1228 12:1eeea035bf87 114
stanley1228 12:1eeea035bf87 115
stanley1228 14:235c64640200 116
stanley1228 12:1eeea035bf87 117 //===Test move==//
stanley1228 12:1eeea035bf87 118 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 12:1eeea035bf87 119 //setPosition(3,2048,10);
stanley1228 12:1eeea035bf87 120 //pc.printf("after=%d\n",rt);
stanley1228 12:1eeea035bf87 121 //myled = 1; // LED is ON
stanley1228 12:1eeea035bf87 122 //wait(4);
stanley1228 12:1eeea035bf87 123 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 12:1eeea035bf87 124 //setPosition(3,-2048,10);
stanley1228 12:1eeea035bf87 125 //myled = 0; // LED is ON
stanley1228 12:1eeea035bf87 126 //wait(4);
stanley1228 12:1eeea035bf87 127
stanley1228 12:1eeea035bf87 128
stanley1228 12:1eeea035bf87 129 //===Test read pos====//
stanley1228 12:1eeea035bf87 130 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 12:1eeea035bf87 131 pc.printf("pos=%d\n",pos);
stanley1228 12:1eeea035bf87 132 wait_ms(200); */
stanley1228 12:1eeea035bf87 133
stanley1228 12:1eeea035bf87 134 //====Test read all pos===
stanley1228 12:1eeea035bf87 135 //static short int pos=0;
stanley1228 12:1eeea035bf87 136 //int i=0;
stanley1228 12:1eeea035bf87 137 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 12:1eeea035bf87 138 //{
stanley1228 12:1eeea035bf87 139 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 12:1eeea035bf87 140 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 12:1eeea035bf87 141 // pos=-1;
stanley1228 12:1eeea035bf87 142
stanley1228 12:1eeea035bf87 143 // pc.printf("X%d=%d,",i,pos);
stanley1228 12:1eeea035bf87 144 //}
stanley1228 12:1eeea035bf87 145 //pc.printf("\n");