hsu han-lin / RobotControl_7Axis

RobotControl_7Axis

Dependents:   RoboticArm DXL_SDK_Porting_Test

RobotControl_7Axis.h@2:71ed7da9ea36, 2017-02-11 (annotated)

Committer:
stanley1228
Date:
Sat Feb 11 13:06:49 2017 +0000
Revision:
2:71ed7da9ea36
Parent:
1:584f36ed8717
Child:
3:e380c7db9133
Test seems like ok

Who changed what in which revision?

UserRevisionLine numberNew 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 1:584f36ed8717 159 //=================================
stanley1228 1:584f36ed8717 160 //==morot max pulse in joint mode
stanley1228 1:584f36ed8717 161 //=================================
stanley1228 1:584f36ed8717 162 #define DEF_JOINT_MODE_MAX_PULSE 4095
stanley1228 1:584f36ed8717 163 #define DEF_JOINT_MODE_MIN_PULSE 0
stanley1228 1:584f36ed8717 164
stanley1228 2:71ed7da9ea36 165 //===================
stanley1228 2:71ed7da9ea36 166 //==ROBOT link length
stanley1228 2:71ed7da9ea36 167 //====================
stanley1228 2:71ed7da9ea36 168 #define L0 0 //可能要刪掉
stanley1228 2:71ed7da9ea36 169 #define L1 100 //upper arm
stanley1228 2:71ed7da9ea36 170 #define L2 100 //forearm
stanley1228 2:71ed7da9ea36 171 #define L3 10 //length of end effector
stanley1228 2:71ed7da9ea36 172 #define X_BASE 0 //基準點只能都先設0
stanley1228 2:71ed7da9ea36 173 #define Y_BASE 0
stanley1228 2:71ed7da9ea36 174 #define Z_BASE 0
stanley1228 2:71ed7da9ea36 175
stanley1228 2:71ed7da9ea36 176
stanley1228 1:584f36ed8717 177
stanley1228 1:584f36ed8717 178
stanley1228 1:584f36ed8717 179 //Function
stanley1228 1:584f36ed8717 180 unsigned char getMapAxisNO(unsigned char index); //index 0~ (MAX_AXIS_NUM-1)
stanley1228 1:584f36ed8717 181 unsigned char getMapAxisID(unsigned char index);
stanley1228 1:584f36ed8717 182 int ROM_Setting();
stanley1228 1:584f36ed8717 183 int Read_pos(float *pos,unsigned char unit);
stanley1228 1:584f36ed8717 184 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ;
stanley1228 2:71ed7da9ea36 185 Matrix R_z1x2y3(float alpha,float beta,float gamma);
stanley1228 2:71ed7da9ea36 186 float norm(const Matrix& v);
stanley1228 2:71ed7da9ea36 187 Matrix Rogridues(float theta,const Matrix& V_A);
stanley1228 2:71ed7da9ea36 188 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 189
stanley1228 1:584f36ed8717 190
stanley1228 1:584f36ed8717 191 #endif