hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

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?

UserRevisionLine numberNew 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 }