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?
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 | 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"); |