hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@13:f0f52287352a, 2017-02-11 (annotated)

Committer:
stanley1228
Date:
Sat Feb 11 13:07:35 2017 +0000
Revision:
13:f0f52287352a
Parent:
12:1eeea035bf87
Child:
14:235c64640200
Test IK

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