![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp@10:328cc5179ffb, 2017-02-09 (annotated)
- Committer:
- stanley1228
- Date:
- Thu Feb 09 17:59:27 2017 +0800
- Revision:
- 10:328cc5179ffb
- Parent:
- 9:54710454ce60
- Child:
- 11:644c13da326d
1.use button to 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 | 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 | 9:54710454ce60 | 35 | #define DEF_PI (3.1415) |
stanley1228 | 0:1780ffc33286 | 36 | |
stanley1228 | 9:54710454ce60 | 37 | |
stanley1228 | 9:54710454ce60 | 38 | //==Pin define==// |
stanley1228 | 0:1780ffc33286 | 39 | DigitalOut myled(LED1); |
stanley1228 | 0:1780ffc33286 | 40 | Serial pc(SERIAL_TX, SERIAL_RX); |
stanley1228 | 10:328cc5179ffb | 41 | DigitalIn mybutton(USER_BUTTON); |
stanley1228 | 0:1780ffc33286 | 42 | |
stanley1228 | 8:37f5a7219fe6 | 43 | int ROM_Setting() |
stanley1228 | 8:37f5a7219fe6 | 44 | { |
stanley1228 | 8:37f5a7219fe6 | 45 | |
stanley1228 | 8:37f5a7219fe6 | 46 | int i=0; |
stanley1228 | 8:37f5a7219fe6 | 47 | |
stanley1228 | 8:37f5a7219fe6 | 48 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 8:37f5a7219fe6 | 49 | { |
stanley1228 | 8:37f5a7219fe6 | 50 | //Set MAX_torgue |
stanley1228 | 8:37f5a7219fe6 | 51 | dxl_write_word(i,MAX_TORQUE,55);// more safe |
stanley1228 | 8:37f5a7219fe6 | 52 | |
stanley1228 | 8:37f5a7219fe6 | 53 | //Set all to multi turn mode==// |
stanley1228 | 8:37f5a7219fe6 | 54 | dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff); |
stanley1228 | 8:37f5a7219fe6 | 55 | dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff); |
stanley1228 | 8:37f5a7219fe6 | 56 | |
stanley1228 | 8:37f5a7219fe6 | 57 | //Set all to multi turn mode==// |
stanley1228 | 8:37f5a7219fe6 | 58 | dxl_write_word(i,MULTITURN_OFFSET,-1024); |
stanley1228 | 8:37f5a7219fe6 | 59 | } |
stanley1228 | 8:37f5a7219fe6 | 60 | |
stanley1228 | 8:37f5a7219fe6 | 61 | |
stanley1228 | 8:37f5a7219fe6 | 62 | //==read and check ==// |
stanley1228 | 8:37f5a7219fe6 | 63 | int txrx_result=0; |
stanley1228 | 8:37f5a7219fe6 | 64 | short int max_torque=0; |
stanley1228 | 8:37f5a7219fe6 | 65 | short int cw_angel_lim=0,ccw_angle_lim=0; |
stanley1228 | 8:37f5a7219fe6 | 66 | short int multi_turn_offset=0; |
stanley1228 | 8:37f5a7219fe6 | 67 | for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 8:37f5a7219fe6 | 68 | { |
stanley1228 | 8:37f5a7219fe6 | 69 | pc.printf("===AXIS_%d===\n",i); |
stanley1228 | 8:37f5a7219fe6 | 70 | |
stanley1228 | 8:37f5a7219fe6 | 71 | //MAX_torgue |
stanley1228 | 8:37f5a7219fe6 | 72 | max_torque = dxl_read_word(i, MAX_TORQUE); |
stanley1228 | 8:37f5a7219fe6 | 73 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 74 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 75 | pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 76 | else |
stanley1228 | 8:37f5a7219fe6 | 77 | pc.printf("MAX_TORQUE=%d\n",max_torque); |
stanley1228 | 8:37f5a7219fe6 | 78 | |
stanley1228 | 8:37f5a7219fe6 | 79 | //CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==// |
stanley1228 | 8:37f5a7219fe6 | 80 | cw_angel_lim=dxl_read_word(i,CW_ANGLE_LIMIT_L); |
stanley1228 | 8:37f5a7219fe6 | 81 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 82 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 83 | pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 84 | else |
stanley1228 | 8:37f5a7219fe6 | 85 | pc.printf("CW_ANGLE_LIMIT=%x\n",cw_angel_lim); |
stanley1228 | 8:37f5a7219fe6 | 86 | |
stanley1228 | 8:37f5a7219fe6 | 87 | ccw_angle_lim=dxl_read_word(i,CCW_ANGLE_LIMIT_L); |
stanley1228 | 8:37f5a7219fe6 | 88 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 89 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 90 | pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 91 | else |
stanley1228 | 8:37f5a7219fe6 | 92 | pc.printf("CCW_ANGLE_LIMIT=%x\n",ccw_angle_lim); |
stanley1228 | 8:37f5a7219fe6 | 93 | |
stanley1228 | 8:37f5a7219fe6 | 94 | |
stanley1228 | 8:37f5a7219fe6 | 95 | //multi turn offset==// |
stanley1228 | 8:37f5a7219fe6 | 96 | multi_turn_offset=dxl_read_word(i,MULTITURN_OFFSET); |
stanley1228 | 8:37f5a7219fe6 | 97 | txrx_result = dxl_get_result(); |
stanley1228 | 8:37f5a7219fe6 | 98 | if(txrx_result!=COMM_RXSUCCESS) |
stanley1228 | 8:37f5a7219fe6 | 99 | pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result); |
stanley1228 | 8:37f5a7219fe6 | 100 | else |
stanley1228 | 8:37f5a7219fe6 | 101 | pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset); |
stanley1228 | 8:37f5a7219fe6 | 102 | } |
stanley1228 | 8:37f5a7219fe6 | 103 | |
stanley1228 | 8:37f5a7219fe6 | 104 | return 0; |
stanley1228 | 8:37f5a7219fe6 | 105 | } |
stanley1228 | 8:37f5a7219fe6 | 106 | |
stanley1228 | 9:54710454ce60 | 107 | int Output_to_Dynamixel(float *Ang_rad) |
stanley1228 | 9:54710454ce60 | 108 | { |
stanley1228 | 9:54710454ce60 | 109 | //========================// |
stanley1228 | 9:54710454ce60 | 110 | //==trnasformat to pulse==// |
stanley1228 | 9:54710454ce60 | 111 | //========================// |
stanley1228 | 9:54710454ce60 | 112 | short int Ang_pulse[MAX_AXIS_NUM]={0}; |
stanley1228 | 9:54710454ce60 | 113 | short int i=0; |
stanley1228 | 9:54710454ce60 | 114 | for(i=0;i<MAX_AXIS_NUM;i++) |
stanley1228 | 9:54710454ce60 | 115 | { |
stanley1228 | 9:54710454ce60 | 116 | Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse |
stanley1228 | 9:54710454ce60 | 117 | } |
stanley1228 | 9:54710454ce60 | 118 | |
stanley1228 | 9:54710454ce60 | 119 | //===================================================================// |
stanley1228 | 9:54710454ce60 | 120 | //==limit axis if not zero ,the return value is the overlimit axis==// |
stanley1228 | 9:54710454ce60 | 121 | //===================================================================// |
stanley1228 | 9:54710454ce60 | 122 | static int Axis_Pulse_lim_H[MAX_AXIS_NUM]= |
stanley1228 | 9:54710454ce60 | 123 | { |
stanley1228 | 9:54710454ce60 | 124 | AXIS1_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 125 | AXIS2_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 126 | AXIS3_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 127 | AXIS4_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 128 | AXIS5_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 129 | AXIS6_PULSE_LIM_H, |
stanley1228 | 9:54710454ce60 | 130 | AXIS7_PULSE_LIM_H}; |
stanley1228 | 9:54710454ce60 | 131 | |
stanley1228 | 9:54710454ce60 | 132 | static int Axis_Pulse_lim_L[MAX_AXIS_NUM]= |
stanley1228 | 9:54710454ce60 | 133 | { |
stanley1228 | 9:54710454ce60 | 134 | AXIS1_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 135 | AXIS2_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 136 | AXIS3_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 137 | AXIS4_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 138 | AXIS5_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 139 | AXIS6_PULSE_LIM_L, |
stanley1228 | 9:54710454ce60 | 140 | AXIS7_PULSE_LIM_L}; |
stanley1228 | 9:54710454ce60 | 141 | |
stanley1228 | 9:54710454ce60 | 142 | for(i=0;i<MAX_AXIS_NUM;i++) |
stanley1228 | 9:54710454ce60 | 143 | { |
stanley1228 | 9:54710454ce60 | 144 | if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) ) |
stanley1228 | 9:54710454ce60 | 145 | { |
stanley1228 | 9:54710454ce60 | 146 | pc.printf("over limit Ang_pulse[%d]=%d,Axis_Pulse_lim_H[%d]=%d,Axis_Pulse_lim_L[%d]=%d\n",i,Ang_pulse[i],i,Axis_Pulse_lim_H[i],i,Axis_Pulse_lim_L[i]); |
stanley1228 | 9:54710454ce60 | 147 | return i+1; |
stanley1228 | 9:54710454ce60 | 148 | } |
stanley1228 | 9:54710454ce60 | 149 | } |
stanley1228 | 9:54710454ce60 | 150 | |
stanley1228 | 9:54710454ce60 | 151 | //================================// |
stanley1228 | 9:54710454ce60 | 152 | //==output to motor by syncpage===// |
stanley1228 | 9:54710454ce60 | 153 | //===============================// |
stanley1228 | 9:54710454ce60 | 154 | short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30}; |
stanley1228 | 9:54710454ce60 | 155 | |
stanley1228 | 9:54710454ce60 | 156 | short int SyncPage1[21]= |
stanley1228 | 9:54710454ce60 | 157 | { |
stanley1228 | 9:54710454ce60 | 158 | ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity |
stanley1228 | 9:54710454ce60 | 159 | ID_AXIS2,Ang_pulse[1],velocity[1], |
stanley1228 | 9:54710454ce60 | 160 | ID_AXIS3,Ang_pulse[2],velocity[2], |
stanley1228 | 9:54710454ce60 | 161 | ID_AXIS4,Ang_pulse[3],velocity[3], |
stanley1228 | 9:54710454ce60 | 162 | ID_AXIS5,Ang_pulse[4],velocity[4], |
stanley1228 | 9:54710454ce60 | 163 | ID_AXIS6,Ang_pulse[5],velocity[5], |
stanley1228 | 9:54710454ce60 | 164 | ID_AXIS7,Ang_pulse[6],velocity[6], |
stanley1228 | 9:54710454ce60 | 165 | }; |
stanley1228 | 9:54710454ce60 | 166 | |
stanley1228 | 9:54710454ce60 | 167 | for(i=0;i<MAX_AXIS_NUM;i++) |
stanley1228 | 9:54710454ce60 | 168 | pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]); |
stanley1228 | 9:54710454ce60 | 169 | |
stanley1228 | 9:54710454ce60 | 170 | |
stanley1228 | 9:54710454ce60 | 171 | |
stanley1228 | 9:54710454ce60 | 172 | syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length); |
stanley1228 | 9:54710454ce60 | 173 | |
stanley1228 | 9:54710454ce60 | 174 | return 0; |
stanley1228 | 9:54710454ce60 | 175 | } |
stanley1228 | 9:54710454ce60 | 176 | |
stanley1228 | 0:1780ffc33286 | 177 | int main() |
stanley1228 | 0:1780ffc33286 | 178 | { |
stanley1228 | 9:54710454ce60 | 179 | int rt =0; |
stanley1228 | 0:1780ffc33286 | 180 | |
stanley1228 | 9:54710454ce60 | 181 | //==================// |
stanley1228 | 9:54710454ce60 | 182 | //==dxl_initialize==// |
stanley1228 | 9:54710454ce60 | 183 | //===================// |
stanley1228 | 0:1780ffc33286 | 184 | pc.printf("start\n",rt); |
stanley1228 | 0:1780ffc33286 | 185 | rt=dxl_initialize( 1, 1); |
stanley1228 | 0:1780ffc33286 | 186 | pc.printf("dxl_initialize rt=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 187 | |
stanley1228 | 8:37f5a7219fe6 | 188 | |
stanley1228 | 9:54710454ce60 | 189 | //=========================// |
stanley1228 | 8:37f5a7219fe6 | 190 | //==ROM parameter setting==// |
stanley1228 | 9:54710454ce60 | 191 | //=========================// |
stanley1228 | 8:37f5a7219fe6 | 192 | //rt=ROM_Setting(); |
stanley1228 | 8:37f5a7219fe6 | 193 | |
stanley1228 | 8:37f5a7219fe6 | 194 | |
stanley1228 | 9:54710454ce60 | 195 | //myled = 0; // LED is ON |
stanley1228 | 8:37f5a7219fe6 | 196 | |
stanley1228 | 9:54710454ce60 | 197 | float Ang_rad[MAX_AXIS_NUM]={0}; |
stanley1228 | 0:1780ffc33286 | 198 | while(1) |
stanley1228 | 0:1780ffc33286 | 199 | { |
stanley1228 | 10:328cc5179ffb | 200 | //==Test Output_to_Dynamixel==// |
stanley1228 | 10:328cc5179ffb | 201 | |
stanley1228 | 10:328cc5179ffb | 202 | Ang_rad[4]=90*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 203 | Ang_rad[5]=90*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 204 | Ang_rad[6]=30*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 205 | |
stanley1228 | 10:328cc5179ffb | 206 | rt=Output_to_Dynamixel(Ang_rad); |
stanley1228 | 10:328cc5179ffb | 207 | pc.printf("Output_to_Dynamixel rt=%d",rt); |
stanley1228 | 10:328cc5179ffb | 208 | |
stanley1228 | 10:328cc5179ffb | 209 | while(mybutton); |
stanley1228 | 10:328cc5179ffb | 210 | |
stanley1228 | 10:328cc5179ffb | 211 | Ang_rad[4]=0*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 212 | Ang_rad[5]=0*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 213 | Ang_rad[6]=0*DEF_PI/180; |
stanley1228 | 10:328cc5179ffb | 214 | |
stanley1228 | 10:328cc5179ffb | 215 | rt=Output_to_Dynamixel(Ang_rad); |
stanley1228 | 10:328cc5179ffb | 216 | pc.printf("Output_to_Dynamixel rt=%d",rt); |
stanley1228 | 10:328cc5179ffb | 217 | |
stanley1228 | 10:328cc5179ffb | 218 | while(mybutton); |
stanley1228 | 10:328cc5179ffb | 219 | |
stanley1228 | 3:8a9407817ba9 | 220 | //pc.printf("before=%d\n",rt); |
stanley1228 | 2:fb9508744bc4 | 221 | |
stanley1228 | 4:53ef39fbf9d9 | 222 | |
stanley1228 | 10:328cc5179ffb | 223 | //===Test move==// |
stanley1228 | 2:fb9508744bc4 | 224 | //dxl_write_word(3,GOAL_POSITION,400); |
stanley1228 | 3:8a9407817ba9 | 225 | //setPosition(3,2048,10); |
stanley1228 | 3:8a9407817ba9 | 226 | //pc.printf("after=%d\n",rt); |
stanley1228 | 0:1780ffc33286 | 227 | //myled = 1; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 228 | //wait(4); |
stanley1228 | 2:fb9508744bc4 | 229 | //dxl_write_word(3,GOAL_POSITION,-400); |
stanley1228 | 3:8a9407817ba9 | 230 | //setPosition(3,-2048,10); |
stanley1228 | 0:1780ffc33286 | 231 | //myled = 0; // LED is ON |
stanley1228 | 3:8a9407817ba9 | 232 | //wait(4); |
stanley1228 | 3:8a9407817ba9 | 233 | |
stanley1228 | 4:53ef39fbf9d9 | 234 | |
stanley1228 | 4:53ef39fbf9d9 | 235 | //===Test read pos====// |
stanley1228 | 4:53ef39fbf9d9 | 236 | /*pos=dxl_read_word(3,PRESENT_POS); |
stanley1228 | 3:8a9407817ba9 | 237 | pc.printf("pos=%d\n",pos); |
stanley1228 | 4:53ef39fbf9d9 | 238 | wait_ms(200); */ |
stanley1228 | 3:8a9407817ba9 | 239 | |
stanley1228 | 4:53ef39fbf9d9 | 240 | //====Test read all pos=== |
stanley1228 | 10:328cc5179ffb | 241 | //static short int pos=0; |
stanley1228 | 10:328cc5179ffb | 242 | //int i=0; |
stanley1228 | 10:328cc5179ffb | 243 | //for(i=ID_AXIS1;i<=ID_AXIS7;i++) |
stanley1228 | 10:328cc5179ffb | 244 | //{ |
stanley1228 | 10:328cc5179ffb | 245 | // pos = dxl_read_word(i, PRESENT_POS); |
stanley1228 | 10:328cc5179ffb | 246 | // if(dxl_get_result()!=COMM_RXSUCCESS) |
stanley1228 | 10:328cc5179ffb | 247 | // pos=-1; |
stanley1228 | 4:53ef39fbf9d9 | 248 | |
stanley1228 | 10:328cc5179ffb | 249 | // pc.printf("X%d=%d,",i,pos); |
stanley1228 | 10:328cc5179ffb | 250 | //} |
stanley1228 | 10:328cc5179ffb | 251 | //pc.printf("\n"); |
stanley1228 | 7:15f0494813f7 | 252 | |
stanley1228 | 7:15f0494813f7 | 253 | |
stanley1228 | 9:54710454ce60 | 254 | |
stanley1228 | 9:54710454ce60 | 255 | |
stanley1228 | 7:15f0494813f7 | 256 | |
stanley1228 | 7:15f0494813f7 | 257 | //====Test syncWrite_u16base=== |
stanley1228 | 9:54710454ce60 | 258 | /*static short int pos3=-100; |
stanley1228 | 7:15f0494813f7 | 259 | pos3=(pos3==-100)? 100 :-100; |
stanley1228 | 7:15f0494813f7 | 260 | |
stanley1228 | 7:15f0494813f7 | 261 | short int SyncPage1[21]= |
stanley1228 | 7:15f0494813f7 | 262 | { |
stanley1228 | 7:15f0494813f7 | 263 | ID_AXIS1,100,5, |
stanley1228 | 7:15f0494813f7 | 264 | ID_AXIS2,100,5, |
stanley1228 | 7:15f0494813f7 | 265 | ID_AXIS3,100,5, |
stanley1228 | 7:15f0494813f7 | 266 | ID_AXIS4,100,5, |
stanley1228 | 7:15f0494813f7 | 267 | ID_AXIS5,100,5, |
stanley1228 | 7:15f0494813f7 | 268 | ID_AXIS6,100,5, |
stanley1228 | 7:15f0494813f7 | 269 | ID_AXIS7,100,5, |
stanley1228 | 7:15f0494813f7 | 270 | }; |
stanley1228 | 7:15f0494813f7 | 271 | |
stanley1228 | 9:54710454ce60 | 272 | wait(0.5); */ |
stanley1228 | 7:15f0494813f7 | 273 | //short int SyncPage1[21]=//Test use |
stanley1228 | 7:15f0494813f7 | 274 | //{ |
stanley1228 | 7:15f0494813f7 | 275 | // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 |
stanley1228 | 7:15f0494813f7 | 276 | // 0x01,0x020,0x360, |
stanley1228 | 7:15f0494813f7 | 277 | // 0x02,0x030,0x170, |
stanley1228 | 7:15f0494813f7 | 278 | // 0x03,0x220,0x380, |
stanley1228 | 7:15f0494813f7 | 279 | // 0x05,0x123,0x121, |
stanley1228 | 7:15f0494813f7 | 280 | // 0x06,0x234,0x143, |
stanley1228 | 7:15f0494813f7 | 281 | // 0x07,0x145,0x167 |
stanley1228 | 7:15f0494813f7 | 282 | //}; |
stanley1228 | 7:15f0494813f7 | 283 | |
stanley1228 | 9:54710454ce60 | 284 | //syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; |
stanley1228 | 7:15f0494813f7 | 285 | |
stanley1228 | 7:15f0494813f7 | 286 | } |
stanley1228 | 4:53ef39fbf9d9 | 287 | } |