![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp@8:37f5a7219fe6, 2017-02-08 (annotated)
- Committer:
- stanley1228
- Date:
- Wed Feb 08 15:50:53 2017 +0800
- Revision:
- 8:37f5a7219fe6
- Parent:
- 7:15f0494813f7
- Child:
- 9:54710454ce60
not change in tx rx
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 | 8:37f5a7219fe6 | 18 | #define MAX_AXIS_NUM ID_AXIS7 |
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 | 8:37f5a7219fe6 | 40 | int ROM_Setting() |
stanley1228 | 8:37f5a7219fe6 | 41 | { |
stanley1228 | 8:37f5a7219fe6 | 42 | |
stanley1228 | 8:37f5a7219fe6 | 43 | int i=0; |
stanley1228 | 8:37f5a7219fe6 | 44 | |
stanley1228 | 8:37f5a7219fe6 | 45 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 8:37f5a7219fe6 | 46 | { |
stanley1228 | 8:37f5a7219fe6 | 47 | //Set MAX_torgue |
stanley1228 | 8:37f5a7219fe6 | 48 | dxl_write_word(i,MAX_TORQUE,55);// more safe |
stanley1228 | 8:37f5a7219fe6 | 49 | |
stanley1228 | 8:37f5a7219fe6 | 50 | //Set all to multi turn mode==// |
stanley1228 | 8:37f5a7219fe6 | 51 | dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff); |
stanley1228 | 8:37f5a7219fe6 | 52 | dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff); |
stanley1228 | 8:37f5a7219fe6 | 53 | |
stanley1228 | 8:37f5a7219fe6 | 54 | //Set all to multi turn mode==// |
stanley1228 | 8:37f5a7219fe6 | 55 | dxl_write_word(i,MULTITURN_OFFSET,-1024); |
stanley1228 | 8:37f5a7219fe6 | 56 | } |
stanley1228 | 8:37f5a7219fe6 | 57 | |
stanley1228 | 8:37f5a7219fe6 | 58 | |
stanley1228 | 8:37f5a7219fe6 | 59 | //==read and check ==// |
stanley1228 | 8:37f5a7219fe6 | 60 | int txrx_result=0; |
stanley1228 | 8:37f5a7219fe6 | 61 | short int max_torque=0; |
stanley1228 | 8:37f5a7219fe6 | 62 | short int cw_angel_lim=0,ccw_angle_lim=0; |
stanley1228 | 8:37f5a7219fe6 | 63 | short int multi_turn_offset=0; |
stanley1228 | 8:37f5a7219fe6 | 64 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 8:37f5a7219fe6 | 65 | { |
stanley1228 | 8:37f5a7219fe6 | 66 | pc.printf("===AXIS_%d===\n",i); |
stanley1228 | 8:37f5a7219fe6 | 67 | |
stanley1228 | 8:37f5a7219fe6 | 68 | //MAX_torgue |
stanley1228 | 8:37f5a7219fe6 | 69 | max_torque = dxl_read_word(i, MAX_TORQUE); |
stanley1228 | 8:37f5a7219fe6 | 70 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 71 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 72 | pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 73 | else |
stanley1228 | 8:37f5a7219fe6 | 74 | pc.printf("MAX_TORQUE=%d\n",max_torque); |
stanley1228 | 8:37f5a7219fe6 | 75 | |
stanley1228 | 8:37f5a7219fe6 | 76 | //CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==// |
stanley1228 | 8:37f5a7219fe6 | 77 | cw_angel_lim=dxl_read_word(i,CW_ANGLE_LIMIT_L); |
stanley1228 | 8:37f5a7219fe6 | 78 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 79 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 80 | pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 81 | else |
stanley1228 | 8:37f5a7219fe6 | 82 | pc.printf("CW_ANGLE_LIMIT=%x\n",cw_angel_lim); |
stanley1228 | 8:37f5a7219fe6 | 83 | |
stanley1228 | 8:37f5a7219fe6 | 84 | ccw_angle_lim=dxl_read_word(i,CCW_ANGLE_LIMIT_L); |
stanley1228 | 8:37f5a7219fe6 | 85 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 86 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 87 | pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 88 | else |
stanley1228 | 8:37f5a7219fe6 | 89 | pc.printf("CCW_ANGLE_LIMIT=%x\n",ccw_angle_lim); |
stanley1228 | 8:37f5a7219fe6 | 90 | |
stanley1228 | 8:37f5a7219fe6 | 91 | |
stanley1228 | 8:37f5a7219fe6 | 92 | //multi turn offset==// |
stanley1228 | 8:37f5a7219fe6 | 93 | multi_turn_offset=dxl_read_word(i,MULTITURN_OFFSET); |
stanley1228 | 8:37f5a7219fe6 | 94 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 95 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 96 | pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 97 | else |
stanley1228 | 8:37f5a7219fe6 | 98 | pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset); |
stanley1228 | 8:37f5a7219fe6 | 99 | } |
stanley1228 | 8:37f5a7219fe6 | 100 | |
stanley1228 | 8:37f5a7219fe6 | 101 | return 0; |
stanley1228 | 8:37f5a7219fe6 | 102 | } |
stanley1228 | 8:37f5a7219fe6 | 103 | |
stanley1228 | 0:1780ffc33286 | 104 | int main() |
stanley1228 | 0:1780ffc33286 | 105 | { |
stanley1228 | 0:1780ffc33286 | 106 | int rt =0; |
stanley1228 | 0:1780ffc33286 | 107 | |
stanley1228 | 0:1780ffc33286 | 108 | pc.printf("start\n",rt); |
stanley1228 | 0:1780ffc33286 | 109 | |
stanley1228 | 0:1780ffc33286 | 110 | rt=dxl_initialize( 1, 1); |
stanley1228 | 0:1780ffc33286 | 111 | pc.printf("dxl_initialize rt=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 112 | |
stanley1228 | 8:37f5a7219fe6 | 113 | short int pos=0; |
stanley1228 | 8:37f5a7219fe6 | 114 | |
stanley1228 | 8:37f5a7219fe6 | 115 | //==ROM parameter setting==// |
stanley1228 | 8:37f5a7219fe6 | 116 | //rt=ROM_Setting(); |
stanley1228 | 8:37f5a7219fe6 | 117 | |
stanley1228 | 8:37f5a7219fe6 | 118 | |
stanley1228 | 8:37f5a7219fe6 | 119 | myled = 0; // LED is ON |
stanley1228 | 0:1780ffc33286 | 120 | /************************** |
stanley1228 | 0:1780ffc33286 | 121 | *** Check communication *** |
stanley1228 | 0:1780ffc33286 | 122 | **************************/ |
stanley1228 | 2:fb9508744bc4 | 123 | //dxl_write_byte(3,ADDRESS_LED,0); |
stanley1228 | 0:1780ffc33286 | 124 | //dxl_ping(3); |
stanley1228 | 8:37f5a7219fe6 | 125 | |
stanley1228 | 0:1780ffc33286 | 126 | while(1) |
stanley1228 | 0:1780ffc33286 | 127 | { |
stanley1228 | 3:8a9407817ba9 | 128 | //pc.printf("before=%d\n",rt); |
stanley1228 | 2:fb9508744bc4 | 129 | |
stanley1228 | 4:53ef39fbf9d9 | 130 | |
stanley1228 | 4:53ef39fbf9d9 | 131 | //===Test move |
stanley1228 | 2:fb9508744bc4 | 132 | //dxl_write_word(3,GOAL_POSITION,400); |
stanley1228 | 3:8a9407817ba9 | 133 | //setPosition(3,2048,10); |
stanley1228 | 3:8a9407817ba9 | 134 | //pc.printf("after=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 135 | //myled = 1; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 136 | //wait(4); |
stanley1228 | 2:fb9508744bc4 | 137 | //dxl_write_word(3,GOAL_POSITION,-400); |
stanley1228 | 3:8a9407817ba9 | 138 | //setPosition(3,-2048,10); |
stanley1228 | 0:1780ffc33286 | 139 | //myled = 0; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 140 | //wait(4); |
stanley1228 | 3:8a9407817ba9 | 141 | |
stanley1228 | 4:53ef39fbf9d9 | 142 | |
stanley1228 | 4:53ef39fbf9d9 | 143 | //===Test read pos====// |
stanley1228 | 4:53ef39fbf9d9 | 144 | /*pos=dxl_read_word(3,PRESENT_POS); |
stanley1228 | 3:8a9407817ba9 | 145 | pc.printf("pos=%d\n",pos); |
stanley1228 | 4:53ef39fbf9d9 | 146 | wait_ms(200); */ |
stanley1228 | 3:8a9407817ba9 | 147 | |
stanley1228 | 4:53ef39fbf9d9 | 148 | //====Test read all pos=== |
stanley1228 | 4:53ef39fbf9d9 | 149 | int i=0; |
stanley1228 | 4:53ef39fbf9d9 | 150 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 4:53ef39fbf9d9 | 151 | { |
stanley1228 | 4:53ef39fbf9d9 | 152 | pos = dxl_read_word(i, PRESENT_POS); |
stanley1228 | 4:53ef39fbf9d9 | 153 | if(dxl_get_result()!=COMM_RXSUCCESS) |
stanley1228 | 4:53ef39fbf9d9 | 154 | pos=-1; |
stanley1228 | 4:53ef39fbf9d9 | 155 | |
stanley1228 | 4:53ef39fbf9d9 | 156 | pc.printf("X%d=%d,",i,pos); |
stanley1228 | 4:53ef39fbf9d9 | 157 | } |
stanley1228 | 4:53ef39fbf9d9 | 158 | pc.printf("\n"); |
stanley1228 | 7:15f0494813f7 | 159 | |
stanley1228 | 7:15f0494813f7 | 160 | |
stanley1228 | 7:15f0494813f7 | 161 | |
stanley1228 | 7:15f0494813f7 | 162 | //====Test syncWrite_u16base=== |
stanley1228 | 7:15f0494813f7 | 163 | static short int pos3=-100; |
stanley1228 | 7:15f0494813f7 | 164 | pos3=(pos3==-100)? 100 :-100; |
stanley1228 | 7:15f0494813f7 | 165 | |
stanley1228 | 7:15f0494813f7 | 166 | short int SyncPage1[21]= |
stanley1228 | 7:15f0494813f7 | 167 | { |
stanley1228 | 7:15f0494813f7 | 168 | ID_AXIS1,100,5, |
stanley1228 | 7:15f0494813f7 | 169 | ID_AXIS2,100,5, |
stanley1228 | 7:15f0494813f7 | 170 | ID_AXIS3,100,5, |
stanley1228 | 7:15f0494813f7 | 171 | ID_AXIS4,100,5, |
stanley1228 | 7:15f0494813f7 | 172 | ID_AXIS5,100,5, |
stanley1228 | 7:15f0494813f7 | 173 | ID_AXIS6,100,5, |
stanley1228 | 7:15f0494813f7 | 174 | ID_AXIS7,100,5, |
stanley1228 | 7:15f0494813f7 | 175 | }; |
stanley1228 | 7:15f0494813f7 | 176 | |
stanley1228 | 8:37f5a7219fe6 | 177 | wait(5); |
stanley1228 | 7:15f0494813f7 | 178 | //short int SyncPage1[21]=//Test use |
stanley1228 | 7:15f0494813f7 | 179 | //{ |
stanley1228 | 7:15f0494813f7 | 180 | // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 |
stanley1228 | 7:15f0494813f7 | 181 | // 0x01,0x020,0x360, |
stanley1228 | 7:15f0494813f7 | 182 | // 0x02,0x030,0x170, |
stanley1228 | 7:15f0494813f7 | 183 | // 0x03,0x220,0x380, |
stanley1228 | 7:15f0494813f7 | 184 | // 0x05,0x123,0x121, |
stanley1228 | 7:15f0494813f7 | 185 | // 0x06,0x234,0x143, |
stanley1228 | 7:15f0494813f7 | 186 | // 0x07,0x145,0x167 |
stanley1228 | 7:15f0494813f7 | 187 | //}; |
stanley1228 | 7:15f0494813f7 | 188 | |
stanley1228 | 7:15f0494813f7 | 189 | |
stanley1228 | 7:15f0494813f7 | 190 | syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; |
stanley1228 | 7:15f0494813f7 | 191 | |
stanley1228 | 7:15f0494813f7 | 192 | } |
stanley1228 | 4:53ef39fbf9d9 | 193 | } |