![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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"); |