hsu han-lin / RobotControl_7Axis

RobotControl_7Axis

Dependents:   RoboticArm DXL_SDK_Porting_Test

RobotControl_7Axis.h@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 #ifndef _ROBOT_CONTROL_7AXIS_HEADER
stanley1228 1:584f36ed8717 2 #define _ROBOT_CONTROL_7AXIS_HEADER
stanley1228 1:584f36ed8717 3
stanley1228 1:584f36ed8717 4
stanley1228 1:584f36ed8717 5 //==========
stanley1228 1:584f36ed8717 6 //==MAX AXIS
stanley1228 1:584f36ed8717 7 //==========
stanley1228 1:584f36ed8717 8 #define MAX_AXIS_NUM 7
stanley1228 1:584f36ed8717 9
stanley1228 1:584f36ed8717 10
stanley1228 1:584f36ed8717 11 //===========================
stanley1228 1:584f36ed8717 12 //==Axis index,ID,NO mapping
stanley1228 1:584f36ed8717 13 //==========================
stanley1228 1:584f36ed8717 14 enum{
stanley1228 1:584f36ed8717 15 Index_AXIS1=0,
stanley1228 1:584f36ed8717 16 Index_AXIS2,
stanley1228 1:584f36ed8717 17 Index_AXIS3,
stanley1228 1:584f36ed8717 18 Index_AXIS4,
stanley1228 1:584f36ed8717 19 Index_AXIS5,
stanley1228 1:584f36ed8717 20 Index_AXIS6,
stanley1228 1:584f36ed8717 21 Index_AXIS7
stanley1228 1:584f36ed8717 22 };
stanley1228 1:584f36ed8717 23
stanley1228 1:584f36ed8717 24 enum{
stanley1228 1:584f36ed8717 25 ID_AXIS1=1,
stanley1228 1:584f36ed8717 26 ID_AXIS2,
stanley1228 1:584f36ed8717 27 ID_AXIS3,
stanley1228 1:584f36ed8717 28 ID_AXIS4,
stanley1228 1:584f36ed8717 29 ID_AXIS5,
stanley1228 1:584f36ed8717 30 ID_AXIS6,
stanley1228 1:584f36ed8717 31 ID_AXIS7
stanley1228 1:584f36ed8717 32 };
stanley1228 1:584f36ed8717 33
stanley1228 1:584f36ed8717 34 enum{
stanley1228 1:584f36ed8717 35 NO_AXIS1=1,
stanley1228 1:584f36ed8717 36 NO_AXIS2,
stanley1228 1:584f36ed8717 37 NO_AXIS3,
stanley1228 1:584f36ed8717 38 NO_AXIS4,
stanley1228 1:584f36ed8717 39 NO_AXIS5,
stanley1228 1:584f36ed8717 40 NO_AXIS6,
stanley1228 1:584f36ed8717 41 NO_AXIS7
stanley1228 1:584f36ed8717 42 };
stanley1228 1:584f36ed8717 43
stanley1228 1:584f36ed8717 44
stanley1228 1:584f36ed8717 45 static const unsigned char gMapAxisNO[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 46 {
stanley1228 1:584f36ed8717 47 NO_AXIS1,
stanley1228 1:584f36ed8717 48 NO_AXIS2,
stanley1228 1:584f36ed8717 49 NO_AXIS3,
stanley1228 1:584f36ed8717 50 NO_AXIS4,
stanley1228 1:584f36ed8717 51 NO_AXIS5,
stanley1228 1:584f36ed8717 52 NO_AXIS6,
stanley1228 1:584f36ed8717 53 NO_AXIS7
stanley1228 1:584f36ed8717 54 };
stanley1228 1:584f36ed8717 55 static const unsigned char gMapAxisID[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 56 {
stanley1228 1:584f36ed8717 57 ID_AXIS1,
stanley1228 1:584f36ed8717 58 ID_AXIS2,
stanley1228 1:584f36ed8717 59 ID_AXIS3,
stanley1228 1:584f36ed8717 60 ID_AXIS4,
stanley1228 1:584f36ed8717 61 ID_AXIS5,
stanley1228 1:584f36ed8717 62 ID_AXIS6,
stanley1228 1:584f36ed8717 63 ID_AXIS7
stanley1228 1:584f36ed8717 64 };
stanley1228 1:584f36ed8717 65
stanley1228 1:584f36ed8717 66
stanley1228 1:584f36ed8717 67 //================
stanley1228 1:584f36ed8717 68 //==Unit transform
stanley1228 1:584f36ed8717 69 //=================
stanley1228 1:584f36ed8717 70 #define DEF_PI (3.1415F)
stanley1228 1:584f36ed8717 71 #define DEF_RATIO_PUS_TO_DEG (0.0879F) //360/4096
stanley1228 1:584f36ed8717 72 #define DEF_RATIO_PUS_TO_RAD (0.0015F) //2pi/4096 0.00153398078788564122971808758949
stanley1228 1:584f36ed8717 73 #define DEF_RATIO_DEG_TO_PUS (11.3778F) //4096/360
stanley1228 1:584f36ed8717 74 #define DEF_RATIO_DEG_TO_RAD (DEF_PI/180) //pi/180 0.01745329251994329576923690768489 (0.0175F)
stanley1228 1:584f36ed8717 75 #define DEF_RATIO_RAD_TO_PUS (651.8986F) //4096/2pi 651.89864690440329530934789477382
stanley1228 1:584f36ed8717 76
stanley1228 1:584f36ed8717 77
stanley1228 1:584f36ed8717 78 //for read pos unit select
stanley1228 1:584f36ed8717 79 enum{
stanley1228 1:584f36ed8717 80 DEF_UNIT_RAD=1,
stanley1228 1:584f36ed8717 81 DEF_UNIT_DEG,
stanley1228 1:584f36ed8717 82 DEF_UNIT_PUS
stanley1228 1:584f36ed8717 83 };
stanley1228 1:584f36ed8717 84
stanley1228 1:584f36ed8717 85 //=====================================
stanley1228 1:584f36ed8717 86 //==robot hard ware dependent parameter
stanley1228 1:584f36ed8717 87 //=====================================
stanley1228 1:584f36ed8717 88
stanley1228 1:584f36ed8717 89 //==robot to Motor offset==// //robot pos=motor position -M2R_OFFSET
stanley1228 1:584f36ed8717 90
stanley1228 1:584f36ed8717 91 #define AXIS1_R2M_OFFSET_DEG 90
stanley1228 1:584f36ed8717 92 #define AXIS2_R2M_OFFSET_DEG 90
stanley1228 1:584f36ed8717 93 #define AXIS3_R2M_OFFSET_DEG 225
stanley1228 1:584f36ed8717 94 #define AXIS4_R2M_OFFSET_DEG 90
stanley1228 1:584f36ed8717 95 #define AXIS5_R2M_OFFSET_DEG 180
stanley1228 1:584f36ed8717 96 #define AXIS6_R2M_OFFSET_DEG 90
stanley1228 1:584f36ed8717 97 #define AXIS7_R2M_OFFSET_DEG 90
stanley1228 1:584f36ed8717 98
stanley1228 1:584f36ed8717 99 //==robot angle limit==//
stanley1228 1:584f36ed8717 100 #define AXIS1_ROBOT_LIM_DEG_L (-80)
stanley1228 1:584f36ed8717 101 #define AXIS1_ROBOT_LIM_DEG_H 180
stanley1228 1:584f36ed8717 102 #define AXIS2_ROBOT_LIM_DEG_L (-18)
stanley1228 1:584f36ed8717 103 #define AXIS2_ROBOT_LIM_DEG_H 180
stanley1228 1:584f36ed8717 104 #define AXIS3_ROBOT_LIM_DEG_L (-225)
stanley1228 1:584f36ed8717 105 #define AXIS3_ROBOT_LIM_DEG_H 105
stanley1228 1:584f36ed8717 106 #define AXIS4_ROBOT_LIM_DEG_L 0 //need to test
stanley1228 1:584f36ed8717 107 #define AXIS4_ROBOT_LIM_DEG_H 114 //need to test
stanley1228 1:584f36ed8717 108 #define AXIS5_ROBOT_LIM_DEG_L (-180)
stanley1228 1:584f36ed8717 109 #define AXIS5_ROBOT_LIM_DEG_H 148
stanley1228 1:584f36ed8717 110 #define AXIS6_ROBOT_LIM_DEG_L (-60)
stanley1228 1:584f36ed8717 111 #define AXIS6_ROBOT_LIM_DEG_H 90
stanley1228 1:584f36ed8717 112 #define AXIS7_ROBOT_LIM_DEG_L (-30)
stanley1228 1:584f36ed8717 113 #define AXIS7_ROBOT_LIM_DEG_H 30
stanley1228 1:584f36ed8717 114
stanley1228 1:584f36ed8717 115
stanley1228 1:584f36ed8717 116 //==robot TORQUE limit==//
stanley1228 1:584f36ed8717 117 #define AXIS1_MAX_TORQUE 55
stanley1228 1:584f36ed8717 118 #define AXIS2_MAX_TORQUE 55
stanley1228 1:584f36ed8717 119 #define AXIS3_MAX_TORQUE 55
stanley1228 1:584f36ed8717 120 #define AXIS4_MAX_TORQUE 55
stanley1228 1:584f36ed8717 121 #define AXIS5_MAX_TORQUE 55
stanley1228 1:584f36ed8717 122 #define AXIS6_MAX_TORQUE 55
stanley1228 1:584f36ed8717 123 #define AXIS7_MAX_TORQUE 55
stanley1228 1:584f36ed8717 124
stanley1228 1:584f36ed8717 125 static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 126 {
stanley1228 1:584f36ed8717 127 (unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 128 (unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 129 (unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 130 (unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 131 (unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 132 (unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
stanley1228 1:584f36ed8717 133 (unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS)
stanley1228 1:584f36ed8717 134 };
stanley1228 1:584f36ed8717 135
stanley1228 1:584f36ed8717 136
stanley1228 1:584f36ed8717 137 static const float grobot_lim_rad_L[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 138 {
stanley1228 1:584f36ed8717 139 AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 140 AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 141 AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 142 AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 143 AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 144 AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 145 AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD
stanley1228 1:584f36ed8717 146 };
stanley1228 1:584f36ed8717 147
stanley1228 1:584f36ed8717 148 static const float grobot_lim_rad_H[MAX_AXIS_NUM]=
stanley1228 1:584f36ed8717 149 {
stanley1228 1:584f36ed8717 150 AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 151 AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 152 AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 153 AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 154 AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 155 AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
stanley1228 1:584f36ed8717 156 AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD
stanley1228 1:584f36ed8717 157 };
stanley1228 1:584f36ed8717 158 //=================================
stanley1228 1:584f36ed8717 159 //==morot max pulse in joint mode
stanley1228 1:584f36ed8717 160 //=================================
stanley1228 1:584f36ed8717 161 #define DEF_JOINT_MODE_MAX_PULSE 4095
stanley1228 1:584f36ed8717 162 #define DEF_JOINT_MODE_MIN_PULSE 0
stanley1228 1:584f36ed8717 163
stanley1228 1:584f36ed8717 164
stanley1228 1:584f36ed8717 165
stanley1228 1:584f36ed8717 166 //Function
stanley1228 1:584f36ed8717 167 unsigned char getMapAxisNO(unsigned char index); //index 0~ (MAX_AXIS_NUM-1)
stanley1228 1:584f36ed8717 168 unsigned char getMapAxisID(unsigned char index);
stanley1228 1:584f36ed8717 169 int ROM_Setting();
stanley1228 1:584f36ed8717 170 int Read_pos(float *pos,unsigned char unit);
stanley1228 1:584f36ed8717 171 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ;
stanley1228 1:584f36ed8717 172
stanley1228 1:584f36ed8717 173
stanley1228 1:584f36ed8717 174
stanley1228 1:584f36ed8717 175
stanley1228 1:584f36ed8717 176 #endif