![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp@7:15f0494813f7, 2017-02-08 (annotated)
- Committer:
- stanley1228
- Date:
- Wed Feb 08 12:27:32 2017 +0800
- Revision:
- 7:15f0494813f7
- Parent:
- 4:53ef39fbf9d9
- Child:
- 8:37f5a7219fe6
sync write test seem ok need more test
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 | 0:1780ffc33286 | 3 | |
stanley1228 | 4:53ef39fbf9d9 | 4 | |
stanley1228 | 4:53ef39fbf9d9 | 5 | |
stanley1228 | 4:53ef39fbf9d9 | 6 | |
stanley1228 | 4:53ef39fbf9d9 | 7 | //stanley robot test define// |
stanley1228 | 4:53ef39fbf9d9 | 8 | enum{ |
stanley1228 | 4:53ef39fbf9d9 | 9 | ID_AXIS1=1, |
stanley1228 | 4:53ef39fbf9d9 | 10 | ID_AXIS2, |
stanley1228 | 4:53ef39fbf9d9 | 11 | ID_AXIS3, |
stanley1228 | 4:53ef39fbf9d9 | 12 | ID_AXIS4, |
stanley1228 | 4:53ef39fbf9d9 | 13 | ID_AXIS5, |
stanley1228 | 4:53ef39fbf9d9 | 14 | ID_AXIS6, |
stanley1228 | 4:53ef39fbf9d9 | 15 | ID_AXIS7 |
stanley1228 | 4:53ef39fbf9d9 | 16 | }; |
stanley1228 | 4:53ef39fbf9d9 | 17 | |
stanley1228 | 4:53ef39fbf9d9 | 18 | #define MAX_AXIS_NUM 7 |
stanley1228 | 4:53ef39fbf9d9 | 19 | |
stanley1228 | 4:53ef39fbf9d9 | 20 | #define AXIS1_PULSE_LIM_L (-910) |
stanley1228 | 4:53ef39fbf9d9 | 21 | #define AXIS1_PULSE_LIM_H 2048 |
stanley1228 | 4:53ef39fbf9d9 | 22 | #define AXIS2_PULSE_LIM_L (-205) |
stanley1228 | 4:53ef39fbf9d9 | 23 | #define AXIS2_PULSE_LIM_H 2048 |
stanley1228 | 4:53ef39fbf9d9 | 24 | #define AXIS3_PULSE_LIM_L (-2840) |
stanley1228 | 4:53ef39fbf9d9 | 25 | #define AXIS3_PULSE_LIM_H 1190 |
stanley1228 | 4:53ef39fbf9d9 | 26 | #define AXIS4_PULSE_LIM_L 0//need to test |
stanley1228 | 4:53ef39fbf9d9 | 27 | #define AXIS4_PULSE_LIM_H 0//need to test |
stanley1228 | 4:53ef39fbf9d9 | 28 | #define AXIS5_PULSE_LIM_L (-2048) |
stanley1228 | 4:53ef39fbf9d9 | 29 | #define AXIS5_PULSE_LIM_H 1680 |
stanley1228 | 4:53ef39fbf9d9 | 30 | #define AXIS6_PULSE_LIM_L (-680) |
stanley1228 | 4:53ef39fbf9d9 | 31 | #define AXIS6_PULSE_LIM_H 1024 |
stanley1228 | 4:53ef39fbf9d9 | 32 | #define AXIS7_PULSE_LIM_L (-420) |
stanley1228 | 4:53ef39fbf9d9 | 33 | #define AXIS7_PULSE_LIM_H 420 |
stanley1228 | 4:53ef39fbf9d9 | 34 | |
stanley1228 | 0:1780ffc33286 | 35 | |
stanley1228 | 0:1780ffc33286 | 36 | DigitalOut myled(LED1); |
stanley1228 | 0:1780ffc33286 | 37 | Serial pc(SERIAL_TX, SERIAL_RX); |
stanley1228 | 0:1780ffc33286 | 38 | |
stanley1228 | 0:1780ffc33286 | 39 | |
stanley1228 | 0:1780ffc33286 | 40 | int main() |
stanley1228 | 0:1780ffc33286 | 41 | { |
stanley1228 | 0:1780ffc33286 | 42 | int rt =0; |
stanley1228 | 0:1780ffc33286 | 43 | |
stanley1228 | 0:1780ffc33286 | 44 | pc.printf("start\n",rt); |
stanley1228 | 0:1780ffc33286 | 45 | |
stanley1228 | 0:1780ffc33286 | 46 | rt=dxl_initialize( 1, 1); |
stanley1228 | 0:1780ffc33286 | 47 | pc.printf("dxl_initialize rt=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 48 | |
stanley1228 | 0:1780ffc33286 | 49 | myled = 0; // LED is ON |
stanley1228 | 0:1780ffc33286 | 50 | |
stanley1228 | 0:1780ffc33286 | 51 | /************************** |
stanley1228 | 0:1780ffc33286 | 52 | *** Check communication *** |
stanley1228 | 0:1780ffc33286 | 53 | **************************/ |
stanley1228 | 2:fb9508744bc4 | 54 | //dxl_write_byte(3,ADDRESS_LED,0); |
stanley1228 | 0:1780ffc33286 | 55 | //dxl_ping(3); |
stanley1228 | 0:1780ffc33286 | 56 | |
stanley1228 | 7:15f0494813f7 | 57 | short int pos=0; //16bit |
stanley1228 | 0:1780ffc33286 | 58 | while(1) |
stanley1228 | 0:1780ffc33286 | 59 | { |
stanley1228 | 3:8a9407817ba9 | 60 | //pc.printf("before=%d\n",rt); |
stanley1228 | 2:fb9508744bc4 | 61 | |
stanley1228 | 4:53ef39fbf9d9 | 62 | |
stanley1228 | 4:53ef39fbf9d9 | 63 | //===Test move |
stanley1228 | 2:fb9508744bc4 | 64 | //dxl_write_word(3,GOAL_POSITION,400); |
stanley1228 | 3:8a9407817ba9 | 65 | //setPosition(3,2048,10); |
stanley1228 | 3:8a9407817ba9 | 66 | //pc.printf("after=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 67 | //myled = 1; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 68 | //wait(4); |
stanley1228 | 2:fb9508744bc4 | 69 | //dxl_write_word(3,GOAL_POSITION,-400); |
stanley1228 | 3:8a9407817ba9 | 70 | //setPosition(3,-2048,10); |
stanley1228 | 0:1780ffc33286 | 71 | //myled = 0; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 72 | //wait(4); |
stanley1228 | 3:8a9407817ba9 | 73 | |
stanley1228 | 4:53ef39fbf9d9 | 74 | |
stanley1228 | 4:53ef39fbf9d9 | 75 | //===Test read pos====// |
stanley1228 | 4:53ef39fbf9d9 | 76 | /*pos=dxl_read_word(3,PRESENT_POS); |
stanley1228 | 3:8a9407817ba9 | 77 | pc.printf("pos=%d\n",pos); |
stanley1228 | 4:53ef39fbf9d9 | 78 | wait_ms(200); */ |
stanley1228 | 3:8a9407817ba9 | 79 | |
stanley1228 | 4:53ef39fbf9d9 | 80 | //====Test read all pos=== |
stanley1228 | 4:53ef39fbf9d9 | 81 | int i=0; |
stanley1228 | 4:53ef39fbf9d9 | 82 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 4:53ef39fbf9d9 | 83 | { |
stanley1228 | 4:53ef39fbf9d9 | 84 | pos = dxl_read_word(i, PRESENT_POS); |
stanley1228 | 4:53ef39fbf9d9 | 85 | if(dxl_get_result()!=COMM_RXSUCCESS) |
stanley1228 | 4:53ef39fbf9d9 | 86 | pos=-1; |
stanley1228 | 4:53ef39fbf9d9 | 87 | |
stanley1228 | 4:53ef39fbf9d9 | 88 | pc.printf("X%d=%d,",i,pos); |
stanley1228 | 4:53ef39fbf9d9 | 89 | } |
stanley1228 | 4:53ef39fbf9d9 | 90 | pc.printf("\n"); |
stanley1228 | 7:15f0494813f7 | 91 | |
stanley1228 | 7:15f0494813f7 | 92 | |
stanley1228 | 7:15f0494813f7 | 93 | |
stanley1228 | 7:15f0494813f7 | 94 | //====Test syncWrite_u16base=== |
stanley1228 | 7:15f0494813f7 | 95 | static short int pos3=-100; |
stanley1228 | 7:15f0494813f7 | 96 | pos3=(pos3==-100)? 100 :-100; |
stanley1228 | 7:15f0494813f7 | 97 | |
stanley1228 | 7:15f0494813f7 | 98 | short int SyncPage1[21]= |
stanley1228 | 7:15f0494813f7 | 99 | { |
stanley1228 | 7:15f0494813f7 | 100 | ID_AXIS1,100,5, |
stanley1228 | 7:15f0494813f7 | 101 | ID_AXIS2,100,5, |
stanley1228 | 7:15f0494813f7 | 102 | ID_AXIS3,100,5, |
stanley1228 | 7:15f0494813f7 | 103 | ID_AXIS4,100,5, |
stanley1228 | 7:15f0494813f7 | 104 | ID_AXIS5,100,5, |
stanley1228 | 7:15f0494813f7 | 105 | ID_AXIS6,100,5, |
stanley1228 | 7:15f0494813f7 | 106 | ID_AXIS7,100,5, |
stanley1228 | 7:15f0494813f7 | 107 | }; |
stanley1228 | 7:15f0494813f7 | 108 | |
stanley1228 | 7:15f0494813f7 | 109 | wait(2); |
stanley1228 | 7:15f0494813f7 | 110 | //short int SyncPage1[21]=//Test use |
stanley1228 | 7:15f0494813f7 | 111 | //{ |
stanley1228 | 7:15f0494813f7 | 112 | // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 |
stanley1228 | 7:15f0494813f7 | 113 | // 0x01,0x020,0x360, |
stanley1228 | 7:15f0494813f7 | 114 | // 0x02,0x030,0x170, |
stanley1228 | 7:15f0494813f7 | 115 | // 0x03,0x220,0x380, |
stanley1228 | 7:15f0494813f7 | 116 | // 0x05,0x123,0x121, |
stanley1228 | 7:15f0494813f7 | 117 | // 0x06,0x234,0x143, |
stanley1228 | 7:15f0494813f7 | 118 | // 0x07,0x145,0x167 |
stanley1228 | 7:15f0494813f7 | 119 | //}; |
stanley1228 | 7:15f0494813f7 | 120 | |
stanley1228 | 7:15f0494813f7 | 121 | |
stanley1228 | 7:15f0494813f7 | 122 | syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; |
stanley1228 | 7:15f0494813f7 | 123 | |
stanley1228 | 7:15f0494813f7 | 124 | } |
stanley1228 | 4:53ef39fbf9d9 | 125 | } |