![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
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 | 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 | } |