RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
RobotControl_7Axis.h@3:e380c7db9133, 2017-03-31 (annotated)
- Committer:
- stanley1228
- Date:
- Fri Mar 31 16:31:30 2017 +0800
- Revision:
- 3:e380c7db9133
- Parent:
- 2:71ed7da9ea36
1.Add output_to_dynamixel_pulse
2.grobot_lim_pus_H
3.grobot_lim_pus_L
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stanley1228 | 1:584f36ed8717 | 1 | #ifndef _ROBOT_CONTROL_7AXIS_HEADER |
stanley1228 | 1:584f36ed8717 | 2 | #define _ROBOT_CONTROL_7AXIS_HEADER |
stanley1228 | 1:584f36ed8717 | 3 | |
stanley1228 | 2:71ed7da9ea36 | 4 | #include "Matrix.h" |
stanley1228 | 2:71ed7da9ea36 | 5 | //#include "MatrixMath.h" |
stanley1228 | 1:584f36ed8717 | 6 | //========== |
stanley1228 | 1:584f36ed8717 | 7 | //==MAX AXIS |
stanley1228 | 1:584f36ed8717 | 8 | //========== |
stanley1228 | 1:584f36ed8717 | 9 | #define MAX_AXIS_NUM 7 |
stanley1228 | 1:584f36ed8717 | 10 | |
stanley1228 | 1:584f36ed8717 | 11 | |
stanley1228 | 1:584f36ed8717 | 12 | //=========================== |
stanley1228 | 1:584f36ed8717 | 13 | //==Axis index,ID,NO mapping |
stanley1228 | 1:584f36ed8717 | 14 | //========================== |
stanley1228 | 1:584f36ed8717 | 15 | enum{ |
stanley1228 | 1:584f36ed8717 | 16 | Index_AXIS1=0, |
stanley1228 | 1:584f36ed8717 | 17 | Index_AXIS2, |
stanley1228 | 1:584f36ed8717 | 18 | Index_AXIS3, |
stanley1228 | 1:584f36ed8717 | 19 | Index_AXIS4, |
stanley1228 | 1:584f36ed8717 | 20 | Index_AXIS5, |
stanley1228 | 1:584f36ed8717 | 21 | Index_AXIS6, |
stanley1228 | 1:584f36ed8717 | 22 | Index_AXIS7 |
stanley1228 | 1:584f36ed8717 | 23 | }; |
stanley1228 | 1:584f36ed8717 | 24 | |
stanley1228 | 1:584f36ed8717 | 25 | enum{ |
stanley1228 | 1:584f36ed8717 | 26 | ID_AXIS1=1, |
stanley1228 | 1:584f36ed8717 | 27 | ID_AXIS2, |
stanley1228 | 1:584f36ed8717 | 28 | ID_AXIS3, |
stanley1228 | 1:584f36ed8717 | 29 | ID_AXIS4, |
stanley1228 | 1:584f36ed8717 | 30 | ID_AXIS5, |
stanley1228 | 1:584f36ed8717 | 31 | ID_AXIS6, |
stanley1228 | 1:584f36ed8717 | 32 | ID_AXIS7 |
stanley1228 | 1:584f36ed8717 | 33 | }; |
stanley1228 | 1:584f36ed8717 | 34 | |
stanley1228 | 1:584f36ed8717 | 35 | enum{ |
stanley1228 | 1:584f36ed8717 | 36 | NO_AXIS1=1, |
stanley1228 | 1:584f36ed8717 | 37 | NO_AXIS2, |
stanley1228 | 1:584f36ed8717 | 38 | NO_AXIS3, |
stanley1228 | 1:584f36ed8717 | 39 | NO_AXIS4, |
stanley1228 | 1:584f36ed8717 | 40 | NO_AXIS5, |
stanley1228 | 1:584f36ed8717 | 41 | NO_AXIS6, |
stanley1228 | 1:584f36ed8717 | 42 | NO_AXIS7 |
stanley1228 | 1:584f36ed8717 | 43 | }; |
stanley1228 | 1:584f36ed8717 | 44 | |
stanley1228 | 1:584f36ed8717 | 45 | |
stanley1228 | 1:584f36ed8717 | 46 | static const unsigned char gMapAxisNO[MAX_AXIS_NUM]= |
stanley1228 | 1:584f36ed8717 | 47 | { |
stanley1228 | 1:584f36ed8717 | 48 | NO_AXIS1, |
stanley1228 | 1:584f36ed8717 | 49 | NO_AXIS2, |
stanley1228 | 1:584f36ed8717 | 50 | NO_AXIS3, |
stanley1228 | 1:584f36ed8717 | 51 | NO_AXIS4, |
stanley1228 | 1:584f36ed8717 | 52 | NO_AXIS5, |
stanley1228 | 1:584f36ed8717 | 53 | NO_AXIS6, |
stanley1228 | 1:584f36ed8717 | 54 | NO_AXIS7 |
stanley1228 | 1:584f36ed8717 | 55 | }; |
stanley1228 | 1:584f36ed8717 | 56 | static const unsigned char gMapAxisID[MAX_AXIS_NUM]= |
stanley1228 | 1:584f36ed8717 | 57 | { |
stanley1228 | 1:584f36ed8717 | 58 | ID_AXIS1, |
stanley1228 | 1:584f36ed8717 | 59 | ID_AXIS2, |
stanley1228 | 1:584f36ed8717 | 60 | ID_AXIS3, |
stanley1228 | 1:584f36ed8717 | 61 | ID_AXIS4, |
stanley1228 | 1:584f36ed8717 | 62 | ID_AXIS5, |
stanley1228 | 1:584f36ed8717 | 63 | ID_AXIS6, |
stanley1228 | 1:584f36ed8717 | 64 | ID_AXIS7 |
stanley1228 | 1:584f36ed8717 | 65 | }; |
stanley1228 | 1:584f36ed8717 | 66 | |
stanley1228 | 1:584f36ed8717 | 67 | |
stanley1228 | 1:584f36ed8717 | 68 | //================ |
stanley1228 | 1:584f36ed8717 | 69 | //==Unit transform |
stanley1228 | 1:584f36ed8717 | 70 | //================= |
stanley1228 | 2:71ed7da9ea36 | 71 | #define DEF_PI (3.1415926F) |
stanley1228 | 1:584f36ed8717 | 72 | #define DEF_RATIO_PUS_TO_DEG (0.0879F) //360/4096 |
stanley1228 | 1:584f36ed8717 | 73 | #define DEF_RATIO_PUS_TO_RAD (0.0015F) //2pi/4096 0.00153398078788564122971808758949 |
stanley1228 | 1:584f36ed8717 | 74 | #define DEF_RATIO_DEG_TO_PUS (11.3778F) //4096/360 |
stanley1228 | 1:584f36ed8717 | 75 | #define DEF_RATIO_DEG_TO_RAD (DEF_PI/180) //pi/180 0.01745329251994329576923690768489 (0.0175F) |
stanley1228 | 1:584f36ed8717 | 76 | #define DEF_RATIO_RAD_TO_PUS (651.8986F) //4096/2pi 651.89864690440329530934789477382 |
stanley1228 | 1:584f36ed8717 | 77 | |
stanley1228 | 1:584f36ed8717 | 78 | |
stanley1228 | 1:584f36ed8717 | 79 | //for read pos unit select |
stanley1228 | 1:584f36ed8717 | 80 | enum{ |
stanley1228 | 1:584f36ed8717 | 81 | DEF_UNIT_RAD=1, |
stanley1228 | 1:584f36ed8717 | 82 | DEF_UNIT_DEG, |
stanley1228 | 1:584f36ed8717 | 83 | DEF_UNIT_PUS |
stanley1228 | 1:584f36ed8717 | 84 | }; |
stanley1228 | 1:584f36ed8717 | 85 | |
stanley1228 | 1:584f36ed8717 | 86 | //===================================== |
stanley1228 | 1:584f36ed8717 | 87 | //==robot hard ware dependent parameter |
stanley1228 | 1:584f36ed8717 | 88 | //===================================== |
stanley1228 | 1:584f36ed8717 | 89 | |
stanley1228 | 1:584f36ed8717 | 90 | //==robot to Motor offset==// //robot pos=motor position -M2R_OFFSET |
stanley1228 | 1:584f36ed8717 | 91 | |
stanley1228 | 1:584f36ed8717 | 92 | #define AXIS1_R2M_OFFSET_DEG 90 |
stanley1228 | 1:584f36ed8717 | 93 | #define AXIS2_R2M_OFFSET_DEG 90 |
stanley1228 | 1:584f36ed8717 | 94 | #define AXIS3_R2M_OFFSET_DEG 225 |
stanley1228 | 1:584f36ed8717 | 95 | #define AXIS4_R2M_OFFSET_DEG 90 |
stanley1228 | 1:584f36ed8717 | 96 | #define AXIS5_R2M_OFFSET_DEG 180 |
stanley1228 | 1:584f36ed8717 | 97 | #define AXIS6_R2M_OFFSET_DEG 90 |
stanley1228 | 1:584f36ed8717 | 98 | #define AXIS7_R2M_OFFSET_DEG 90 |
stanley1228 | 1:584f36ed8717 | 99 | |
stanley1228 | 1:584f36ed8717 | 100 | //==robot angle limit==// |
stanley1228 | 1:584f36ed8717 | 101 | #define AXIS1_ROBOT_LIM_DEG_L (-80) |
stanley1228 | 1:584f36ed8717 | 102 | #define AXIS1_ROBOT_LIM_DEG_H 180 |
stanley1228 | 1:584f36ed8717 | 103 | #define AXIS2_ROBOT_LIM_DEG_L (-18) |
stanley1228 | 1:584f36ed8717 | 104 | #define AXIS2_ROBOT_LIM_DEG_H 180 |
stanley1228 | 1:584f36ed8717 | 105 | #define AXIS3_ROBOT_LIM_DEG_L (-225) |
stanley1228 | 1:584f36ed8717 | 106 | #define AXIS3_ROBOT_LIM_DEG_H 105 |
stanley1228 | 1:584f36ed8717 | 107 | #define AXIS4_ROBOT_LIM_DEG_L 0 //need to test |
stanley1228 | 1:584f36ed8717 | 108 | #define AXIS4_ROBOT_LIM_DEG_H 114 //need to test |
stanley1228 | 1:584f36ed8717 | 109 | #define AXIS5_ROBOT_LIM_DEG_L (-180) |
stanley1228 | 1:584f36ed8717 | 110 | #define AXIS5_ROBOT_LIM_DEG_H 148 |
stanley1228 | 1:584f36ed8717 | 111 | #define AXIS6_ROBOT_LIM_DEG_L (-60) |
stanley1228 | 1:584f36ed8717 | 112 | #define AXIS6_ROBOT_LIM_DEG_H 90 |
stanley1228 | 1:584f36ed8717 | 113 | #define AXIS7_ROBOT_LIM_DEG_L (-30) |
stanley1228 | 1:584f36ed8717 | 114 | #define AXIS7_ROBOT_LIM_DEG_H 30 |
stanley1228 | 1:584f36ed8717 | 115 | |
stanley1228 | 1:584f36ed8717 | 116 | |
stanley1228 | 1:584f36ed8717 | 117 | //==robot TORQUE limit==// |
stanley1228 | 1:584f36ed8717 | 118 | #define AXIS1_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 119 | #define AXIS2_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 120 | #define AXIS3_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 121 | #define AXIS4_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 122 | #define AXIS5_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 123 | #define AXIS6_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 124 | #define AXIS7_MAX_TORQUE 55 |
stanley1228 | 1:584f36ed8717 | 125 | |
stanley1228 | 1:584f36ed8717 | 126 | static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]= |
stanley1228 | 1:584f36ed8717 | 127 | { |
stanley1228 | 1:584f36ed8717 | 128 | (unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 129 | (unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 130 | (unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 131 | (unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 132 | (unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 133 | (unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), |
stanley1228 | 1:584f36ed8717 | 134 | (unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS) |
stanley1228 | 1:584f36ed8717 | 135 | }; |
stanley1228 | 1:584f36ed8717 | 136 | |
stanley1228 | 1:584f36ed8717 | 137 | |
stanley1228 | 1:584f36ed8717 | 138 | static const float grobot_lim_rad_L[MAX_AXIS_NUM]= |
stanley1228 | 1:584f36ed8717 | 139 | { |
stanley1228 | 1:584f36ed8717 | 140 | AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 141 | AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 142 | AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 143 | AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 144 | AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 145 | AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 146 | AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD |
stanley1228 | 1:584f36ed8717 | 147 | }; |
stanley1228 | 1:584f36ed8717 | 148 | |
stanley1228 | 1:584f36ed8717 | 149 | static const float grobot_lim_rad_H[MAX_AXIS_NUM]= |
stanley1228 | 1:584f36ed8717 | 150 | { |
stanley1228 | 1:584f36ed8717 | 151 | AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 152 | AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 153 | AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 154 | AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 155 | AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 156 | AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, |
stanley1228 | 1:584f36ed8717 | 157 | AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD |
stanley1228 | 1:584f36ed8717 | 158 | }; |
stanley1228 | 3:e380c7db9133 | 159 | |
stanley1228 | 3:e380c7db9133 | 160 | static const float grobot_lim_pus_L[MAX_AXIS_NUM]= |
stanley1228 | 3:e380c7db9133 | 161 | { |
stanley1228 | 3:e380c7db9133 | 162 | AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 163 | AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 164 | AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 165 | AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 166 | AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 167 | AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 168 | AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS |
stanley1228 | 3:e380c7db9133 | 169 | }; |
stanley1228 | 3:e380c7db9133 | 170 | |
stanley1228 | 3:e380c7db9133 | 171 | static const float grobot_lim_pus_H[MAX_AXIS_NUM]= |
stanley1228 | 3:e380c7db9133 | 172 | { |
stanley1228 | 3:e380c7db9133 | 173 | AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 174 | AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 175 | AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 176 | AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 177 | AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 178 | AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS, |
stanley1228 | 3:e380c7db9133 | 179 | AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS |
stanley1228 | 3:e380c7db9133 | 180 | }; |
stanley1228 | 1:584f36ed8717 | 181 | //================================= |
stanley1228 | 1:584f36ed8717 | 182 | //==morot max pulse in joint mode |
stanley1228 | 1:584f36ed8717 | 183 | //================================= |
stanley1228 | 1:584f36ed8717 | 184 | #define DEF_JOINT_MODE_MAX_PULSE 4095 |
stanley1228 | 1:584f36ed8717 | 185 | #define DEF_JOINT_MODE_MIN_PULSE 0 |
stanley1228 | 1:584f36ed8717 | 186 | |
stanley1228 | 2:71ed7da9ea36 | 187 | //=================== |
stanley1228 | 2:71ed7da9ea36 | 188 | //==ROBOT link length |
stanley1228 | 2:71ed7da9ea36 | 189 | //==================== |
stanley1228 | 2:71ed7da9ea36 | 190 | #define L0 0 //可能要刪掉 |
stanley1228 | 2:71ed7da9ea36 | 191 | #define L1 100 //upper arm |
stanley1228 | 2:71ed7da9ea36 | 192 | #define L2 100 //forearm |
stanley1228 | 2:71ed7da9ea36 | 193 | #define L3 10 //length of end effector |
stanley1228 | 2:71ed7da9ea36 | 194 | #define X_BASE 0 //基準點只能都先設0 |
stanley1228 | 2:71ed7da9ea36 | 195 | #define Y_BASE 0 |
stanley1228 | 2:71ed7da9ea36 | 196 | #define Z_BASE 0 |
stanley1228 | 2:71ed7da9ea36 | 197 | |
stanley1228 | 2:71ed7da9ea36 | 198 | |
stanley1228 | 1:584f36ed8717 | 199 | |
stanley1228 | 1:584f36ed8717 | 200 | |
stanley1228 | 1:584f36ed8717 | 201 | //Function |
stanley1228 | 1:584f36ed8717 | 202 | unsigned char getMapAxisNO(unsigned char index); //index 0~ (MAX_AXIS_NUM-1) |
stanley1228 | 1:584f36ed8717 | 203 | unsigned char getMapAxisID(unsigned char index); |
stanley1228 | 1:584f36ed8717 | 204 | int ROM_Setting(); |
stanley1228 | 1:584f36ed8717 | 205 | int Read_pos(float *pos,unsigned char unit); |
stanley1228 | 1:584f36ed8717 | 206 | int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ; |
stanley1228 | 3:e380c7db9133 | 207 | int Output_to_Dynamixel_pulse(const unsigned short int *Ang_pus,const unsigned short int *velocity); |
stanley1228 | 2:71ed7da9ea36 | 208 | Matrix R_z1x2y3(float alpha,float beta,float gamma); |
stanley1228 | 2:71ed7da9ea36 | 209 | float norm(const Matrix& v); |
stanley1228 | 2:71ed7da9ea36 | 210 | Matrix Rogridues(float theta,const Matrix& V_A); |
stanley1228 | 2:71ed7da9ea36 | 211 | int IK_7DOF(const float l1,const float l2,const float l3,const float x_base,const float y_base,const float z_base,const float x_end,const float y_end,const float z_end,const float alpha,const float beta,const float gamma,const float Rednt_alpha,float* theta); |
stanley1228 | 1:584f36ed8717 | 212 | |
stanley1228 | 1:584f36ed8717 | 213 | |
stanley1228 | 1:584f36ed8717 | 214 | #endif |