hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

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?

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