hsu han-lin / RobotControl_7Axis

RobotControl_7Axis

Dependents:   RoboticArm DXL_SDK_Porting_Test

RobotControl_7Axis.cpp@1:584f36ed8717, 2017-02-11 (annotated)

Committer:
stanley1228
Date:
Sat Feb 11 00:22:11 2017 +0800
Revision:
1:584f36ed8717
Parent:
0:c62673155cbf
Child:
2:71ed7da9ea36
1.add RobotControl_7Axis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 1:584f36ed8717 1 #include "mbed.h"
stanley1228 1:584f36ed8717 2 #include "dynamixel.h"
stanley1228 1:584f36ed8717 3 #include "RobotControl_7Axis.h"
stanley1228 1:584f36ed8717 4
stanley1228 1:584f36ed8717 5 extern Serial pc;
stanley1228 1:584f36ed8717 6
stanley1228 1:584f36ed8717 7
stanley1228 1:584f36ed8717 8 #if (DEBUG)
stanley1228 1:584f36ed8717 9 #define DBGMSG(x) pc.printf x;
stanley1228 1:584f36ed8717 10 #else
stanley1228 1:584f36ed8717 11 #define DBGMSG(x)
stanley1228 1:584f36ed8717 12 #endif
stanley1228 1:584f36ed8717 13
stanley1228 1:584f36ed8717 14
stanley1228 1:584f36ed8717 15
stanley1228 1:584f36ed8717 16 unsigned char getMapAxisNO(unsigned char index)
stanley1228 1:584f36ed8717 17 {
stanley1228 1:584f36ed8717 18 return gMapAxisNO[index];
stanley1228 1:584f36ed8717 19
stanley1228 1:584f36ed8717 20 }
stanley1228 1:584f36ed8717 21
stanley1228 1:584f36ed8717 22 unsigned char getMapAxisID(unsigned char index)
stanley1228 1:584f36ed8717 23 {
stanley1228 1:584f36ed8717 24 return gMapAxisID[index];
stanley1228 1:584f36ed8717 25 }
stanley1228 1:584f36ed8717 26
stanley1228 1:584f36ed8717 27
stanley1228 1:584f36ed8717 28 int ROM_Setting() //new
stanley1228 1:584f36ed8717 29 {
stanley1228 1:584f36ed8717 30 //==calculate Max torque to set in rom
stanley1228 1:584f36ed8717 31 const short int Max_torque[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 32 {
stanley1228 1:584f36ed8717 33 AXIS1_MAX_TORQUE,
stanley1228 1:584f36ed8717 34 AXIS2_MAX_TORQUE,
stanley1228 1:584f36ed8717 35 AXIS3_MAX_TORQUE,
stanley1228 1:584f36ed8717 36 AXIS4_MAX_TORQUE,
stanley1228 1:584f36ed8717 37 AXIS5_MAX_TORQUE,
stanley1228 1:584f36ed8717 38 AXIS6_MAX_TORQUE,
stanley1228 1:584f36ed8717 39 AXIS7_MAX_TORQUE
stanley1228 1:584f36ed8717 40 };
stanley1228 1:584f36ed8717 41
stanley1228 1:584f36ed8717 42
stanley1228 1:584f36ed8717 43 //==Calculate angle limit==//
stanley1228 1:584f36ed8717 44 const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 45 {
stanley1228 1:584f36ed8717 46 AXIS1_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 47 AXIS2_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 48 AXIS3_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 49 AXIS4_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 50 AXIS5_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 51 AXIS6_R2M_OFFSET_DEG,
stanley1228 1:584f36ed8717 52 AXIS7_R2M_OFFSET_DEG
stanley1228 1:584f36ed8717 53 };
stanley1228 1:584f36ed8717 54
stanley1228 1:584f36ed8717 55 const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 56 {
stanley1228 1:584f36ed8717 57 AXIS1_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 58 AXIS2_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 59 AXIS3_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 60 AXIS4_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 61 AXIS5_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 62 AXIS6_ROBOT_LIM_DEG_L,
stanley1228 1:584f36ed8717 63 AXIS7_ROBOT_LIM_DEG_L
stanley1228 1:584f36ed8717 64 };
stanley1228 1:584f36ed8717 65
stanley1228 1:584f36ed8717 66 const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 67 {
stanley1228 1:584f36ed8717 68 AXIS1_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 69 AXIS2_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 70 AXIS3_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 71 AXIS4_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 72 AXIS5_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 73 AXIS6_ROBOT_LIM_DEG_H,
stanley1228 1:584f36ed8717 74 AXIS7_ROBOT_LIM_DEG_H
stanley1228 1:584f36ed8717 75 };
stanley1228 1:584f36ed8717 76
stanley1228 1:584f36ed8717 77 unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0};
stanley1228 1:584f36ed8717 78 unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0};
stanley1228 1:584f36ed8717 79
stanley1228 1:584f36ed8717 80
stanley1228 1:584f36ed8717 81 int i=0;
stanley1228 1:584f36ed8717 82 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 83 {
stanley1228 1:584f36ed8717 84 Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
stanley1228 1:584f36ed8717 85 Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
stanley1228 1:584f36ed8717 86 }
stanley1228 1:584f36ed8717 87
stanley1228 1:584f36ed8717 88
stanley1228 1:584f36ed8717 89
stanley1228 1:584f36ed8717 90 //==writing to ROM==//
stanley1228 1:584f36ed8717 91 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 92 {
stanley1228 1:584f36ed8717 93 //==Set MAX_torgue==//
stanley1228 1:584f36ed8717 94 dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);// more safe
stanley1228 1:584f36ed8717 95
stanley1228 1:584f36ed8717 96 //==Set angel limit==//
stanley1228 1:584f36ed8717 97 dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]);
stanley1228 1:584f36ed8717 98 dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]);
stanley1228 1:584f36ed8717 99
stanley1228 1:584f36ed8717 100 //==Set MULTITURN_OFFSET to 0==//
stanley1228 1:584f36ed8717 101 dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0);
stanley1228 1:584f36ed8717 102 }
stanley1228 1:584f36ed8717 103
stanley1228 1:584f36ed8717 104
stanley1228 1:584f36ed8717 105 //==read and check ==//
stanley1228 1:584f36ed8717 106 int txrx_result=0;
stanley1228 1:584f36ed8717 107 short int max_torque=0;
stanley1228 1:584f36ed8717 108 short int cw_angel_lim=0,ccw_angle_lim=0;
stanley1228 1:584f36ed8717 109 short int multi_turn_offset=0;
stanley1228 1:584f36ed8717 110 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 111 {
stanley1228 1:584f36ed8717 112 pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);
stanley1228 1:584f36ed8717 113
stanley1228 1:584f36ed8717 114 //==MAX_torgue==//
stanley1228 1:584f36ed8717 115 max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE);
stanley1228 1:584f36ed8717 116 txrx_result = dxl_get_result();
stanley1228 1:584f36ed8717 117 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 118 pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result);
stanley1228 1:584f36ed8717 119 else
stanley1228 1:584f36ed8717 120 pc.printf("MAX_TORQUE=%d\n",max_torque);
stanley1228 1:584f36ed8717 121
stanley1228 1:584f36ed8717 122 //==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==//
stanley1228 1:584f36ed8717 123 cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L);
stanley1228 1:584f36ed8717 124 txrx_result = dxl_get_result();
stanley1228 1:584f36ed8717 125 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 126 pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result);
stanley1228 1:584f36ed8717 127 else
stanley1228 1:584f36ed8717 128 pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG);
stanley1228 1:584f36ed8717 129
stanley1228 1:584f36ed8717 130 ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L);
stanley1228 1:584f36ed8717 131 txrx_result = dxl_get_result();
stanley1228 1:584f36ed8717 132 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 133 pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result);
stanley1228 1:584f36ed8717 134 else
stanley1228 1:584f36ed8717 135 pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG);
stanley1228 1:584f36ed8717 136
stanley1228 1:584f36ed8717 137
stanley1228 1:584f36ed8717 138 //==multi turn offset==//
stanley1228 1:584f36ed8717 139 multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET);
stanley1228 1:584f36ed8717 140 txrx_result = dxl_get_result();
stanley1228 1:584f36ed8717 141 if(txrx_result!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 142 pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result);
stanley1228 1:584f36ed8717 143 else
stanley1228 1:584f36ed8717 144 pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
stanley1228 1:584f36ed8717 145 }
stanley1228 1:584f36ed8717 146
stanley1228 1:584f36ed8717 147 return 0;
stanley1228 1:584f36ed8717 148 }
stanley1228 1:584f36ed8717 149
stanley1228 1:584f36ed8717 150
stanley1228 1:584f36ed8717 151
stanley1228 1:584f36ed8717 152
stanley1228 1:584f36ed8717 153 int Read_pos(float *pos,unsigned char unit)
stanley1228 1:584f36ed8717 154 {
stanley1228 1:584f36ed8717 155 int i=0;
stanley1228 1:584f36ed8717 156 short int pulse=0;
stanley1228 1:584f36ed8717 157 int rt=0;
stanley1228 1:584f36ed8717 158
stanley1228 1:584f36ed8717 159 if(unit==DEF_UNIT_RAD)
stanley1228 1:584f36ed8717 160 {
stanley1228 1:584f36ed8717 161 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 162 {
stanley1228 1:584f36ed8717 163 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 1:584f36ed8717 164 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 165 {
stanley1228 1:584f36ed8717 166 rt=-gMapAxisNO[i];
stanley1228 1:584f36ed8717 167 pos[i]=0xffff;
stanley1228 1:584f36ed8717 168 }
stanley1228 1:584f36ed8717 169 else
stanley1228 1:584f36ed8717 170 {
stanley1228 1:584f36ed8717 171 pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset
stanley1228 1:584f36ed8717 172 pos[i]=pulse*DEF_RATIO_PUS_TO_RAD;
stanley1228 1:584f36ed8717 173 }
stanley1228 1:584f36ed8717 174 }
stanley1228 1:584f36ed8717 175
stanley1228 1:584f36ed8717 176 }
stanley1228 1:584f36ed8717 177 else if(unit==DEF_UNIT_DEG)
stanley1228 1:584f36ed8717 178 {
stanley1228 1:584f36ed8717 179 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 180 {
stanley1228 1:584f36ed8717 181 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 1:584f36ed8717 182 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 183 {
stanley1228 1:584f36ed8717 184 rt=-gMapAxisNO[i];
stanley1228 1:584f36ed8717 185 pos[i]=0xffff;
stanley1228 1:584f36ed8717 186 }
stanley1228 1:584f36ed8717 187 else
stanley1228 1:584f36ed8717 188 {
stanley1228 1:584f36ed8717 189 pulse-=gr2m_offset_pulse[i];
stanley1228 1:584f36ed8717 190 pos[i]=pulse*DEF_RATIO_PUS_TO_DEG;
stanley1228 1:584f36ed8717 191 }
stanley1228 1:584f36ed8717 192 }
stanley1228 1:584f36ed8717 193 }
stanley1228 1:584f36ed8717 194 else if(unit==DEF_UNIT_PUS)
stanley1228 1:584f36ed8717 195 {
stanley1228 1:584f36ed8717 196 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 197 {
stanley1228 1:584f36ed8717 198 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 1:584f36ed8717 199 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 200 {
stanley1228 1:584f36ed8717 201 rt=-gMapAxisNO[i];
stanley1228 1:584f36ed8717 202 pos[i]=0xffff;
stanley1228 1:584f36ed8717 203 }
stanley1228 1:584f36ed8717 204 else
stanley1228 1:584f36ed8717 205 {
stanley1228 1:584f36ed8717 206 pulse-=gr2m_offset_pulse[i];
stanley1228 1:584f36ed8717 207 pos[i]=pulse;
stanley1228 1:584f36ed8717 208 }
stanley1228 1:584f36ed8717 209 }
stanley1228 1:584f36ed8717 210
stanley1228 1:584f36ed8717 211 }
stanley1228 1:584f36ed8717 212 else
stanley1228 1:584f36ed8717 213 {
stanley1228 1:584f36ed8717 214 //non offset value
stanley1228 1:584f36ed8717 215 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 216 {
stanley1228 1:584f36ed8717 217 pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 1:584f36ed8717 218 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 1:584f36ed8717 219 {
stanley1228 1:584f36ed8717 220 rt=-gMapAxisNO[i];
stanley1228 1:584f36ed8717 221 pos[i]=0xffff;
stanley1228 1:584f36ed8717 222 }
stanley1228 1:584f36ed8717 223 }
stanley1228 1:584f36ed8717 224 }
stanley1228 1:584f36ed8717 225
stanley1228 1:584f36ed8717 226 return rt;
stanley1228 1:584f36ed8717 227 }
stanley1228 1:584f36ed8717 228
stanley1228 1:584f36ed8717 229
stanley1228 1:584f36ed8717 230
stanley1228 1:584f36ed8717 231
stanley1228 1:584f36ed8717 232 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity)
stanley1228 1:584f36ed8717 233 {
stanley1228 1:584f36ed8717 234 unsigned char i=0;
stanley1228 1:584f36ed8717 235
stanley1228 1:584f36ed8717 236 //===================================================================//
stanley1228 1:584f36ed8717 237 //==limit axis if not zero ,the return value is the overlimit axis==//
stanley1228 1:584f36ed8717 238 //===================================================================//
stanley1228 1:584f36ed8717 239 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 240 {
stanley1228 1:584f36ed8717 241 if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) )
stanley1228 1:584f36ed8717 242 {
stanley1228 1:584f36ed8717 243 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 1:584f36ed8717 244 return -gMapAxisNO[i];
stanley1228 1:584f36ed8717 245 }
stanley1228 1:584f36ed8717 246 }
stanley1228 1:584f36ed8717 247
stanley1228 1:584f36ed8717 248 //===================================================//
stanley1228 1:584f36ed8717 249 //==trnasformat to pulse and offset to motor domain==//
stanley1228 1:584f36ed8717 250 //====================================================//
stanley1228 1:584f36ed8717 251 short int Ang_pulse[MAX_AXIS_NUM]={0};
stanley1228 1:584f36ed8717 252 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 253 {
stanley1228 1:584f36ed8717 254 Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS);
stanley1228 1:584f36ed8717 255 Ang_pulse[i]+=gr2m_offset_pulse[i];
stanley1228 1:584f36ed8717 256
stanley1228 1:584f36ed8717 257 if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )// 0~4095
stanley1228 1:584f36ed8717 258 {
stanley1228 1:584f36ed8717 259 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 1:584f36ed8717 260 return -gMapAxisNO[i];
stanley1228 1:584f36ed8717 261 }
stanley1228 1:584f36ed8717 262 }
stanley1228 1:584f36ed8717 263
stanley1228 1:584f36ed8717 264 //================================//
stanley1228 1:584f36ed8717 265 //==output to motor by syncpage===//
stanley1228 1:584f36ed8717 266 //===============================//
stanley1228 1:584f36ed8717 267 unsigned short int SyncPage1[21]=
stanley1228 1:584f36ed8717 268 {
stanley1228 1:584f36ed8717 269 ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
stanley1228 1:584f36ed8717 270 ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2],
stanley1228 1:584f36ed8717 271 ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3],
stanley1228 1:584f36ed8717 272 ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4],
stanley1228 1:584f36ed8717 273 ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5],
stanley1228 1:584f36ed8717 274 ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6],
stanley1228 1:584f36ed8717 275 ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7],
stanley1228 1:584f36ed8717 276 };
stanley1228 1:584f36ed8717 277
stanley1228 1:584f36ed8717 278
stanley1228 1:584f36ed8717 279 #if (DEBUG)
stanley1228 1:584f36ed8717 280 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 1:584f36ed8717 281 pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]);
stanley1228 1:584f36ed8717 282 #endif
stanley1228 1:584f36ed8717 283
stanley1228 1:584f36ed8717 284 syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
stanley1228 1:584f36ed8717 285
stanley1228 1:584f36ed8717 286 return 0;
stanley1228 1:584f36ed8717 287 }
stanley1228 1:584f36ed8717 288