hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@17:2b67b218da4d, 2017-04-17 (annotated)

Committer:
stanley1228
Date:
Mon Apr 17 13:51:42 2017 +0800
Revision:
17:2b67b218da4d
Parent:
14:235c64640200 
1.??????position

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 17:2b67b218da4d 81 ////==Test Output_to_Dynamixel==//
stanley1228 17:2b67b218da4d 82 //Ang_rad[Index_AXIS5]=140*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 83 //Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 84 //Ang_rad[Index_AXIS7]=10*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 85 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 17:2b67b218da4d 86 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 87
stanley1228 17:2b67b218da4d 88 //while(mybutton);
stanley1228 11:644c13da326d 89
stanley1228 17:2b67b218da4d 90 ////
stanley1228 17:2b67b218da4d 91 //Ang_rad[Index_AXIS5]=0*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 92 //Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 93 //Ang_rad[Index_AXIS7]=-10*DEF_RATIO_DEG_TO_RAD;
stanley1228 17:2b67b218da4d 94 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 17:2b67b218da4d 95 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 96
stanley1228 17:2b67b218da4d 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 17:2b67b218da4d 102 float pos_deg[MAX_AXIS_NUM]={0};
stanley1228 17:2b67b218da4d 103 rt=Read_pos(pos_deg,DEF_UNIT_DEG);
stanley1228 17:2b67b218da4d 104 DBGMSG(("Read_pos rt==%d\n",rt));
stanley1228 17:2b67b218da4d 105
stanley1228 17:2b67b218da4d 106 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 17:2b67b218da4d 107 {
stanley1228 17:2b67b218da4d 108 DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
stanley1228 17:2b67b218da4d 109 }
stanley1228 17:2b67b218da4d 110
stanley1228 17:2b67b218da4d 111 wait_ms(500);
stanley1228 17:2b67b218da4d 112 /*unsigned short int pos=0;
stanley1228 17:2b67b218da4d 113 for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 17:2b67b218da4d 114 {
stanley1228 17:2b67b218da4d 115 pos = dxl_read_word(i, PRESENT_POS);
stanley1228 17:2b67b218da4d 116 if((rt=dxl_get_result())!=COMM_RXSUCCESS)
stanley1228 17:2b67b218da4d 117 DBGMSG(("X%d dxl_get_result=%d\n",i,rt))
stanley1228 17:2b67b218da4d 118 else
stanley1228 17:2b67b218da4d 119 DBGMSG(("X%d=%d\n",i,pos))
stanley1228 17:2b67b218da4d 120 }*/
stanley1228 17:2b67b218da4d 121
stanley1228 7:15f0494813f7 122 }
stanley1228 4:53ef39fbf9d9 123 }
stanley1228 12:1eeea035bf87 124
stanley1228 12:1eeea035bf87 125
stanley1228 14:235c64640200 126
stanley1228 12:1eeea035bf87 127 //===Test move==//
stanley1228 12:1eeea035bf87 128 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 12:1eeea035bf87 129 //setPosition(3,2048,10);
stanley1228 12:1eeea035bf87 130 //pc.printf("after=%d\n",rt);
stanley1228 12:1eeea035bf87 131 //myled = 1; // LED is ON
stanley1228 12:1eeea035bf87 132 //wait(4);
stanley1228 12:1eeea035bf87 133 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 12:1eeea035bf87 134 //setPosition(3,-2048,10);
stanley1228 12:1eeea035bf87 135 //myled = 0; // LED is ON
stanley1228 12:1eeea035bf87 136 //wait(4);
stanley1228 12:1eeea035bf87 137
stanley1228 12:1eeea035bf87 138
stanley1228 12:1eeea035bf87 139 //===Test read pos====//
stanley1228 12:1eeea035bf87 140 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 12:1eeea035bf87 141 pc.printf("pos=%d\n",pos);
stanley1228 12:1eeea035bf87 142 wait_ms(200); */
stanley1228 12:1eeea035bf87 143
stanley1228 12:1eeea035bf87 144 //====Test read all pos===
stanley1228 12:1eeea035bf87 145 //static short int pos=0;
stanley1228 12:1eeea035bf87 146 //int i=0;
stanley1228 12:1eeea035bf87 147 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 12:1eeea035bf87 148 //{
stanley1228 12:1eeea035bf87 149 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 12:1eeea035bf87 150 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 12:1eeea035bf87 151 // pos=-1;
stanley1228 12:1eeea035bf87 152
stanley1228 12:1eeea035bf87 153 // pc.printf("X%d=%d,",i,pos);
stanley1228 12:1eeea035bf87 154 //}
stanley1228 12:1eeea035bf87 155 //pc.printf("\n");