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