![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp@4:53ef39fbf9d9, 2017-02-08 (annotated)
- Committer:
- stanley1228
- Date:
- Wed Feb 08 00:32:55 2017 +0800
- Revision:
- 4:53ef39fbf9d9
- Parent:
- 3:8a9407817ba9
- Child:
- 7:15f0494813f7
1.Add if(dxl_get_result()!=COMM_RXSUCCESS) pos=-1;
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 | 3:8a9407817ba9 | 57 | short int pos=0; //16bit |
stanley1228 | 0:1780ffc33286 | 58 | while(1) |
stanley1228 | 0:1780ffc33286 | 59 | { |
stanley1228 | 2:fb9508744bc4 | 60 | |
stanley1228 | 3:8a9407817ba9 | 61 | //pc.printf("before=%d\n",rt); |
stanley1228 | 2:fb9508744bc4 | 62 | |
stanley1228 | 4:53ef39fbf9d9 | 63 | |
stanley1228 | 4:53ef39fbf9d9 | 64 | //===Test move |
stanley1228 | 2:fb9508744bc4 | 65 | //dxl_write_word(3,GOAL_POSITION,400); |
stanley1228 | 3:8a9407817ba9 | 66 | //setPosition(3,2048,10); |
stanley1228 | 3:8a9407817ba9 | 67 | //pc.printf("after=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 68 | //myled = 1; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 69 | //wait(4); |
stanley1228 | 2:fb9508744bc4 | 70 | //dxl_write_word(3,GOAL_POSITION,-400); |
stanley1228 | 3:8a9407817ba9 | 71 | //setPosition(3,-2048,10); |
stanley1228 | 0:1780ffc33286 | 72 | //myled = 0; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 73 | //wait(4); |
stanley1228 | 3:8a9407817ba9 | 74 | |
stanley1228 | 4:53ef39fbf9d9 | 75 | |
stanley1228 | 4:53ef39fbf9d9 | 76 | //===Test read pos====// |
stanley1228 | 4:53ef39fbf9d9 | 77 | /*pos=dxl_read_word(3,PRESENT_POS); |
stanley1228 | 3:8a9407817ba9 | 78 | pc.printf("pos=%d\n",pos); |
stanley1228 | 4:53ef39fbf9d9 | 79 | wait_ms(200); */ |
stanley1228 | 3:8a9407817ba9 | 80 | |
stanley1228 | 4:53ef39fbf9d9 | 81 | //====Test read all pos=== |
stanley1228 | 4:53ef39fbf9d9 | 82 | int i=0; |
stanley1228 | 4:53ef39fbf9d9 | 83 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 4:53ef39fbf9d9 | 84 | { |
stanley1228 | 4:53ef39fbf9d9 | 85 | pos = dxl_read_word(i, PRESENT_POS); |
stanley1228 | 4:53ef39fbf9d9 | 86 | if(dxl_get_result()!=COMM_RXSUCCESS) |
stanley1228 | 4:53ef39fbf9d9 | 87 | pos=-1; |
stanley1228 | 4:53ef39fbf9d9 | 88 | |
stanley1228 | 4:53ef39fbf9d9 | 89 | pc.printf("X%d=%d,",i,pos); |
stanley1228 | 4:53ef39fbf9d9 | 90 | } |
stanley1228 | 4:53ef39fbf9d9 | 91 | pc.printf("\n"); |
stanley1228 | 4:53ef39fbf9d9 | 92 | wait_ms(500); |
stanley1228 | 0:1780ffc33286 | 93 | } |
stanley1228 | 4:53ef39fbf9d9 | 94 | } |