hsu han-lin
RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
RobotControl_7Axis.cpp@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?
| User | Revision | Line number | New 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 | 2:71ed7da9ea36 | 5 | #include <vector> |
| stanley1228 | 2:71ed7da9ea36 | 6 | #include "Matrix.h" |
| stanley1228 | 2:71ed7da9ea36 | 7 | #include "MatrixMath.h" |
| stanley1228 | 2:71ed7da9ea36 | 8 | #include <math.h> |
| stanley1228 | 2:71ed7da9ea36 | 9 | |
| stanley1228 | 2:71ed7da9ea36 | 10 | |
| stanley1228 | 1:584f36ed8717 | 11 | extern Serial pc; |
| stanley1228 | 1:584f36ed8717 | 12 | |
| stanley1228 | 1:584f36ed8717 | 13 | |
| stanley1228 | 1:584f36ed8717 | 14 | #if (DEBUG) |
| stanley1228 | 1:584f36ed8717 | 15 | #define DBGMSG(x) pc.printf x; |
| stanley1228 | 1:584f36ed8717 | 16 | #else |
| stanley1228 | 1:584f36ed8717 | 17 | #define DBGMSG(x) |
| stanley1228 | 1:584f36ed8717 | 18 | #endif |
| stanley1228 | 1:584f36ed8717 | 19 | |
| stanley1228 | 1:584f36ed8717 | 20 | |
| stanley1228 | 1:584f36ed8717 | 21 | |
| stanley1228 | 1:584f36ed8717 | 22 | unsigned char getMapAxisNO(unsigned char index) |
| stanley1228 | 1:584f36ed8717 | 23 | { |
| stanley1228 | 1:584f36ed8717 | 24 | return gMapAxisNO[index]; |
| stanley1228 | 1:584f36ed8717 | 25 | |
| stanley1228 | 1:584f36ed8717 | 26 | } |
| stanley1228 | 1:584f36ed8717 | 27 | |
| stanley1228 | 1:584f36ed8717 | 28 | unsigned char getMapAxisID(unsigned char index) |
| stanley1228 | 1:584f36ed8717 | 29 | { |
| stanley1228 | 1:584f36ed8717 | 30 | return gMapAxisID[index]; |
| stanley1228 | 1:584f36ed8717 | 31 | } |
| stanley1228 | 1:584f36ed8717 | 32 | |
| stanley1228 | 1:584f36ed8717 | 33 | |
| stanley1228 | 1:584f36ed8717 | 34 | int ROM_Setting() //new |
| stanley1228 | 1:584f36ed8717 | 35 | { |
| stanley1228 | 1:584f36ed8717 | 36 | //==calculate Max torque to set in rom |
| stanley1228 | 1:584f36ed8717 | 37 | const short int Max_torque[MAX_AXIS_NUM]= |
| stanley1228 | 1:584f36ed8717 | 38 | { |
| stanley1228 | 1:584f36ed8717 | 39 | AXIS1_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 40 | AXIS2_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 41 | AXIS3_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 42 | AXIS4_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 43 | AXIS5_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 44 | AXIS6_MAX_TORQUE, |
| stanley1228 | 1:584f36ed8717 | 45 | AXIS7_MAX_TORQUE |
| stanley1228 | 1:584f36ed8717 | 46 | }; |
| stanley1228 | 1:584f36ed8717 | 47 | |
| stanley1228 | 1:584f36ed8717 | 48 | |
| stanley1228 | 1:584f36ed8717 | 49 | //==Calculate angle limit==// |
| stanley1228 | 1:584f36ed8717 | 50 | const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]= |
| stanley1228 | 1:584f36ed8717 | 51 | { |
| stanley1228 | 1:584f36ed8717 | 52 | AXIS1_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 53 | AXIS2_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 54 | AXIS3_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 55 | AXIS4_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 56 | AXIS5_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 57 | AXIS6_R2M_OFFSET_DEG, |
| stanley1228 | 1:584f36ed8717 | 58 | AXIS7_R2M_OFFSET_DEG |
| stanley1228 | 1:584f36ed8717 | 59 | }; |
| stanley1228 | 1:584f36ed8717 | 60 | |
| stanley1228 | 1:584f36ed8717 | 61 | const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]= |
| stanley1228 | 1:584f36ed8717 | 62 | { |
| stanley1228 | 1:584f36ed8717 | 63 | AXIS1_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 64 | AXIS2_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 65 | AXIS3_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 66 | AXIS4_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 67 | AXIS5_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 68 | AXIS6_ROBOT_LIM_DEG_L, |
| stanley1228 | 1:584f36ed8717 | 69 | AXIS7_ROBOT_LIM_DEG_L |
| stanley1228 | 1:584f36ed8717 | 70 | }; |
| stanley1228 | 1:584f36ed8717 | 71 | |
| stanley1228 | 1:584f36ed8717 | 72 | const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]= |
| stanley1228 | 1:584f36ed8717 | 73 | { |
| stanley1228 | 1:584f36ed8717 | 74 | AXIS1_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 75 | AXIS2_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 76 | AXIS3_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 77 | AXIS4_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 78 | AXIS5_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 79 | AXIS6_ROBOT_LIM_DEG_H, |
| stanley1228 | 1:584f36ed8717 | 80 | AXIS7_ROBOT_LIM_DEG_H |
| stanley1228 | 1:584f36ed8717 | 81 | }; |
| stanley1228 | 1:584f36ed8717 | 82 | |
| stanley1228 | 1:584f36ed8717 | 83 | unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0}; |
| stanley1228 | 1:584f36ed8717 | 84 | unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0}; |
| stanley1228 | 1:584f36ed8717 | 85 | |
| stanley1228 | 1:584f36ed8717 | 86 | |
| stanley1228 | 1:584f36ed8717 | 87 | int i=0; |
| stanley1228 | 1:584f36ed8717 | 88 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 89 | { |
| stanley1228 | 1:584f36ed8717 | 90 | Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS; |
| stanley1228 | 1:584f36ed8717 | 91 | Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS; |
| stanley1228 | 1:584f36ed8717 | 92 | } |
| stanley1228 | 1:584f36ed8717 | 93 | |
| stanley1228 | 1:584f36ed8717 | 94 | |
| stanley1228 | 1:584f36ed8717 | 95 | |
| stanley1228 | 1:584f36ed8717 | 96 | //==writing to ROM==// |
| stanley1228 | 1:584f36ed8717 | 97 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 98 | { |
| stanley1228 | 1:584f36ed8717 | 99 | //==Set MAX_torgue==// |
| stanley1228 | 1:584f36ed8717 | 100 | dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);// more safe |
| stanley1228 | 1:584f36ed8717 | 101 | |
| stanley1228 | 1:584f36ed8717 | 102 | //==Set angel limit==// |
| stanley1228 | 1:584f36ed8717 | 103 | dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]); |
| stanley1228 | 1:584f36ed8717 | 104 | dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]); |
| stanley1228 | 1:584f36ed8717 | 105 | |
| stanley1228 | 1:584f36ed8717 | 106 | //==Set MULTITURN_OFFSET to 0==// |
| stanley1228 | 1:584f36ed8717 | 107 | dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0); |
| stanley1228 | 1:584f36ed8717 | 108 | } |
| stanley1228 | 1:584f36ed8717 | 109 | |
| stanley1228 | 1:584f36ed8717 | 110 | |
| stanley1228 | 1:584f36ed8717 | 111 | //==read and check ==// |
| stanley1228 | 1:584f36ed8717 | 112 | int txrx_result=0; |
| stanley1228 | 1:584f36ed8717 | 113 | short int max_torque=0; |
| stanley1228 | 1:584f36ed8717 | 114 | short int cw_angel_lim=0,ccw_angle_lim=0; |
| stanley1228 | 1:584f36ed8717 | 115 | short int multi_turn_offset=0; |
| stanley1228 | 1:584f36ed8717 | 116 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 117 | { |
| stanley1228 | 1:584f36ed8717 | 118 | pc.printf("===AXIS_%d===\n",gMapAxisNO[i]); |
| stanley1228 | 1:584f36ed8717 | 119 | |
| stanley1228 | 1:584f36ed8717 | 120 | //==MAX_torgue==// |
| stanley1228 | 1:584f36ed8717 | 121 | max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE); |
| stanley1228 | 1:584f36ed8717 | 122 | txrx_result = dxl_get_result(); |
| stanley1228 | 1:584f36ed8717 | 123 | if(txrx_result!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 124 | pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result); |
| stanley1228 | 1:584f36ed8717 | 125 | else |
| stanley1228 | 1:584f36ed8717 | 126 | pc.printf("MAX_TORQUE=%d\n",max_torque); |
| stanley1228 | 1:584f36ed8717 | 127 | |
| stanley1228 | 1:584f36ed8717 | 128 | //==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==// |
| stanley1228 | 1:584f36ed8717 | 129 | cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L); |
| stanley1228 | 1:584f36ed8717 | 130 | txrx_result = dxl_get_result(); |
| stanley1228 | 1:584f36ed8717 | 131 | if(txrx_result!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 132 | pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result); |
| stanley1228 | 1:584f36ed8717 | 133 | else |
| stanley1228 | 1:584f36ed8717 | 134 | pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG); |
| stanley1228 | 1:584f36ed8717 | 135 | |
| stanley1228 | 1:584f36ed8717 | 136 | ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L); |
| stanley1228 | 1:584f36ed8717 | 137 | txrx_result = dxl_get_result(); |
| stanley1228 | 1:584f36ed8717 | 138 | if(txrx_result!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 139 | pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result); |
| stanley1228 | 1:584f36ed8717 | 140 | else |
| stanley1228 | 1:584f36ed8717 | 141 | pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG); |
| stanley1228 | 1:584f36ed8717 | 142 | |
| stanley1228 | 1:584f36ed8717 | 143 | |
| stanley1228 | 1:584f36ed8717 | 144 | //==multi turn offset==// |
| stanley1228 | 1:584f36ed8717 | 145 | multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET); |
| stanley1228 | 1:584f36ed8717 | 146 | txrx_result = dxl_get_result(); |
| stanley1228 | 1:584f36ed8717 | 147 | if(txrx_result!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 148 | pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result); |
| stanley1228 | 1:584f36ed8717 | 149 | else |
| stanley1228 | 1:584f36ed8717 | 150 | pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset); |
| stanley1228 | 1:584f36ed8717 | 151 | } |
| stanley1228 | 1:584f36ed8717 | 152 | |
| stanley1228 | 1:584f36ed8717 | 153 | return 0; |
| stanley1228 | 1:584f36ed8717 | 154 | } |
| stanley1228 | 1:584f36ed8717 | 155 | |
| stanley1228 | 1:584f36ed8717 | 156 | |
| stanley1228 | 1:584f36ed8717 | 157 | |
| stanley1228 | 1:584f36ed8717 | 158 | |
| stanley1228 | 1:584f36ed8717 | 159 | int Read_pos(float *pos,unsigned char unit) |
| stanley1228 | 1:584f36ed8717 | 160 | { |
| stanley1228 | 1:584f36ed8717 | 161 | int i=0; |
| stanley1228 | 1:584f36ed8717 | 162 | short int pulse=0; |
| stanley1228 | 1:584f36ed8717 | 163 | int rt=0; |
| stanley1228 | 1:584f36ed8717 | 164 | |
| stanley1228 | 1:584f36ed8717 | 165 | if(unit==DEF_UNIT_RAD) |
| stanley1228 | 1:584f36ed8717 | 166 | { |
| stanley1228 | 1:584f36ed8717 | 167 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 168 | { |
| stanley1228 | 1:584f36ed8717 | 169 | pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); |
| stanley1228 | 1:584f36ed8717 | 170 | if(dxl_get_result()!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 171 | { |
| stanley1228 | 1:584f36ed8717 | 172 | rt=-gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 173 | pos[i]=0xffff; |
| stanley1228 | 1:584f36ed8717 | 174 | } |
| stanley1228 | 1:584f36ed8717 | 175 | else |
| stanley1228 | 1:584f36ed8717 | 176 | { |
| stanley1228 | 1:584f36ed8717 | 177 | pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset |
| stanley1228 | 1:584f36ed8717 | 178 | pos[i]=pulse*DEF_RATIO_PUS_TO_RAD; |
| stanley1228 | 1:584f36ed8717 | 179 | } |
| stanley1228 | 1:584f36ed8717 | 180 | } |
| stanley1228 | 1:584f36ed8717 | 181 | |
| stanley1228 | 1:584f36ed8717 | 182 | } |
| stanley1228 | 1:584f36ed8717 | 183 | else if(unit==DEF_UNIT_DEG) |
| stanley1228 | 1:584f36ed8717 | 184 | { |
| stanley1228 | 1:584f36ed8717 | 185 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 186 | { |
| stanley1228 | 1:584f36ed8717 | 187 | pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); |
| stanley1228 | 1:584f36ed8717 | 188 | if(dxl_get_result()!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 189 | { |
| stanley1228 | 1:584f36ed8717 | 190 | rt=-gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 191 | pos[i]=0xffff; |
| stanley1228 | 1:584f36ed8717 | 192 | } |
| stanley1228 | 1:584f36ed8717 | 193 | else |
| stanley1228 | 1:584f36ed8717 | 194 | { |
| stanley1228 | 1:584f36ed8717 | 195 | pulse-=gr2m_offset_pulse[i]; |
| stanley1228 | 1:584f36ed8717 | 196 | pos[i]=pulse*DEF_RATIO_PUS_TO_DEG; |
| stanley1228 | 1:584f36ed8717 | 197 | } |
| stanley1228 | 1:584f36ed8717 | 198 | } |
| stanley1228 | 1:584f36ed8717 | 199 | } |
| stanley1228 | 1:584f36ed8717 | 200 | else if(unit==DEF_UNIT_PUS) |
| stanley1228 | 1:584f36ed8717 | 201 | { |
| stanley1228 | 1:584f36ed8717 | 202 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 203 | { |
| stanley1228 | 1:584f36ed8717 | 204 | pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); |
| stanley1228 | 1:584f36ed8717 | 205 | if(dxl_get_result()!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 206 | { |
| stanley1228 | 1:584f36ed8717 | 207 | rt=-gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 208 | pos[i]=0xffff; |
| stanley1228 | 1:584f36ed8717 | 209 | } |
| stanley1228 | 1:584f36ed8717 | 210 | else |
| stanley1228 | 1:584f36ed8717 | 211 | { |
| stanley1228 | 1:584f36ed8717 | 212 | pulse-=gr2m_offset_pulse[i]; |
| stanley1228 | 1:584f36ed8717 | 213 | pos[i]=pulse; |
| stanley1228 | 1:584f36ed8717 | 214 | } |
| stanley1228 | 1:584f36ed8717 | 215 | } |
| stanley1228 | 1:584f36ed8717 | 216 | |
| stanley1228 | 1:584f36ed8717 | 217 | } |
| stanley1228 | 1:584f36ed8717 | 218 | else |
| stanley1228 | 1:584f36ed8717 | 219 | { |
| stanley1228 | 1:584f36ed8717 | 220 | //non offset value |
| stanley1228 | 1:584f36ed8717 | 221 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 222 | { |
| stanley1228 | 1:584f36ed8717 | 223 | pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS); |
| stanley1228 | 1:584f36ed8717 | 224 | if(dxl_get_result()!=COMM_RXSUCCESS) |
| stanley1228 | 1:584f36ed8717 | 225 | { |
| stanley1228 | 1:584f36ed8717 | 226 | rt=-gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 227 | pos[i]=0xffff; |
| stanley1228 | 1:584f36ed8717 | 228 | } |
| stanley1228 | 1:584f36ed8717 | 229 | } |
| stanley1228 | 1:584f36ed8717 | 230 | } |
| stanley1228 | 1:584f36ed8717 | 231 | |
| stanley1228 | 1:584f36ed8717 | 232 | return rt; |
| stanley1228 | 1:584f36ed8717 | 233 | } |
| stanley1228 | 1:584f36ed8717 | 234 | |
| stanley1228 | 1:584f36ed8717 | 235 | |
| stanley1228 | 1:584f36ed8717 | 236 | |
| stanley1228 | 1:584f36ed8717 | 237 | |
| stanley1228 | 1:584f36ed8717 | 238 | int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) |
| stanley1228 | 1:584f36ed8717 | 239 | { |
| stanley1228 | 1:584f36ed8717 | 240 | unsigned char i=0; |
| stanley1228 | 1:584f36ed8717 | 241 | |
| stanley1228 | 1:584f36ed8717 | 242 | //===================================================================// |
| stanley1228 | 1:584f36ed8717 | 243 | //==limit axis if not zero ,the return value is the overlimit axis==// |
| stanley1228 | 1:584f36ed8717 | 244 | //===================================================================// |
| stanley1228 | 1:584f36ed8717 | 245 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 246 | { |
| stanley1228 | 1:584f36ed8717 | 247 | if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) ) |
| stanley1228 | 1:584f36ed8717 | 248 | { |
| stanley1228 | 1:584f36ed8717 | 249 | 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 | 250 | return -gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 251 | } |
| stanley1228 | 1:584f36ed8717 | 252 | } |
| stanley1228 | 1:584f36ed8717 | 253 | |
| stanley1228 | 1:584f36ed8717 | 254 | //===================================================// |
| stanley1228 | 1:584f36ed8717 | 255 | //==trnasformat to pulse and offset to motor domain==// |
| stanley1228 | 1:584f36ed8717 | 256 | //====================================================// |
| stanley1228 | 1:584f36ed8717 | 257 | short int Ang_pulse[MAX_AXIS_NUM]={0}; |
| stanley1228 | 1:584f36ed8717 | 258 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 259 | { |
| stanley1228 | 1:584f36ed8717 | 260 | Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS); |
| stanley1228 | 1:584f36ed8717 | 261 | Ang_pulse[i]+=gr2m_offset_pulse[i]; |
| stanley1228 | 1:584f36ed8717 | 262 | |
| stanley1228 | 1:584f36ed8717 | 263 | if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )// 0~4095 |
| stanley1228 | 1:584f36ed8717 | 264 | { |
| stanley1228 | 1:584f36ed8717 | 265 | 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 | 266 | return -gMapAxisNO[i]; |
| stanley1228 | 1:584f36ed8717 | 267 | } |
| stanley1228 | 1:584f36ed8717 | 268 | } |
| stanley1228 | 1:584f36ed8717 | 269 | |
| stanley1228 | 1:584f36ed8717 | 270 | //================================// |
| stanley1228 | 1:584f36ed8717 | 271 | //==output to motor by syncpage===// |
| stanley1228 | 1:584f36ed8717 | 272 | //===============================// |
| stanley1228 | 1:584f36ed8717 | 273 | unsigned short int SyncPage1[21]= |
| stanley1228 | 1:584f36ed8717 | 274 | { |
| stanley1228 | 1:584f36ed8717 | 275 | ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity |
| stanley1228 | 1:584f36ed8717 | 276 | ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2], |
| stanley1228 | 1:584f36ed8717 | 277 | ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3], |
| stanley1228 | 1:584f36ed8717 | 278 | ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4], |
| stanley1228 | 1:584f36ed8717 | 279 | ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5], |
| stanley1228 | 1:584f36ed8717 | 280 | ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6], |
| stanley1228 | 1:584f36ed8717 | 281 | ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7], |
| stanley1228 | 1:584f36ed8717 | 282 | }; |
| stanley1228 | 1:584f36ed8717 | 283 | |
| stanley1228 | 1:584f36ed8717 | 284 | |
| stanley1228 | 1:584f36ed8717 | 285 | #if (DEBUG) |
| stanley1228 | 1:584f36ed8717 | 286 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 1:584f36ed8717 | 287 | pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]); |
| stanley1228 | 1:584f36ed8717 | 288 | #endif |
| stanley1228 | 1:584f36ed8717 | 289 | |
| stanley1228 | 1:584f36ed8717 | 290 | syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length); |
| stanley1228 | 1:584f36ed8717 | 291 | |
| stanley1228 | 1:584f36ed8717 | 292 | return 0; |
| stanley1228 | 1:584f36ed8717 | 293 | } |
| stanley1228 | 1:584f36ed8717 | 294 | |
| stanley1228 | 2:71ed7da9ea36 | 295 | |
| stanley1228 | 2:71ed7da9ea36 | 296 | |
| stanley1228 | 2:71ed7da9ea36 | 297 | |
| stanley1228 | 2:71ed7da9ea36 | 298 | //歐拉 Z1X2Y3 Intrinsic Rotaions相對於當前坐標系的的旋轉 |
| stanley1228 | 2:71ed7da9ea36 | 299 | Matrix R_z1x2y3(float alpha,float beta,float gamma) |
| stanley1228 | 2:71ed7da9ea36 | 300 | { |
| stanley1228 | 2:71ed7da9ea36 | 301 | Matrix ans(3,3); |
| stanley1228 | 2:71ed7da9ea36 | 302 | ans << cos(alpha)*cos(gamma)-sin(alpha)*sin(beta)*sin(gamma) << -cos(beta)*sin(alpha) << cos(alpha)*sin(gamma)+cos(gamma)*sin(alpha)*sin(beta) |
| stanley1228 | 2:71ed7da9ea36 | 303 | << cos(gamma)*sin(alpha)+cos(alpha)*sin(beta)*sin(gamma) << cos(alpha)*cos(beta) << sin(alpha)*sin(gamma)-cos(alpha)*cos(gamma)*sin(beta) |
| stanley1228 | 2:71ed7da9ea36 | 304 | << -cos(beta)*sin(gamma) << sin(beta) << cos(beta)*cos(gamma); |
| stanley1228 | 2:71ed7da9ea36 | 305 | |
| stanley1228 | 2:71ed7da9ea36 | 306 | |
| stanley1228 | 2:71ed7da9ea36 | 307 | return ans; |
| stanley1228 | 2:71ed7da9ea36 | 308 | } |
| stanley1228 | 2:71ed7da9ea36 | 309 | |
| stanley1228 | 2:71ed7da9ea36 | 310 | float norm(const Matrix& v) |
| stanley1228 | 2:71ed7da9ea36 | 311 | { |
| stanley1228 | 2:71ed7da9ea36 | 312 | int i=0; |
| stanley1228 | 2:71ed7da9ea36 | 313 | float ans=0; |
| stanley1228 | 2:71ed7da9ea36 | 314 | |
| stanley1228 | 2:71ed7da9ea36 | 315 | for(i=1;i<=v.getRows();i++) |
| stanley1228 | 2:71ed7da9ea36 | 316 | { |
| stanley1228 | 2:71ed7da9ea36 | 317 | ans+=pow(v(i,1),2); |
| stanley1228 | 2:71ed7da9ea36 | 318 | } |
| stanley1228 | 2:71ed7da9ea36 | 319 | |
| stanley1228 | 2:71ed7da9ea36 | 320 | ans=sqrt(ans); |
| stanley1228 | 2:71ed7da9ea36 | 321 | |
| stanley1228 | 2:71ed7da9ea36 | 322 | return ans; |
| stanley1228 | 2:71ed7da9ea36 | 323 | } |
| stanley1228 | 2:71ed7da9ea36 | 324 | |
| stanley1228 | 2:71ed7da9ea36 | 325 | |
| stanley1228 | 2:71ed7da9ea36 | 326 | Matrix Rogridues(float theta,const Matrix& V_A) |
| stanley1228 | 2:71ed7da9ea36 | 327 | { |
| stanley1228 | 2:71ed7da9ea36 | 328 | Matrix R_a(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 329 | |
| stanley1228 | 2:71ed7da9ea36 | 330 | float cs = cos( theta ); |
| stanley1228 | 2:71ed7da9ea36 | 331 | float sn = sin( theta ); |
| stanley1228 | 2:71ed7da9ea36 | 332 | |
| stanley1228 | 2:71ed7da9ea36 | 333 | R_a << cs + pow(V_A.getNumber(1,1),2)*(1-cs) << V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)-V_A.getNumber(3,1)*sn << V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(2,1)*sn << 0 |
| stanley1228 | 2:71ed7da9ea36 | 334 | << V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)+V_A.getNumber(3,1)*sn << cos(theta)+pow(V_A.getNumber(2,1),2)*(1-cs) << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(1,1)*sn << 0 |
| stanley1228 | 2:71ed7da9ea36 | 335 | << V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(2,1)*sn << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(1,1)*sn << cs+pow(V_A.getNumber(3,1),2)*(1-cs) << 0 |
| stanley1228 | 2:71ed7da9ea36 | 336 | << 0 << 0 << 0 << 1; |
| stanley1228 | 2:71ed7da9ea36 | 337 | |
| stanley1228 | 2:71ed7da9ea36 | 338 | return R_a; |
| stanley1228 | 2:71ed7da9ea36 | 339 | } |
| stanley1228 | 2:71ed7da9ea36 | 340 | //enum{ |
| stanley1228 | 2:71ed7da9ea36 | 341 | // AXIS1=0, |
| stanley1228 | 2:71ed7da9ea36 | 342 | // AXIS2, |
| stanley1228 | 2:71ed7da9ea36 | 343 | // AXIS3, |
| stanley1228 | 2:71ed7da9ea36 | 344 | // AXIS4, |
| stanley1228 | 2:71ed7da9ea36 | 345 | // AXIS5, |
| stanley1228 | 2:71ed7da9ea36 | 346 | // AXIS6, |
| stanley1228 | 2:71ed7da9ea36 | 347 | // AXIS7 |
| stanley1228 | 2:71ed7da9ea36 | 348 | //}; |
| stanley1228 | 2:71ed7da9ea36 | 349 | |
| stanley1228 | 2:71ed7da9ea36 | 350 | 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 | 2:71ed7da9ea36 | 351 | { |
| stanley1228 | 2:71ed7da9ea36 | 352 | int i=0; |
| stanley1228 | 2:71ed7da9ea36 | 353 | |
| stanley1228 | 2:71ed7da9ea36 | 354 | //Out put initial to zero |
| stanley1228 | 2:71ed7da9ea36 | 355 | for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) |
| stanley1228 | 2:71ed7da9ea36 | 356 | { |
| stanley1228 | 2:71ed7da9ea36 | 357 | theta[i]=0; |
| stanley1228 | 2:71ed7da9ea36 | 358 | } |
| stanley1228 | 2:71ed7da9ea36 | 359 | |
| stanley1228 | 2:71ed7da9ea36 | 360 | Matrix R(3,3); |
| stanley1228 | 2:71ed7da9ea36 | 361 | R=R_z1x2y3(alpha,beta,gamma); |
| stanley1228 | 2:71ed7da9ea36 | 362 | |
| stanley1228 | 2:71ed7da9ea36 | 363 | Matrix V_H_hat_x(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 364 | V_H_hat_x=Matrix::ExportCol(R,1);//取出歐拉角轉換的旋轉矩陣，取出第1行為X軸旋轉後向量 |
| stanley1228 | 2:71ed7da9ea36 | 365 | V_H_hat_x*=1/norm(V_H_hat_x); |
| stanley1228 | 2:71ed7da9ea36 | 366 | |
| stanley1228 | 2:71ed7da9ea36 | 367 | Matrix V_H_hat_y(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 368 | V_H_hat_y=Matrix::ExportCol(R,2);//取出歐拉角轉換的旋轉矩陣，取出第2行為Y軸旋轉後向量 |
| stanley1228 | 2:71ed7da9ea36 | 369 | V_H_hat_y*=1/norm(V_H_hat_y); |
| stanley1228 | 2:71ed7da9ea36 | 370 | |
| stanley1228 | 2:71ed7da9ea36 | 371 | |
| stanley1228 | 2:71ed7da9ea36 | 372 | Matrix V_r_end(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 373 | V_r_end <<x_end-x_base |
| stanley1228 | 2:71ed7da9ea36 | 374 | <<y_end-y_base |
| stanley1228 | 2:71ed7da9ea36 | 375 | <<z_end-z_base; |
| stanley1228 | 2:71ed7da9ea36 | 376 | |
| stanley1228 | 2:71ed7da9ea36 | 377 | |
| stanley1228 | 2:71ed7da9ea36 | 378 | Matrix V_r_h(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 379 | V_r_h=V_H_hat_x*L3; |
| stanley1228 | 2:71ed7da9ea36 | 380 | |
| stanley1228 | 2:71ed7da9ea36 | 381 | Matrix V_r_wst(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 382 | V_r_wst=V_r_end-V_r_h; |
| stanley1228 | 2:71ed7da9ea36 | 383 | |
| stanley1228 | 2:71ed7da9ea36 | 384 | //theat 4 |
| stanley1228 | 2:71ed7da9ea36 | 385 | theta[Index_AXIS4]=-(float)(DEF_PI-acos((pow(l1,2)+pow(l2,2)-pow(norm(V_r_wst),2))/(2*l1*l2))); |
| stanley1228 | 2:71ed7da9ea36 | 386 | |
| stanley1228 | 2:71ed7da9ea36 | 387 | |
| stanley1228 | 2:71ed7da9ea36 | 388 | Matrix V_r_m(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 389 | V_r_m=(pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*pow(norm(V_r_wst),2))*V_r_wst; |
| stanley1228 | 2:71ed7da9ea36 | 390 | |
| stanley1228 | 2:71ed7da9ea36 | 391 | |
| stanley1228 | 2:71ed7da9ea36 | 392 | |
| stanley1228 | 2:71ed7da9ea36 | 393 | //Redundant circle 半徑R |
| stanley1228 | 2:71ed7da9ea36 | 394 | float Rednt_cir_R = pow(l1,2)- pow( (pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*norm(V_r_wst)) , 2); |
| stanley1228 | 2:71ed7da9ea36 | 395 | Rednt_cir_R=sqrt(Rednt_cir_R); |
| stanley1228 | 2:71ed7da9ea36 | 396 | |
| stanley1228 | 2:71ed7da9ea36 | 397 | |
| stanley1228 | 2:71ed7da9ea36 | 398 | //圓中心點到Elbow向量 V_r_u |
| stanley1228 | 2:71ed7da9ea36 | 399 | Matrix V_shz(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 400 | V_shz <<0 |
| stanley1228 | 2:71ed7da9ea36 | 401 | <<0 |
| stanley1228 | 2:71ed7da9ea36 | 402 | <<1; |
| stanley1228 | 2:71ed7da9ea36 | 403 | |
| stanley1228 | 2:71ed7da9ea36 | 404 | Matrix V_alpha_hat(3,1);//V_alpha_hat=cross(V_r_wst,V_shz)/norm(cross(V_r_wst,V_shz)); |
| stanley1228 | 2:71ed7da9ea36 | 405 | Matrix temp_cross(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 406 | temp_cross=MatrixMath::cross(V_r_wst,V_shz); //錯誤 |
| stanley1228 | 2:71ed7da9ea36 | 407 | V_alpha_hat=temp_cross*(1/norm(temp_cross)); |
| stanley1228 | 2:71ed7da9ea36 | 408 | |
| stanley1228 | 2:71ed7da9ea36 | 409 | Matrix V_beta_hat(3,1);//V_beta_hat=cross(V_r_wst,V_alpha_hat)/norm(cross(V_r_wst,V_alpha_hat)); |
| stanley1228 | 2:71ed7da9ea36 | 410 | temp_cross=MatrixMath::cross(V_r_wst,V_alpha_hat); |
| stanley1228 | 2:71ed7da9ea36 | 411 | V_beta_hat=temp_cross*(1/norm(temp_cross)); |
| stanley1228 | 2:71ed7da9ea36 | 412 | |
| stanley1228 | 2:71ed7da9ea36 | 413 | |
| stanley1228 | 2:71ed7da9ea36 | 414 | |
| stanley1228 | 2:71ed7da9ea36 | 415 | Matrix temp(4,4);//temp=Rogridues(Rednt_alpha,V_r_wst/norm(V_r_wst)) *[Rednt_cir_R*V_beta_hat;1]; %Rednt_alpha的方向和論文上的方向性相反 |
| stanley1228 | 2:71ed7da9ea36 | 416 | Matrix V_r_wst_unit =V_r_wst*(1/norm(V_r_wst)); |
| stanley1228 | 2:71ed7da9ea36 | 417 | Matrix V_temp3x1(3,1);//需要寫一個可以補1的函試 |
| stanley1228 | 2:71ed7da9ea36 | 418 | Matrix V_temp4x1(4,1); |
| stanley1228 | 2:71ed7da9ea36 | 419 | V_temp3x1=V_beta_hat*Rednt_cir_R; |
| stanley1228 | 2:71ed7da9ea36 | 420 | V_temp4x1.Vec_ext_1_row(V_temp3x1,1); //3x1 extend to 4x1 |
| stanley1228 | 2:71ed7da9ea36 | 421 | |
| stanley1228 | 2:71ed7da9ea36 | 422 | temp=Rogridues(Rednt_alpha,V_r_wst_unit)*V_temp4x1; |
| stanley1228 | 2:71ed7da9ea36 | 423 | |
| stanley1228 | 2:71ed7da9ea36 | 424 | |
| stanley1228 | 2:71ed7da9ea36 | 425 | Matrix V_R_u(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 426 | V_R_u.Vec_export_3_row(temp); |
| stanley1228 | 2:71ed7da9ea36 | 427 | |
| stanley1228 | 2:71ed7da9ea36 | 428 | |
| stanley1228 | 2:71ed7da9ea36 | 429 | Matrix V_r_u(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 430 | V_r_u=V_r_m+V_R_u; |
| stanley1228 | 2:71ed7da9ea36 | 431 | |
| stanley1228 | 2:71ed7da9ea36 | 432 | theta[Index_AXIS1]=atan2(-V_r_u(1,1),-V_r_u(3,1));//theta(1)=atan2(-V_r_u(1),-V_r_u(3)); |
| stanley1228 | 2:71ed7da9ea36 | 433 | |
| stanley1228 | 2:71ed7da9ea36 | 434 | |
| stanley1228 | 2:71ed7da9ea36 | 435 | if (theta[Index_AXIS1] !=0) |
| stanley1228 | 2:71ed7da9ea36 | 436 | theta[Index_AXIS2]=atan2(V_r_u(2,1),-V_r_u(1,1)/sin(theta[Index_AXIS1])); |
| stanley1228 | 2:71ed7da9ea36 | 437 | else |
| stanley1228 | 2:71ed7da9ea36 | 438 | theta[Index_AXIS2]=atan2(-V_r_u(2,1),V_r_u(3,1)); |
| stanley1228 | 2:71ed7da9ea36 | 439 | |
| stanley1228 | 2:71ed7da9ea36 | 440 | |
| stanley1228 | 2:71ed7da9ea36 | 441 | |
| stanley1228 | 2:71ed7da9ea36 | 442 | //theat 3 |
| stanley1228 | 2:71ed7da9ea36 | 443 | //theta(3)=atan2( sin(theta(2))*sin(theta(1))*V_r_wst(1)+cos(theta(2))*V_r_wst(2)+sin(theta(2))*cos(theta(1))*V_r_wst(3),cos(theta(1))*V_r_wst(1)-sin(theta(1))*V_r_wst(3)); |
| stanley1228 | 2:71ed7da9ea36 | 444 | theta[Index_AXIS3]=atan2( sin(theta[Index_AXIS2])*sin(theta[Index_AXIS1])*V_r_wst(1,1)+cos(theta[Index_AXIS2])*V_r_wst(2,1)+sin(theta[Index_AXIS2])*cos(theta[Index_AXIS1])*V_r_wst(3,1),cos(theta[Index_AXIS1])*V_r_wst(1,1)-sin(theta[Index_AXIS1])*V_r_wst(3,1)); |
| stanley1228 | 2:71ed7da9ea36 | 445 | |
| stanley1228 | 2:71ed7da9ea36 | 446 | |
| stanley1228 | 2:71ed7da9ea36 | 447 | |
| stanley1228 | 2:71ed7da9ea36 | 448 | //theat 5 |
| stanley1228 | 2:71ed7da9ea36 | 449 | Matrix V_r_f(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 450 | V_r_f=V_r_wst-V_r_u; |
| stanley1228 | 2:71ed7da9ea36 | 451 | |
| stanley1228 | 2:71ed7da9ea36 | 452 | Matrix V_Axis6(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 453 | V_Axis6=MatrixMath::cross(V_H_hat_y,-V_r_f)*(1/norm(MatrixMath::cross(V_H_hat_y,-V_r_f)));//V_Axis6=cross(V_H_hat_y,-V_r_f)/norm(cross(V_H_hat_y,-V_r_f)); |
| stanley1228 | 2:71ed7da9ea36 | 454 | |
| stanley1228 | 2:71ed7da9ea36 | 455 | Matrix V_r_wst_u(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 456 | V_r_wst_u=V_r_wst+V_Axis6; |
| stanley1228 | 2:71ed7da9ea36 | 457 | |
| stanley1228 | 2:71ed7da9ea36 | 458 | Matrix A1_4(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 459 | A1_4=MatrixMath::RotY(theta[Index_AXIS1])*MatrixMath::RotX(theta[Index_AXIS2])*MatrixMath::RotZ(theta[Index_AXIS3])*MatrixMath::Tz(-l1)*MatrixMath::RotY(theta[Index_AXIS4]);//A1_4=Ry(theta(1))*Rx(theta(2))*Rz(theta(3))*Tz(-L1)*Ry(theta(4)); |
| stanley1228 | 2:71ed7da9ea36 | 460 | |
| stanley1228 | 2:71ed7da9ea36 | 461 | |
| stanley1228 | 2:71ed7da9ea36 | 462 | Matrix V_temp_f(4,1); |
| stanley1228 | 2:71ed7da9ea36 | 463 | Matrix V_r_wst_u_4x1(4,1); |
| stanley1228 | 2:71ed7da9ea36 | 464 | V_r_wst_u_4x1.Vec_ext_1_row(V_r_wst_u,1); |
| stanley1228 | 2:71ed7da9ea36 | 465 | |
| stanley1228 | 2:71ed7da9ea36 | 466 | |
| stanley1228 | 2:71ed7da9ea36 | 467 | V_temp_f=MatrixMath::Inv(A1_4)*V_r_wst_u_4x1;//V_temp_f=inv(A1_4)*[V_r_wst_u;1]; %(3.31) 這個是補一列1上去的意思,need fix |
| stanley1228 | 2:71ed7da9ea36 | 468 | theta[Index_AXIS5]=atan2(V_temp_f(2,1),V_temp_f(1,1));//theta(5)=atan2(V_temp_f(2),V_temp_f(1)); |
| stanley1228 | 2:71ed7da9ea36 | 469 | |
| stanley1228 | 2:71ed7da9ea36 | 470 | |
| stanley1228 | 2:71ed7da9ea36 | 471 | //theat 6 |
| stanley1228 | 2:71ed7da9ea36 | 472 | Matrix V_r_wst_r(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 473 | V_r_wst_r=V_r_wst+V_H_hat_y; |
| stanley1228 | 2:71ed7da9ea36 | 474 | |
| stanley1228 | 2:71ed7da9ea36 | 475 | Matrix A1_5(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 476 | A1_5=A1_4*MatrixMath::RotZ(theta[Index_AXIS5])*MatrixMath::Tz(-l2);//A1_5=A1_4*Rz(theta(5))*Tz(-L2); |
| stanley1228 | 2:71ed7da9ea36 | 477 | |
| stanley1228 | 2:71ed7da9ea36 | 478 | Matrix V_temp_g(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 479 | Matrix V_r_wst_r_4x1(4,1); |
| stanley1228 | 2:71ed7da9ea36 | 480 | V_r_wst_r_4x1.Vec_ext_1_row(V_r_wst_r,1); |
| stanley1228 | 2:71ed7da9ea36 | 481 | |
| stanley1228 | 2:71ed7da9ea36 | 482 | V_temp_g=MatrixMath::Inv(A1_5)*V_r_wst_r_4x1; //V_temp_g=inv(A1_5)*[V_r_wst_r;1]; %(3.38) 這個是補一列1上去的意思,need fix |
| stanley1228 | 2:71ed7da9ea36 | 483 | |
| stanley1228 | 2:71ed7da9ea36 | 484 | theta[Index_AXIS6]=atan2(V_temp_g(3,1),V_temp_g(2,1)); |
| stanley1228 | 2:71ed7da9ea36 | 485 | |
| stanley1228 | 2:71ed7da9ea36 | 486 | |
| stanley1228 | 2:71ed7da9ea36 | 487 | //theat 7 |
| stanley1228 | 2:71ed7da9ea36 | 488 | Matrix V_r_wst_f(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 489 | V_r_wst_f=V_r_wst+V_H_hat_x; |
| stanley1228 | 2:71ed7da9ea36 | 490 | |
| stanley1228 | 2:71ed7da9ea36 | 491 | Matrix A1_6(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 492 | A1_6=A1_5*MatrixMath::RotX(theta[Index_AXIS6]); |
| stanley1228 | 2:71ed7da9ea36 | 493 | |
| stanley1228 | 2:71ed7da9ea36 | 494 | Matrix V_temp_h(3,1); |
| stanley1228 | 2:71ed7da9ea36 | 495 | Matrix V_r_wst_f_4x1(4,1); |
| stanley1228 | 2:71ed7da9ea36 | 496 | V_r_wst_f_4x1.Vec_ext_1_row(V_r_wst_f,1); |
| stanley1228 | 2:71ed7da9ea36 | 497 | |
| stanley1228 | 2:71ed7da9ea36 | 498 | V_temp_h=MatrixMath::Inv(A1_6)*V_r_wst_f_4x1; //V_temp_h=inv(A1_6)*[V_r_wst_f;1]; |
| stanley1228 | 2:71ed7da9ea36 | 499 | |
| stanley1228 | 2:71ed7da9ea36 | 500 | theta[Index_AXIS7]=atan2(-V_temp_h(1,1),-V_temp_h(3,1));//theta(7)=atan2(-V_temp_h(1),-V_temp_h(3)); |
| stanley1228 | 2:71ed7da9ea36 | 501 | |
| stanley1228 | 2:71ed7da9ea36 | 502 | |
| stanley1228 | 2:71ed7da9ea36 | 503 | return 0; |
| stanley1228 | 2:71ed7da9ea36 | 504 | } |
| stanley1228 | 2:71ed7da9ea36 | 505 | |
| stanley1228 | 2:71ed7da9ea36 | 506 | |
| stanley1228 | 2:71ed7da9ea36 | 507 | |
| stanley1228 | 2:71ed7da9ea36 | 508 | //int keepitforawhile_deleteitafter() |
| stanley1228 | 2:71ed7da9ea36 | 509 | //{ |
| stanley1228 | 2:71ed7da9ea36 | 510 | // |
| stanley1228 | 2:71ed7da9ea36 | 511 | // //DigitalOut myled(LED1); |
| stanley1228 | 2:71ed7da9ea36 | 512 | // //Timer t,t2; |
| stanley1228 | 2:71ed7da9ea36 | 513 | // |
| stanley1228 | 2:71ed7da9ea36 | 514 | // //t.start(); |
| stanley1228 | 2:71ed7da9ea36 | 515 | // float theta[7]={0}; |
| stanley1228 | 2:71ed7da9ea36 | 516 | // int rt = IK_7DOF_stanley(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-M_PI*0.5,theta); |
| stanley1228 | 2:71ed7da9ea36 | 517 | ////--- |
| stanley1228 | 2:71ed7da9ea36 | 518 | // Matrix myMatrix(3,3); |
| stanley1228 | 2:71ed7da9ea36 | 519 | // Matrix anotherMatrix; |
| stanley1228 | 2:71ed7da9ea36 | 520 | // |
| stanley1228 | 2:71ed7da9ea36 | 521 | // // Fill Matrix with data. |
| stanley1228 | 2:71ed7da9ea36 | 522 | // myMatrix << 1 << 2 << 3 |
| stanley1228 | 2:71ed7da9ea36 | 523 | // << 4 << 5 << 6 |
| stanley1228 | 2:71ed7da9ea36 | 524 | // << 7 << 8 << 9; |
| stanley1228 | 2:71ed7da9ea36 | 525 | // |
| stanley1228 | 2:71ed7da9ea36 | 526 | // printf( "\nmyMatrix:\n\n"); |
| stanley1228 | 2:71ed7da9ea36 | 527 | // myMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 528 | // printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 529 | // |
| stanley1228 | 2:71ed7da9ea36 | 530 | // /*Matrix myMatrix2(3,3); |
| stanley1228 | 2:71ed7da9ea36 | 531 | // myMatrix2 << 1 << 0 << 0 |
| stanley1228 | 2:71ed7da9ea36 | 532 | // << 0 << 1 << 0 |
| stanley1228 | 2:71ed7da9ea36 | 533 | // << 0 << 0 << 1;*/ |
| stanley1228 | 2:71ed7da9ea36 | 534 | // float mydet = MatrixMath::det( myMatrix ); |
| stanley1228 | 2:71ed7da9ea36 | 535 | // |
| stanley1228 | 2:71ed7da9ea36 | 536 | // printf( "det mymatrix=%f:\n\n",mydet); |
| stanley1228 | 2:71ed7da9ea36 | 537 | // |
| stanley1228 | 2:71ed7da9ea36 | 538 | // |
| stanley1228 | 2:71ed7da9ea36 | 539 | // |
| stanley1228 | 2:71ed7da9ea36 | 540 | // // Matrix operations // |
| stanley1228 | 2:71ed7da9ea36 | 541 | // |
| stanley1228 | 2:71ed7da9ea36 | 542 | // // Add 5 to negative Matrix |
| stanley1228 | 2:71ed7da9ea36 | 543 | // anotherMatrix = - myMatrix + 5; |
| stanley1228 | 2:71ed7da9ea36 | 544 | // |
| stanley1228 | 2:71ed7da9ea36 | 545 | // printf( "Result Matrix: anotherMatrix = - myMatrix + 5\n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 546 | // anotherMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 547 | // printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 548 | // |
| stanley1228 | 2:71ed7da9ea36 | 549 | // // Matrix Multiplication * |
| stanley1228 | 2:71ed7da9ea36 | 550 | // anotherMatrix *= myMatrix; |
| stanley1228 | 2:71ed7da9ea36 | 551 | // |
| stanley1228 | 2:71ed7da9ea36 | 552 | // printf( "\nanotherMatrix = anotherMatrix * myMatrix\n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 553 | // anotherMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 554 | // printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 555 | // |
| stanley1228 | 2:71ed7da9ea36 | 556 | // // Scalar Matrix Multiplication anotherMatrix *= 0.5 |
| stanley1228 | 2:71ed7da9ea36 | 557 | // anotherMatrix *= 0.5; |
| stanley1228 | 2:71ed7da9ea36 | 558 | // |
| stanley1228 | 2:71ed7da9ea36 | 559 | // printf( "\nResult Matrix *= 0.5:\n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 560 | // anotherMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 561 | // printf( " _______________________________ \n" ); |
| stanley1228 | 2:71ed7da9ea36 | 562 | // |
| stanley1228 | 2:71ed7da9ea36 | 563 | // |
| stanley1228 | 2:71ed7da9ea36 | 564 | // printf("\n\n *** MEMBER OPERATIONS *** \n\n"); |
| stanley1228 | 2:71ed7da9ea36 | 565 | // |
| stanley1228 | 2:71ed7da9ea36 | 566 | // //Copy myMatrix |
| stanley1228 | 2:71ed7da9ea36 | 567 | // Matrix temp( myMatrix ); |
| stanley1228 | 2:71ed7da9ea36 | 568 | // |
| stanley1228 | 2:71ed7da9ea36 | 569 | // // Resize Matrix |
| stanley1228 | 2:71ed7da9ea36 | 570 | // temp.Resize(4,4); |
| stanley1228 | 2:71ed7da9ea36 | 571 | // printf("\nAdded one Column, one Row to the limits of myMatrix saved in temp Matrix\n"); |
| stanley1228 | 2:71ed7da9ea36 | 572 | // temp.print(); |
| stanley1228 | 2:71ed7da9ea36 | 573 | // |
| stanley1228 | 2:71ed7da9ea36 | 574 | // //Delete those new elements, we don't need them anyway. |
| stanley1228 | 2:71ed7da9ea36 | 575 | // Matrix::DeleteRow( temp, 4 ); |
| stanley1228 | 2:71ed7da9ea36 | 576 | // Matrix::DeleteCol( temp, 4 ); |
| stanley1228 | 2:71ed7da9ea36 | 577 | // |
| stanley1228 | 2:71ed7da9ea36 | 578 | // printf("\nBack to normal\n"); |
| stanley1228 | 2:71ed7da9ea36 | 579 | // temp.print(); |
| stanley1228 | 2:71ed7da9ea36 | 580 | // |
| stanley1228 | 2:71ed7da9ea36 | 581 | // |
| stanley1228 | 2:71ed7da9ea36 | 582 | // // Make room at the begining of Matrix |
| stanley1228 | 2:71ed7da9ea36 | 583 | // Matrix::AddRow( temp, 1 ); |
| stanley1228 | 2:71ed7da9ea36 | 584 | // Matrix::AddCol( temp, 1 ); |
| stanley1228 | 2:71ed7da9ea36 | 585 | // |
| stanley1228 | 2:71ed7da9ea36 | 586 | // printf("\nAdded Just one Row and column to the beginning\n"); |
| stanley1228 | 2:71ed7da9ea36 | 587 | // temp.print(); |
| stanley1228 | 2:71ed7da9ea36 | 588 | // |
| stanley1228 | 2:71ed7da9ea36 | 589 | // // Take the second Row as a new Matrix |
| stanley1228 | 2:71ed7da9ea36 | 590 | // anotherMatrix = Matrix::ExportRow( temp, 2 ); |
| stanley1228 | 2:71ed7da9ea36 | 591 | // printf("\nExport Second Row \n"); |
| stanley1228 | 2:71ed7da9ea36 | 592 | // anotherMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 593 | // |
| stanley1228 | 2:71ed7da9ea36 | 594 | // // The second Column as a new Matrix, then transpose it to make it a Row |
| stanley1228 | 2:71ed7da9ea36 | 595 | // anotherMatrix = Matrix::ExportCol( temp, 2 ); |
| stanley1228 | 2:71ed7da9ea36 | 596 | // anotherMatrix = MatrixMath::Transpose( anotherMatrix ); |
| stanley1228 | 2:71ed7da9ea36 | 597 | // printf("\nAnd Export Second Column and Transpose it \n"); |
| stanley1228 | 2:71ed7da9ea36 | 598 | // anotherMatrix.print(); |
| stanley1228 | 2:71ed7da9ea36 | 599 | // |
| stanley1228 | 2:71ed7da9ea36 | 600 | // // This will Check to see if your are reduce to a single Row or Column |
| stanley1228 | 2:71ed7da9ea36 | 601 | // temp = Matrix::ToPackedVector( myMatrix ); |
| stanley1228 | 2:71ed7da9ea36 | 602 | // printf("\nInitial Matrix turned into a single Row\n"); |
| stanley1228 | 2:71ed7da9ea36 | 603 | // temp.print(); |
| stanley1228 | 2:71ed7da9ea36 | 604 | // |
| stanley1228 | 2:71ed7da9ea36 | 605 | // // Matrix Math // |
| stanley1228 | 2:71ed7da9ea36 | 606 | // printf("\n\n *** Matrix Inverse and Determinant ***\n"); |
| stanley1228 | 2:71ed7da9ea36 | 607 | // |
| stanley1228 | 2:71ed7da9ea36 | 608 | // //Matrix BigMat( 8, 8 ); |
| stanley1228 | 2:71ed7da9ea36 | 609 | // // |
| stanley1228 | 2:71ed7da9ea36 | 610 | // //BigMat << 1 << 0.3 << 1.0 << 1 << 3 << 0.5 << 7.12 << 899 |
| stanley1228 | 2:71ed7da9ea36 | 611 | // // << 2 << 3.2 << 4.1 << 0 << 4 << 0.8 << 9.26 << 321 |
| stanley1228 | 2:71ed7da9ea36 | 612 | // // << 5 << 6.0 << 1 << 1 << 2 << 7.4 << 3.87 << 562 |
| stanley1228 | 2:71ed7da9ea36 | 613 | // // << 1 << 0.0 << 2.7 << 1 << 1 << 4.6 << 1.21 << 478 |
| stanley1228 | 2:71ed7da9ea36 | 614 | // // << 2 << 3.7 << 48 << 2 << 0 << 77 << 0.19 << 147 |
| stanley1228 | 2:71ed7da9ea36 | 615 | // // << 1 << 1.0 << 3.8 << 7 << 1 << 9.9 << 7.25 << 365 |
| stanley1228 | 2:71ed7da9ea36 | 616 | // // << 9 << 0.9 << 2.7 << 8 << 0 << 13 << 4.16 << 145 |
| stanley1228 | 2:71ed7da9ea36 | 617 | // // << 7 << 23 << 28 << 9 << 9 << 1.7 << 9.16 << 156; |
| stanley1228 | 2:71ed7da9ea36 | 618 | // |
| stanley1228 | 2:71ed7da9ea36 | 619 | // //printf( "\nBigMat:\n"); |
| stanley1228 | 2:71ed7da9ea36 | 620 | // //BigMat.print(); |
| stanley1228 | 2:71ed7da9ea36 | 621 | // //printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 622 | // |
| stanley1228 | 2:71ed7da9ea36 | 623 | // //t2.start(); |
| stanley1228 | 2:71ed7da9ea36 | 624 | // //float determ = MatrixMath::det( BigMat );//有問題 |
| stanley1228 | 2:71ed7da9ea36 | 625 | // |
| stanley1228 | 2:71ed7da9ea36 | 626 | // //Matrix myInv = MatrixMath::Inv( BigMat ); |
| stanley1228 | 2:71ed7da9ea36 | 627 | // ////t2.stop(); |
| stanley1228 | 2:71ed7da9ea36 | 628 | // |
| stanley1228 | 2:71ed7da9ea36 | 629 | // //printf( "\nBigMat's Determinant is: %f \n", determ); |
| stanley1228 | 2:71ed7da9ea36 | 630 | // //printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 631 | // // |
| stanley1228 | 2:71ed7da9ea36 | 632 | // //printf( "\nBigMat's Inverse is:\n"); |
| stanley1228 | 2:71ed7da9ea36 | 633 | // //myInv.print(); |
| stanley1228 | 2:71ed7da9ea36 | 634 | // //printf( "\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 635 | // |
| stanley1228 | 2:71ed7da9ea36 | 636 | // //*** Homogenous Transformations **// |
| stanley1228 | 2:71ed7da9ea36 | 637 | // |
| stanley1228 | 2:71ed7da9ea36 | 638 | // printf( "\n\n *** TRANSFORMATIONS *** \n\n"); |
| stanley1228 | 2:71ed7da9ea36 | 639 | // |
| stanley1228 | 2:71ed7da9ea36 | 640 | // Matrix rot; |
| stanley1228 | 2:71ed7da9ea36 | 641 | // |
| stanley1228 | 2:71ed7da9ea36 | 642 | // printf( " RotX 0.5 rad \n" ); |
| stanley1228 | 2:71ed7da9ea36 | 643 | // rot = MatrixMath::RotX(0.5); |
| stanley1228 | 2:71ed7da9ea36 | 644 | // rot.print(); |
| stanley1228 | 2:71ed7da9ea36 | 645 | // printf( " _______________________________ \n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 646 | // |
| stanley1228 | 2:71ed7da9ea36 | 647 | // printf( " RotY 0.5 rad \n" ); |
| stanley1228 | 2:71ed7da9ea36 | 648 | // rot = MatrixMath::RotY(0.5); |
| stanley1228 | 2:71ed7da9ea36 | 649 | // rot.print(); |
| stanley1228 | 2:71ed7da9ea36 | 650 | // printf( " _______________________________ \n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 651 | // |
| stanley1228 | 2:71ed7da9ea36 | 652 | // printf( " RotZ 0.5 rad \n" ); |
| stanley1228 | 2:71ed7da9ea36 | 653 | // rot = MatrixMath::RotZ(0.5); |
| stanley1228 | 2:71ed7da9ea36 | 654 | // rot.print(); |
| stanley1228 | 2:71ed7da9ea36 | 655 | // printf( " _______________________________ \n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 656 | // |
| stanley1228 | 2:71ed7da9ea36 | 657 | // //printf( " Transl 5x 3y 4z\n" ); //有問題 |
| stanley1228 | 2:71ed7da9ea36 | 658 | // //rot = MatrixMath::Transl( 5, 3, 4 ); |
| stanley1228 | 2:71ed7da9ea36 | 659 | // //rot.print(); |
| stanley1228 | 2:71ed7da9ea36 | 660 | // //printf( " _______________________________ \n\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 661 | // |
| stanley1228 | 2:71ed7da9ea36 | 662 | // //--- |
| stanley1228 | 2:71ed7da9ea36 | 663 | // |
| stanley1228 | 2:71ed7da9ea36 | 664 | // //t.stop(); |
| stanley1228 | 2:71ed7da9ea36 | 665 | // |
| stanley1228 | 2:71ed7da9ea36 | 666 | // //float bigtime = t2.read(); |
| stanley1228 | 2:71ed7da9ea36 | 667 | // //float average = 12.149647 - bigtime; |
| stanley1228 | 2:71ed7da9ea36 | 668 | // |
| stanley1228 | 2:71ed7da9ea36 | 669 | // //printf( "\n\nThe time for all those operations in mbed was : %f seconds\n", t.read() ); |
| stanley1228 | 2:71ed7da9ea36 | 670 | // //printf( "\nOnly operations witout any print takes: 12.149647 seconds\n" ); |
| stanley1228 | 2:71ed7da9ea36 | 671 | // //printf( "\nDue to the 8x8 matrix alone takes: %f \n",bigtime ); |
| stanley1228 | 2:71ed7da9ea36 | 672 | // //printf( "\nSo normal 4x4 matrix ops: %f\n", average ); |
| stanley1228 | 2:71ed7da9ea36 | 673 | // |
| stanley1228 | 2:71ed7da9ea36 | 674 | // //while(1) { |
| stanley1228 | 2:71ed7da9ea36 | 675 | // // myled = 1; |
| stanley1228 | 2:71ed7da9ea36 | 676 | // // wait(0.2); |
| stanley1228 | 2:71ed7da9ea36 | 677 | // // myled = 0; |
| stanley1228 | 2:71ed7da9ea36 | 678 | // // wait(0.2); |
| stanley1228 | 2:71ed7da9ea36 | 679 | // //} |
| stanley1228 | 2:71ed7da9ea36 | 680 | //} |