hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@11:644c13da326d, 2017-02-10 (annotated)

Committer:
stanley1228
Date:
Fri Feb 10 22:56:11 2017 +0800
Revision:
11:644c13da326d
Parent:
10:328cc5179ffb
Child:
12:1eeea035bf87
1.lib no change

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 11:644c13da326d 5 #define DEBUG 1
stanley1228 11:644c13da326d 6
stanley1228 11:644c13da326d 7
stanley1228 11:644c13da326d 8 #if (DEBUG)
stanley1228 11:644c13da326d 9 #define DBGMSG(x) pc.printf x;
stanley1228 11:644c13da326d 10 #else
stanley1228 11:644c13da326d 11 #define DBGMSG(x)
stanley1228 11:644c13da326d 12 #endif
stanley1228 11:644c13da326d 13
stanley1228 11:644c13da326d 14
stanley1228 11:644c13da326d 15
stanley1228 11:644c13da326d 16 #define MAX_AXIS_NUM 7
stanley1228 11:644c13da326d 17 enum{
stanley1228 11:644c13da326d 18 Index_AXIS1=0,
stanley1228 11:644c13da326d 19 Index_AXIS2,
stanley1228 11:644c13da326d 20 Index_AXIS3,
stanley1228 11:644c13da326d 21 Index_AXIS4,
stanley1228 11:644c13da326d 22 Index_AXIS5,
stanley1228 11:644c13da326d 23 Index_AXIS6,
stanley1228 11:644c13da326d 24 Index_AXIS7
stanley1228 11:644c13da326d 25 };
stanley1228 11:644c13da326d 26
stanley1228 11:644c13da326d 27 enum{
stanley1228 11:644c13da326d 28 ID_AXIS1=1,
stanley1228 11:644c13da326d 29 ID_AXIS2,
stanley1228 11:644c13da326d 30 ID_AXIS3,
stanley1228 11:644c13da326d 31 ID_AXIS4,
stanley1228 11:644c13da326d 32 ID_AXIS5,
stanley1228 11:644c13da326d 33 ID_AXIS6,
stanley1228 11:644c13da326d 34 ID_AXIS7
stanley1228 11:644c13da326d 35 };
stanley1228 11:644c13da326d 36
stanley1228 11:644c13da326d 37 enum{
stanley1228 11:644c13da326d 38 NO_AXIS1=1,
stanley1228 11:644c13da326d 39 NO_AXIS2,
stanley1228 11:644c13da326d 40 NO_AXIS3,
stanley1228 11:644c13da326d 41 NO_AXIS4,
stanley1228 11:644c13da326d 42 NO_AXIS5,
stanley1228 11:644c13da326d 43 NO_AXIS6,
stanley1228 11:644c13da326d 44 NO_AXIS7
stanley1228 11:644c13da326d 45 };
stanley1228 11:644c13da326d 46
stanley1228 11:644c13da326d 47
stanley1228 11:644c13da326d 48 static const unsigned char gMapAxisNO[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 49 {
stanley1228 11:644c13da326d 50 NO_AXIS1,
stanley1228 11:644c13da326d 51 NO_AXIS2,
stanley1228 11:644c13da326d 52 NO_AXIS3,
stanley1228 11:644c13da326d 53 NO_AXIS4,
stanley1228 11:644c13da326d 54 NO_AXIS5,
stanley1228 11:644c13da326d 55 NO_AXIS6,
stanley1228 11:644c13da326d 56 NO_AXIS7
stanley1228 11:644c13da326d 57 };
stanley1228 11:644c13da326d 58 static const unsigned char gMapAxisID[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 59 {
stanley1228 11:644c13da326d 60 ID_AXIS1,
stanley1228 11:644c13da326d 61 ID_AXIS2,
stanley1228 11:644c13da326d 62 ID_AXIS3,
stanley1228 11:644c13da326d 63 ID_AXIS4,
stanley1228 11:644c13da326d 64 ID_AXIS5,
stanley1228 11:644c13da326d 65 ID_AXIS6,
stanley1228 11:644c13da326d 66 ID_AXIS7
stanley1228 11:644c13da326d 67 };
stanley1228 11:644c13da326d 68 //#define AXIS1_PULSE_LIM_L (-910)
stanley1228 11:644c13da326d 69 //#define AXIS1_PULSE_LIM_H 2048
stanley1228 11:644c13da326d 70 //#define AXIS2_PULSE_LIM_L (-205)
stanley1228 11:644c13da326d 71 //#define AXIS2_PULSE_LIM_H 2048
stanley1228 11:644c13da326d 72 //#define AXIS3_PULSE_LIM_L (-2840)
stanley1228 11:644c13da326d 73 //#define AXIS3_PULSE_LIM_H 1190
stanley1228 11:644c13da326d 74 //#define AXIS4_PULSE_LIM_L 0//need to test
stanley1228 11:644c13da326d 75 //#define AXIS4_PULSE_LIM_H 0//need to test
stanley1228 11:644c13da326d 76 //#define AXIS5_PULSE_LIM_L (-2048)
stanley1228 11:644c13da326d 77 //#define AXIS5_PULSE_LIM_H 1680
stanley1228 11:644c13da326d 78 //#define AXIS6_PULSE_LIM_L (-680)
stanley1228 11:644c13da326d 79 //#define AXIS6_PULSE_LIM_H 1024
stanley1228 11:644c13da326d 80 //#define AXIS7_PULSE_LIM_L (-420)
stanley1228 11:644c13da326d 81 //#define AXIS7_PULSE_LIM_H 420
stanley1228 11:644c13da326d 82
stanley1228 11:644c13da326d 83 enum{
stanley1228 11:644c13da326d 84 DEF_UNIT_RAD=1,
stanley1228 11:644c13da326d 85 DEF_UNIT_DEG,
stanley1228 11:644c13da326d 86 DEF_UNIT_PUS
stanley1228 11:644c13da326d 87 };
stanley1228 11:644c13da326d 88
stanley1228 11:644c13da326d 89
stanley1228 11:644c13da326d 90 #define DEF_PI (3.1415F)
stanley1228 11:644c13da326d 91 #define DEF_RATIO_PUS_TO_DEG (0.0879F) //360/4096
stanley1228 11:644c13da326d 92 #define DEF_RATIO_PUS_TO_RAD (0.0015F) //2pi/4096 0.00153398078788564122971808758949
stanley1228 11:644c13da326d 93 #define DEF_RATIO_DEG_TO_PUS (11.3778F) //4096/360
stanley1228 11:644c13da326d 94 #define DEF_RATIO_DEG_TO_RAD (DEF_PI/180) //pi/180 0.01745329251994329576923690768489 (0.0175F)
stanley1228 11:644c13da326d 95 #define DEF_RATIO_RAD_TO_PUS (651.8986F) //4096/2pi 651.89864690440329530934789477382
stanley1228 11:644c13da326d 96
stanley1228 11:644c13da326d 97 //==robot to Motor offset==// robot pos=motor position -M2R_OFFSET
stanley1228 11:644c13da326d 98 #define AXIS1_R2M_OFFSET_DEG 90
stanley1228 11:644c13da326d 99 #define AXIS2_R2M_OFFSET_DEG 90
stanley1228 11:644c13da326d 100 #define AXIS3_R2M_OFFSET_DEG 225
stanley1228 11:644c13da326d 101 #define AXIS4_R2M_OFFSET_DEG 90
stanley1228 11:644c13da326d 102 #define AXIS5_R2M_OFFSET_DEG 180
stanley1228 11:644c13da326d 103 #define AXIS6_R2M_OFFSET_DEG 90
stanley1228 11:644c13da326d 104 #define AXIS7_R2M_OFFSET_DEG 90
stanley1228 11:644c13da326d 105
stanley1228 11:644c13da326d 106 static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 107 {
stanley1228 11:644c13da326d 108 (unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 109 (unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 110 (unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 111 (unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 112 (unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 113 (unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 11:644c13da326d 114 (unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS)
stanley1228 11:644c13da326d 115 };
stanley1228 11:644c13da326d 116
stanley1228 11:644c13da326d 117
stanley1228 11:644c13da326d 118 //==robot angle limit==//
stanley1228 11:644c13da326d 119 #define AXIS1_ROBOT_LIM_DEG_L (-80)
stanley1228 11:644c13da326d 120 #define AXIS1_ROBOT_LIM_DEG_H 180
stanley1228 11:644c13da326d 121 #define AXIS2_ROBOT_LIM_DEG_L (-18)
stanley1228 11:644c13da326d 122 #define AXIS2_ROBOT_LIM_DEG_H 180
stanley1228 11:644c13da326d 123 #define AXIS3_ROBOT_LIM_DEG_L (-225)
stanley1228 11:644c13da326d 124 #define AXIS3_ROBOT_LIM_DEG_H 105
stanley1228 11:644c13da326d 125 #define AXIS4_ROBOT_LIM_DEG_L 0 //need to test
stanley1228 11:644c13da326d 126 #define AXIS4_ROBOT_LIM_DEG_H 114 //need to test
stanley1228 11:644c13da326d 127 #define AXIS5_ROBOT_LIM_DEG_L (-180)
stanley1228 11:644c13da326d 128 #define AXIS5_ROBOT_LIM_DEG_H 148
stanley1228 11:644c13da326d 129 #define AXIS6_ROBOT_LIM_DEG_L (-60)
stanley1228 11:644c13da326d 130 #define AXIS6_ROBOT_LIM_DEG_H 90
stanley1228 11:644c13da326d 131 #define AXIS7_ROBOT_LIM_DEG_L (-30)
stanley1228 11:644c13da326d 132 #define AXIS7_ROBOT_LIM_DEG_H 30
stanley1228 11:644c13da326d 133
stanley1228 11:644c13da326d 134 static const float grobot_lim_rad_L[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 135 {
stanley1228 11:644c13da326d 136 AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 137 AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 138 AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 139 AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 140 AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 141 AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 142 AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD
stanley1228 11:644c13da326d 143 };
stanley1228 11:644c13da326d 144
stanley1228 11:644c13da326d 145 static const float grobot_lim_rad_H[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 146 {
stanley1228 11:644c13da326d 147 AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 148 AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 149 AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 150 AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 151 AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 152 AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 11:644c13da326d 153 AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD
stanley1228 11:644c13da326d 154 };
stanley1228 11:644c13da326d 155
stanley1228 11:644c13da326d 156
stanley1228 4:53ef39fbf9d9 157
stanley1228 4:53ef39fbf9d9 158
stanley1228 11:644c13da326d 159 //==robot angle limit==//
stanley1228 11:644c13da326d 160 #define AXIS1_MAX_TORQUE 55
stanley1228 11:644c13da326d 161 #define AXIS2_MAX_TORQUE 55
stanley1228 11:644c13da326d 162 #define AXIS3_MAX_TORQUE 55
stanley1228 11:644c13da326d 163 #define AXIS4_MAX_TORQUE 55
stanley1228 11:644c13da326d 164 #define AXIS5_MAX_TORQUE 55
stanley1228 11:644c13da326d 165 #define AXIS6_MAX_TORQUE 55
stanley1228 11:644c13da326d 166 #define AXIS7_MAX_TORQUE 55
stanley1228 4:53ef39fbf9d9 167
stanley1228 11:644c13da326d 168 //==morot max pulse in joint mode==//
stanley1228 11:644c13da326d 169 #define DEF_JOINT_MODE_MAX_PULSE 4095
stanley1228 11:644c13da326d 170 #define DEF_JOINT_MODE_MIN_PULSE 0
stanley1228 4:53ef39fbf9d9 171
stanley1228 0:1780ffc33286 172
stanley1228 9:54710454ce60 173
stanley1228 9:54710454ce60 174 //==Pin define==//
stanley1228 0:1780ffc33286 175 DigitalOut myled(LED1);
stanley1228 0:1780ffc33286 176 Serial pc(SERIAL_TX, SERIAL_RX);
stanley1228 10:328cc5179ffb 177 DigitalIn mybutton(USER_BUTTON);
stanley1228 0:1780ffc33286 178
stanley1228 11:644c13da326d 179 int ROM_Setting() //new
stanley1228 8:37f5a7219fe6 180 {
stanley1228 11:644c13da326d 181 //==calculate Max torque to set in rom
stanley1228 11:644c13da326d 182 const short int Max_torque[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 183 {
stanley1228 11:644c13da326d 184 AXIS1_MAX_TORQUE,
stanley1228 11:644c13da326d 185 AXIS2_MAX_TORQUE,
stanley1228 11:644c13da326d 186 AXIS3_MAX_TORQUE,
stanley1228 11:644c13da326d 187 AXIS4_MAX_TORQUE,
stanley1228 11:644c13da326d 188 AXIS5_MAX_TORQUE,
stanley1228 11:644c13da326d 189 AXIS6_MAX_TORQUE,
stanley1228 11:644c13da326d 190 AXIS7_MAX_TORQUE
stanley1228 11:644c13da326d 191 };
stanley1228 11:644c13da326d 192
stanley1228 8:37f5a7219fe6 193
stanley1228 11:644c13da326d 194 //==Calculate angle limit==//
stanley1228 11:644c13da326d 195 const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 196 {
stanley1228 11:644c13da326d 197 AXIS1_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 198 AXIS2_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 199 AXIS3_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 200 AXIS4_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 201 AXIS5_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 202 AXIS6_R2M_OFFSET_DEG,
stanley1228 11:644c13da326d 203 AXIS7_R2M_OFFSET_DEG
stanley1228 11:644c13da326d 204 };
stanley1228 11:644c13da326d 205
stanley1228 11:644c13da326d 206 const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]=
stanley1228 8:37f5a7219fe6 207 {
stanley1228 11:644c13da326d 208 AXIS1_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 209 AXIS2_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 210 AXIS3_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 211 AXIS4_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 212 AXIS5_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 213 AXIS6_ROBOT_LIM_DEG_L,
stanley1228 11:644c13da326d 214 AXIS7_ROBOT_LIM_DEG_L
stanley1228 11:644c13da326d 215 };
stanley1228 11:644c13da326d 216
stanley1228 11:644c13da326d 217 const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]=
stanley1228 11:644c13da326d 218 {
stanley1228 11:644c13da326d 219 AXIS1_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 220 AXIS2_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 221 AXIS3_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 222 AXIS4_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 223 AXIS5_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 224 AXIS6_ROBOT_LIM_DEG_H,
stanley1228 11:644c13da326d 225 AXIS7_ROBOT_LIM_DEG_H
stanley1228 11:644c13da326d 226 };
stanley1228 11:644c13da326d 227
stanley1228 11:644c13da326d 228 unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 229 unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0};
stanley1228 8:37f5a7219fe6 230
stanley1228 11:644c13da326d 231
stanley1228 11:644c13da326d 232 int i=0;
stanley1228 11:644c13da326d 233 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 234 {
stanley1228 11:644c13da326d 235 Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
stanley1228 11:644c13da326d 236 Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
stanley1228 11:644c13da326d 237 }
stanley1228 11:644c13da326d 238
stanley1228 11:644c13da326d 239
stanley1228 8:37f5a7219fe6 240
stanley1228 11:644c13da326d 241 //==writing to ROM==//
stanley1228 11:644c13da326d 242 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 243 {
stanley1228 11:644c13da326d 244 //==Set MAX_torgue==//
stanley1228 11:644c13da326d 245 dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);// more safe
stanley1228 11:644c13da326d 246
stanley1228 11:644c13da326d 247 //==Set angel limit==//
stanley1228 11:644c13da326d 248 dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]);
stanley1228 11:644c13da326d 249 dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]);
stanley1228 11:644c13da326d 250
stanley1228 11:644c13da326d 251 //==Set MULTITURN_OFFSET to 0==//
stanley1228 11:644c13da326d 252 dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0);
stanley1228 8:37f5a7219fe6 253 }
stanley1228 8:37f5a7219fe6 254
stanley1228 8:37f5a7219fe6 255
stanley1228 8:37f5a7219fe6 256 //==read and check ==//
stanley1228 8:37f5a7219fe6 257 int txrx_result=0;
stanley1228 8:37f5a7219fe6 258 short int max_torque=0;
stanley1228 8:37f5a7219fe6 259 short int cw_angel_lim=0,ccw_angle_lim=0;
stanley1228 8:37f5a7219fe6 260 short int multi_turn_offset=0;
stanley1228 11:644c13da326d 261 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 8:37f5a7219fe6 262 {
stanley1228 11:644c13da326d 263 pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);
stanley1228 8:37f5a7219fe6 264
stanley1228 11:644c13da326d 265 //==MAX_torgue==//
stanley1228 11:644c13da326d 266 max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE);
stanley1228 8:37f5a7219fe6 267 txrx_result = dxl_get_result();
stanley1228 8:37f5a7219fe6 268 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 8:37f5a7219fe6 269 pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result);
stanley1228 8:37f5a7219fe6 270 else
stanley1228 8:37f5a7219fe6 271 pc.printf("MAX_TORQUE=%d\n",max_torque);
stanley1228 8:37f5a7219fe6 272
stanley1228 11:644c13da326d 273 //==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==//
stanley1228 11:644c13da326d 274 cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L);
stanley1228 8:37f5a7219fe6 275 txrx_result = dxl_get_result();
stanley1228 8:37f5a7219fe6 276 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 8:37f5a7219fe6 277 pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result);
stanley1228 8:37f5a7219fe6 278 else
stanley1228 11:644c13da326d 279 pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG);
stanley1228 8:37f5a7219fe6 280
stanley1228 11:644c13da326d 281 ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L);
stanley1228 8:37f5a7219fe6 282 txrx_result = dxl_get_result();
stanley1228 8:37f5a7219fe6 283 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 8:37f5a7219fe6 284 pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result);
stanley1228 8:37f5a7219fe6 285 else
stanley1228 11:644c13da326d 286 pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG);
stanley1228 8:37f5a7219fe6 287
stanley1228 8:37f5a7219fe6 288
stanley1228 11:644c13da326d 289 //==multi turn offset==//
stanley1228 11:644c13da326d 290 multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET);
stanley1228 8:37f5a7219fe6 291 txrx_result = dxl_get_result();
stanley1228 8:37f5a7219fe6 292 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 8:37f5a7219fe6 293 pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result);
stanley1228 8:37f5a7219fe6 294 else
stanley1228 8:37f5a7219fe6 295 pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
stanley1228 8:37f5a7219fe6 296 }
stanley1228 8:37f5a7219fe6 297
stanley1228 8:37f5a7219fe6 298 return 0;
stanley1228 8:37f5a7219fe6 299 }
stanley1228 8:37f5a7219fe6 300
stanley1228 11:644c13da326d 301
stanley1228 11:644c13da326d 302
stanley1228 11:644c13da326d 303
stanley1228 11:644c13da326d 304 int Read_pos(float *pos,unsigned char unit)
stanley1228 11:644c13da326d 305 {
stanley1228 11:644c13da326d 306 int i=0;
stanley1228 11:644c13da326d 307 short int pulse=0;
stanley1228 11:644c13da326d 308 int rt=0;
stanley1228 11:644c13da326d 309
stanley1228 11:644c13da326d 310 if(unit==DEF_UNIT_RAD)
stanley1228 11:644c13da326d 311 {
stanley1228 11:644c13da326d 312 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 313 {
stanley1228 11:644c13da326d 314 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 11:644c13da326d 315 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 11:644c13da326d 316 {
stanley1228 11:644c13da326d 317 rt=-gMapAxisNO[i];
stanley1228 11:644c13da326d 318 pos[i]=0xffff;
stanley1228 11:644c13da326d 319 }
stanley1228 11:644c13da326d 320 else
stanley1228 11:644c13da326d 321 {
stanley1228 11:644c13da326d 322 pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset
stanley1228 11:644c13da326d 323 pos[i]=pulse*DEF_RATIO_PUS_TO_RAD;
stanley1228 11:644c13da326d 324 }
stanley1228 11:644c13da326d 325 }
stanley1228 11:644c13da326d 326
stanley1228 11:644c13da326d 327 }
stanley1228 11:644c13da326d 328 else if(unit==DEF_UNIT_DEG)
stanley1228 11:644c13da326d 329 {
stanley1228 11:644c13da326d 330 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 331 {
stanley1228 11:644c13da326d 332 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 11:644c13da326d 333 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 11:644c13da326d 334 {
stanley1228 11:644c13da326d 335 rt=-gMapAxisNO[i];
stanley1228 11:644c13da326d 336 pos[i]=0xffff;
stanley1228 11:644c13da326d 337 }
stanley1228 11:644c13da326d 338 else
stanley1228 11:644c13da326d 339 {
stanley1228 11:644c13da326d 340 pulse-=gr2m_offset_pulse[i];
stanley1228 11:644c13da326d 341 pos[i]=pulse*DEF_RATIO_PUS_TO_DEG;
stanley1228 11:644c13da326d 342 }
stanley1228 11:644c13da326d 343 }
stanley1228 11:644c13da326d 344 }
stanley1228 11:644c13da326d 345 else if(unit==DEF_UNIT_PUS)
stanley1228 11:644c13da326d 346 {
stanley1228 11:644c13da326d 347 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 348 {
stanley1228 11:644c13da326d 349 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 11:644c13da326d 350 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 11:644c13da326d 351 {
stanley1228 11:644c13da326d 352 rt=-gMapAxisNO[i];
stanley1228 11:644c13da326d 353 pos[i]=0xffff;
stanley1228 11:644c13da326d 354 }
stanley1228 11:644c13da326d 355 else
stanley1228 11:644c13da326d 356 {
stanley1228 11:644c13da326d 357 pulse-=gr2m_offset_pulse[i];
stanley1228 11:644c13da326d 358 pos[i]=pulse;
stanley1228 11:644c13da326d 359 }
stanley1228 11:644c13da326d 360 }
stanley1228 11:644c13da326d 361
stanley1228 11:644c13da326d 362 }
stanley1228 11:644c13da326d 363 else
stanley1228 11:644c13da326d 364 {
stanley1228 11:644c13da326d 365 //non offset value
stanley1228 11:644c13da326d 366 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 367 {
stanley1228 11:644c13da326d 368 pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 11:644c13da326d 369 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 11:644c13da326d 370 {
stanley1228 11:644c13da326d 371 rt=-gMapAxisNO[i];
stanley1228 11:644c13da326d 372 pos[i]=0xffff;
stanley1228 11:644c13da326d 373 }
stanley1228 11:644c13da326d 374 }
stanley1228 11:644c13da326d 375 }
stanley1228 11:644c13da326d 376
stanley1228 11:644c13da326d 377 return rt;
stanley1228 11:644c13da326d 378 }
stanley1228 11:644c13da326d 379
stanley1228 11:644c13da326d 380
stanley1228 11:644c13da326d 381
stanley1228 11:644c13da326d 382
stanley1228 11:644c13da326d 383 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) //new
stanley1228 11:644c13da326d 384 {
stanley1228 11:644c13da326d 385 unsigned char i=0;
stanley1228 11:644c13da326d 386
stanley1228 11:644c13da326d 387 //===================================================================//
stanley1228 11:644c13da326d 388 //==limit axis if not zero ,the return value is the overlimit axis==//
stanley1228 11:644c13da326d 389 //===================================================================//
stanley1228 11:644c13da326d 390 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 391 //{
stanley1228 11:644c13da326d 392 // if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) )
stanley1228 11:644c13da326d 393 // {
stanley1228 11:644c13da326d 394 // DBGMSG(("AXIS%d over limit Ang_rad=%f,grobot_lim_rad_L=%f,grobot_lim_rad_H=%f\n",gMapAxisNO[i],Ang_rad[i],grobot_lim_rad_L[i],grobot_lim_rad_H[i]))
stanley1228 11:644c13da326d 395 // return -gMapAxisNO[i];
stanley1228 11:644c13da326d 396 // }
stanley1228 11:644c13da326d 397 //}
stanley1228 11:644c13da326d 398
stanley1228 11:644c13da326d 399 //===================================================//
stanley1228 11:644c13da326d 400 //==trnasformat to pulse and offset to motor domain==//
stanley1228 11:644c13da326d 401 //====================================================//
stanley1228 11:644c13da326d 402 short int Ang_pulse[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 403 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 404 {
stanley1228 11:644c13da326d 405 Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS);
stanley1228 11:644c13da326d 406 Ang_pulse[i]+=gr2m_offset_pulse[i];
stanley1228 11:644c13da326d 407
stanley1228 11:644c13da326d 408 if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )// 0~4095
stanley1228 11:644c13da326d 409 {
stanley1228 11:644c13da326d 410 DBGMSG(("AXIS%d over range of mortor Ang_pulse=%d,JOINT_MODE_MIN_PULSE=%d,JOINT_MODE_MAX_PULSE=%d\n",gMapAxisNO[i],Ang_pulse[i],DEF_JOINT_MODE_MIN_PULSE,DEF_JOINT_MODE_MAX_PULSE))
stanley1228 11:644c13da326d 411 return -gMapAxisNO[i];
stanley1228 11:644c13da326d 412 }
stanley1228 11:644c13da326d 413 }
stanley1228 11:644c13da326d 414
stanley1228 11:644c13da326d 415 //================================//
stanley1228 11:644c13da326d 416 //==output to motor by syncpage===//
stanley1228 11:644c13da326d 417 //===============================//
stanley1228 11:644c13da326d 418 unsigned short int SyncPage1[21]=
stanley1228 11:644c13da326d 419 {
stanley1228 11:644c13da326d 420 ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
stanley1228 11:644c13da326d 421 ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2],
stanley1228 11:644c13da326d 422 ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3],
stanley1228 11:644c13da326d 423 ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4],
stanley1228 11:644c13da326d 424 ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5],
stanley1228 11:644c13da326d 425 ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6],
stanley1228 11:644c13da326d 426 ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7],
stanley1228 11:644c13da326d 427 };
stanley1228 11:644c13da326d 428
stanley1228 11:644c13da326d 429
stanley1228 11:644c13da326d 430 #if (DEBUG)
stanley1228 11:644c13da326d 431 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 432 pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]);
stanley1228 11:644c13da326d 433 #endif
stanley1228 11:644c13da326d 434
stanley1228 11:644c13da326d 435 syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
stanley1228 11:644c13da326d 436
stanley1228 11:644c13da326d 437 return 0;
stanley1228 11:644c13da326d 438 }
stanley1228 11:644c13da326d 439
stanley1228 11:644c13da326d 440
stanley1228 11:644c13da326d 441 /*
stanley1228 11:644c13da326d 442 int Output_to_Dynamixel(float *Ang_rad) //old
stanley1228 9:54710454ce60 443 {
stanley1228 9:54710454ce60 444 //========================//
stanley1228 9:54710454ce60 445 //==trnasformat to pulse==//
stanley1228 9:54710454ce60 446 //========================//
stanley1228 9:54710454ce60 447 short int Ang_pulse[MAX_AXIS_NUM]={0};
stanley1228 9:54710454ce60 448 short int i=0;
stanley1228 9:54710454ce60 449 for(i=0;i<MAX_AXIS_NUM;i++)
stanley1228 9:54710454ce60 450 {
stanley1228 9:54710454ce60 451 Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse
stanley1228 9:54710454ce60 452 }
stanley1228 9:54710454ce60 453
stanley1228 9:54710454ce60 454 //===================================================================//
stanley1228 9:54710454ce60 455 //==limit axis if not zero ,the return value is the overlimit axis==//
stanley1228 9:54710454ce60 456 //===================================================================//
stanley1228 9:54710454ce60 457 static int Axis_Pulse_lim_H[MAX_AXIS_NUM]=
stanley1228 9:54710454ce60 458 {
stanley1228 9:54710454ce60 459 AXIS1_PULSE_LIM_H,
stanley1228 9:54710454ce60 460 AXIS2_PULSE_LIM_H,
stanley1228 9:54710454ce60 461 AXIS3_PULSE_LIM_H,
stanley1228 9:54710454ce60 462 AXIS4_PULSE_LIM_H,
stanley1228 9:54710454ce60 463 AXIS5_PULSE_LIM_H,
stanley1228 9:54710454ce60 464 AXIS6_PULSE_LIM_H,
stanley1228 9:54710454ce60 465 AXIS7_PULSE_LIM_H};
stanley1228 9:54710454ce60 466
stanley1228 9:54710454ce60 467 static int Axis_Pulse_lim_L[MAX_AXIS_NUM]=
stanley1228 9:54710454ce60 468 {
stanley1228 9:54710454ce60 469 AXIS1_PULSE_LIM_L,
stanley1228 9:54710454ce60 470 AXIS2_PULSE_LIM_L,
stanley1228 9:54710454ce60 471 AXIS3_PULSE_LIM_L,
stanley1228 9:54710454ce60 472 AXIS4_PULSE_LIM_L,
stanley1228 9:54710454ce60 473 AXIS5_PULSE_LIM_L,
stanley1228 9:54710454ce60 474 AXIS6_PULSE_LIM_L,
stanley1228 9:54710454ce60 475 AXIS7_PULSE_LIM_L};
stanley1228 9:54710454ce60 476
stanley1228 9:54710454ce60 477 for(i=0;i<MAX_AXIS_NUM;i++)
stanley1228 9:54710454ce60 478 {
stanley1228 9:54710454ce60 479 if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) )
stanley1228 9:54710454ce60 480 {
stanley1228 9:54710454ce60 481 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 482 return i+1;
stanley1228 9:54710454ce60 483 }
stanley1228 9:54710454ce60 484 }
stanley1228 9:54710454ce60 485
stanley1228 9:54710454ce60 486 //================================//
stanley1228 9:54710454ce60 487 //==output to motor by syncpage===//
stanley1228 9:54710454ce60 488 //===============================//
stanley1228 9:54710454ce60 489 short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30};
stanley1228 9:54710454ce60 490
stanley1228 9:54710454ce60 491 short int SyncPage1[21]=
stanley1228 9:54710454ce60 492 {
stanley1228 9:54710454ce60 493 ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity
stanley1228 9:54710454ce60 494 ID_AXIS2,Ang_pulse[1],velocity[1],
stanley1228 9:54710454ce60 495 ID_AXIS3,Ang_pulse[2],velocity[2],
stanley1228 9:54710454ce60 496 ID_AXIS4,Ang_pulse[3],velocity[3],
stanley1228 9:54710454ce60 497 ID_AXIS5,Ang_pulse[4],velocity[4],
stanley1228 9:54710454ce60 498 ID_AXIS6,Ang_pulse[5],velocity[5],
stanley1228 9:54710454ce60 499 ID_AXIS7,Ang_pulse[6],velocity[6],
stanley1228 9:54710454ce60 500 };
stanley1228 9:54710454ce60 501
stanley1228 9:54710454ce60 502 for(i=0;i<MAX_AXIS_NUM;i++)
stanley1228 9:54710454ce60 503 pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]);
stanley1228 9:54710454ce60 504
stanley1228 9:54710454ce60 505
stanley1228 9:54710454ce60 506
stanley1228 9:54710454ce60 507 syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
stanley1228 9:54710454ce60 508
stanley1228 9:54710454ce60 509 return 0;
stanley1228 9:54710454ce60 510 }
stanley1228 11:644c13da326d 511 */
stanley1228 9:54710454ce60 512
stanley1228 0:1780ffc33286 513 int main()
stanley1228 0:1780ffc33286 514 {
stanley1228 9:54710454ce60 515 int rt =0;
stanley1228 11:644c13da326d 516 unsigned char i=0;
stanley1228 11:644c13da326d 517
stanley1228 9:54710454ce60 518 //==================//
stanley1228 9:54710454ce60 519 //==dxl_initialize==//
stanley1228 9:54710454ce60 520 //===================//
stanley1228 11:644c13da326d 521 DBGMSG(("start\n"))
stanley1228 11:644c13da326d 522 //pc.printf("start\n",rt);
stanley1228 0:1780ffc33286 523 rt=dxl_initialize( 1, 1);
stanley1228 11:644c13da326d 524 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 11:644c13da326d 525 //pc.printf("dxl_initialize rt=%d\n",rt);
stanley1228 0:1780ffc33286 526
stanley1228 8:37f5a7219fe6 527
stanley1228 9:54710454ce60 528 //=========================//
stanley1228 8:37f5a7219fe6 529 //==ROM parameter setting==//
stanley1228 9:54710454ce60 530 //=========================//
stanley1228 8:37f5a7219fe6 531 //rt=ROM_Setting();
stanley1228 11:644c13da326d 532 //while(1);
stanley1228 8:37f5a7219fe6 533
stanley1228 9:54710454ce60 534 //myled = 0; // LED is ON
stanley1228 8:37f5a7219fe6 535
stanley1228 9:54710454ce60 536 float Ang_rad[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 537 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 0:1780ffc33286 538 while(1)
stanley1228 0:1780ffc33286 539 {
stanley1228 10:328cc5179ffb 540 //==Test Output_to_Dynamixel==//
stanley1228 11:644c13da326d 541 //Ang_rad[Index_AXIS5]=160*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 542 //Ang_rad[Index_AXIS6]=100*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 543 //Ang_rad[Index_AXIS7]=90*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 544 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 11:644c13da326d 545 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 546
stanley1228 11:644c13da326d 547 //while(mybutton);
stanley1228 11:644c13da326d 548
stanley1228 11:644c13da326d 549 //
stanley1228 11:644c13da326d 550 //Ang_rad[Index_AXIS5]=-180*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 551 //Ang_rad[Index_AXIS6]=-70*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 552 //Ang_rad[Index_AXIS7]=-90*DEF_RATIO_DEG_TO_RAD;
stanley1228 11:644c13da326d 553 //rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 11:644c13da326d 554 //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
stanley1228 10:328cc5179ffb 555
stanley1228 11:644c13da326d 556 //while(mybutton);
stanley1228 11:644c13da326d 557
stanley1228 11:644c13da326d 558
stanley1228 10:328cc5179ffb 559
stanley1228 11:644c13da326d 560 //==Read robot pos by pos_deg==//
stanley1228 11:644c13da326d 561 float pos_deg[MAX_AXIS_NUM]={0};
stanley1228 11:644c13da326d 562 rt=Read_pos(pos_deg,DEF_UNIT_RAD);
stanley1228 11:644c13da326d 563 pc.printf("Read_pos rt==%d\n",rt);
stanley1228 10:328cc5179ffb 564
stanley1228 11:644c13da326d 565 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 11:644c13da326d 566 {
stanley1228 11:644c13da326d 567 pc.printf("X%d=%.2f,",gMapAxisNO[i],pos_deg[i]);
stanley1228 11:644c13da326d 568 }
stanley1228 11:644c13da326d 569
stanley1228 10:328cc5179ffb 570 //===Test move==//
stanley1228 2:fb9508744bc4 571 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 3:8a9407817ba9 572 //setPosition(3,2048,10);
stanley1228 3:8a9407817ba9 573 //pc.printf("after=%d\n",rt);
stanley1228 0:1780ffc33286 574 //myled = 1; // LED is ON
stanley1228 3:8a9407817ba9 575 //wait(4);
stanley1228 2:fb9508744bc4 576 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 3:8a9407817ba9 577 //setPosition(3,-2048,10);
stanley1228 0:1780ffc33286 578 //myled = 0; // LED is ON
stanley1228 3:8a9407817ba9 579 //wait(4);
stanley1228 3:8a9407817ba9 580
stanley1228 4:53ef39fbf9d9 581
stanley1228 4:53ef39fbf9d9 582 //===Test read pos====//
stanley1228 4:53ef39fbf9d9 583 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 3:8a9407817ba9 584 pc.printf("pos=%d\n",pos);
stanley1228 4:53ef39fbf9d9 585 wait_ms(200); */
stanley1228 3:8a9407817ba9 586
stanley1228 4:53ef39fbf9d9 587 //====Test read all pos===
stanley1228 10:328cc5179ffb 588 //static short int pos=0;
stanley1228 10:328cc5179ffb 589 //int i=0;
stanley1228 10:328cc5179ffb 590 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 10:328cc5179ffb 591 //{
stanley1228 10:328cc5179ffb 592 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 10:328cc5179ffb 593 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 10:328cc5179ffb 594 // pos=-1;
stanley1228 4:53ef39fbf9d9 595
stanley1228 10:328cc5179ffb 596 // pc.printf("X%d=%d,",i,pos);
stanley1228 10:328cc5179ffb 597 //}
stanley1228 10:328cc5179ffb 598 //pc.printf("\n");
stanley1228 7:15f0494813f7 599
stanley1228 7:15f0494813f7 600
stanley1228 9:54710454ce60 601
stanley1228 9:54710454ce60 602
stanley1228 7:15f0494813f7 603
stanley1228 7:15f0494813f7 604 //====Test syncWrite_u16base===
stanley1228 9:54710454ce60 605 /*static short int pos3=-100;
stanley1228 7:15f0494813f7 606 pos3=(pos3==-100)? 100 :-100;
stanley1228 7:15f0494813f7 607
stanley1228 7:15f0494813f7 608 short int SyncPage1[21]=
stanley1228 7:15f0494813f7 609 {
stanley1228 7:15f0494813f7 610 ID_AXIS1,100,5,
stanley1228 7:15f0494813f7 611 ID_AXIS2,100,5,
stanley1228 7:15f0494813f7 612 ID_AXIS3,100,5,
stanley1228 7:15f0494813f7 613 ID_AXIS4,100,5,
stanley1228 7:15f0494813f7 614 ID_AXIS5,100,5,
stanley1228 7:15f0494813f7 615 ID_AXIS6,100,5,
stanley1228 7:15f0494813f7 616 ID_AXIS7,100,5,
stanley1228 7:15f0494813f7 617 };
stanley1228 7:15f0494813f7 618
stanley1228 9:54710454ce60 619 wait(0.5); */
stanley1228 7:15f0494813f7 620 //short int SyncPage1[21]=//Test use
stanley1228 7:15f0494813f7 621 //{
stanley1228 7:15f0494813f7 622 // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512
stanley1228 7:15f0494813f7 623 // 0x01,0x020,0x360,
stanley1228 7:15f0494813f7 624 // 0x02,0x030,0x170,
stanley1228 7:15f0494813f7 625 // 0x03,0x220,0x380,
stanley1228 7:15f0494813f7 626 // 0x05,0x123,0x121,
stanley1228 7:15f0494813f7 627 // 0x06,0x234,0x143,
stanley1228 7:15f0494813f7 628 // 0x07,0x145,0x167
stanley1228 7:15f0494813f7 629 //};
stanley1228 7:15f0494813f7 630
stanley1228 9:54710454ce60 631 //syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
stanley1228 7:15f0494813f7 632
stanley1228 7:15f0494813f7 633 }
stanley1228 4:53ef39fbf9d9 634 }