Library for controlling #MeArm from phenoptix (http://www.phenoptix.com/collections/mearm). Solves inverse kinematics of the arm to position gripper in 3D space.
Dependents: wizArm_WIZwiki-W7500ECO wizArm_WIZwiki-W7500ECO MeArm_Lab2_Task1 MeArm_Lab3_Task1 ... more
MeArm.cpp
00001 /* 00002 @file MeArm.cpp 00003 */ 00004 #include "mbed.h" 00005 #include "MeArm.h " 00006 00007 MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin) 00008 { 00009 // PwmOut objects for servos 00010 _middle_servo = new PwmOut(middle_pin); 00011 _left_servo = new PwmOut(left_pin); 00012 _right_servo = new PwmOut(right_pin); 00013 _claw_servo = new PwmOut(claw_pin); 00014 00015 // set link lengths (mm) 00016 _a[0] = 15.0; 00017 _a[1] = 80.0; 00018 _a[2] = 80.0; 00019 _a[3] = 65.0; 00020 00021 // default calibration values, will need changing for each MeArm 00022 _claw_cal[0] = 2200; 00023 _claw_cal[1] = 1400; 00024 //middle 00025 _theta_1_cal[0] = 1650; 00026 _theta_1_cal[1] = 2700; 00027 // right 00028 _theta_2_cal[0] = 2000; 00029 _theta_2_cal[1] = 1000; 00030 //left 00031 _theta_3_cal[0] = 1900; 00032 _theta_3_cal[1] = 850; 00033 00034 // height of shoulder servo off the floor (including gears under gripper) 00035 _base_height = 30.0; 00036 00037 // set PWM period - all channels share so just need to set one 00038 _middle_servo->period_ms(20); 00039 00040 } 00041 00042 MeArm::~MeArm() 00043 { 00044 delete _middle_servo, _left_servo, _right_servo, _claw_servo; 00045 } 00046 00047 void MeArm::set_claw_calibration_values(int open_val, int closed_val) 00048 { 00049 _claw_cal[0] = closed_val; 00050 _claw_cal[1] = open_val; 00051 } 00052 00053 void MeArm::set_middle_calibration_values(int zero, int ninety) 00054 { 00055 _theta_1_cal[0] = zero; 00056 _theta_1_cal[1] = ninety; 00057 } 00058 void MeArm::set_right_calibration_values(int zero, int ninety) 00059 { 00060 _theta_2_cal[0] = zero; 00061 _theta_2_cal[1] = ninety; 00062 } 00063 void MeArm::set_left_calibration_values(int zero, int ninety) 00064 { 00065 _theta_3_cal[0] = zero; 00066 _theta_3_cal[1] = ninety; 00067 } 00068 00069 void MeArm::set_claw(float value) 00070 { 00071 // open - closed value 00072 int range = _claw_cal[1] -_claw_cal[0]; 00073 00074 // closed + val * range 00075 int pw = _claw_cal[0] + int(range*value); 00076 00077 _claw_servo->pulsewidth_us(pw); 00078 } 00079 00080 void MeArm::goto_xyz(float x, float y, float z) 00081 { 00082 // set position 00083 _x=x; 00084 _y=y; 00085 _z=z - _base_height; 00086 00087 //printf("x = %f y = %f z = %f\n",x,y,z); 00088 00089 // do inverse kinematics 00090 calc_middle_angle(); 00091 calc_right_angle(); 00092 calc_left_angle(); 00093 00094 // convert to pulse-width 00095 calc_pw(); 00096 00097 // update servo position 00098 move_servos(); 00099 } 00100 00101 // base servo 00102 void MeArm::calc_middle_angle() 00103 { 00104 _theta[0] = RAD2DEG * atan2( _y , _x); 00105 //printf("theta[0] = %f\n",_theta[0]); 00106 } 00107 00108 // "elbow" servo 00109 void MeArm::calc_left_angle() 00110 { 00111 // for the elbow, we use the 2-link planar simplification, 00112 // so work out the direct distance (radius) from the shoulder to the target in xy 00113 // then subtract the shoulder and gripper offsets to get the 'x' value 00114 float d = sqrt( _x*_x + _y*_y ); 00115 float x_bar = d - _a[0] - _a[3]; 00116 00117 float D = (x_bar*x_bar + _z*_z - _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[2]*_a[1]); 00118 00119 if ( D*D > 1.0) { // if this is the case, we will get complex roots and hence invalid solution 00120 printf("Error!\n"); 00121 exit(1); 00122 } 00123 00124 // from IK derivation 00125 // we want the -ve root to get the 'elbow' up solution 00126 _theta[2] = RAD2DEG*atan2( -sqrt(1-D*D) , D); 00127 //printf("theta[2] = %f\n",_theta[2]); 00128 00129 00130 // we don't actually control the elbow angle in the MeArm 00131 // we need to convert to the angle that we do control 00132 _theta[2] = -(_theta[1] + _theta[2]); 00133 //printf("theta[2] = %f\n",_theta[2]); 00134 00135 } 00136 00137 // shoulder servo 00138 void MeArm::calc_right_angle() 00139 { 00140 // for the shoulder, we use the 2-link planar simplification, 00141 // so work out the direct distance (radius) from the shoulder to the target in xy 00142 // then subtract the shoulder and gripper offsets to get the 'x' value 00143 00144 float d = sqrt( _x*_x + _y*_y ); 00145 float x_bar = d - _a[0] - _a[3]; 00146 00147 // from IK derivation 00148 float E = (x_bar*x_bar + _z*_z + _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[1]); 00149 float r = sqrt( x_bar*x_bar + _z*_z ); 00150 00151 if (E > r) { // if this is the case, we will get complex roots and hence invalid solution 00152 printf("Error!\n"); 00153 exit(1); 00154 } 00155 00156 // from IK derivation 00157 // we want the +ve root to get the 'elbow' up solution 00158 _theta[1] = RAD2DEG*( atan2(sqrt( r*r - E*E ),E) + atan2(_z,x_bar) ); 00159 //printf("theta[1] = %f\n",_theta[1]); 00160 00161 } 00162 00163 void MeArm::calc_pw(){ 00164 00165 // 90 - 0 00166 float range = (_theta_1_cal[1] - _theta_1_cal[0])/90.0; 00167 _pw[0] = _theta_1_cal[0] + int(range*_theta[0]); 00168 00169 range = (_theta_2_cal[1] - _theta_2_cal[0])/90.0; 00170 _pw[1] = _theta_2_cal[0] + int(range*_theta[1]); 00171 00172 range = (_theta_3_cal[1] - _theta_3_cal[0])/90.0; 00173 _pw[2] = _theta_3_cal[0] + int(range*_theta[2]); 00174 00175 //printf("pw[0] = %i pw[1] = %i pw[2] = %i\n",_pw[0],_pw[1],_pw[2]); 00176 00177 } 00178 00179 void MeArm::move_servos() { 00180 00181 //printf("Move servos\n"); 00182 _middle_servo->pulsewidth_us(_pw[0]); 00183 _right_servo->pulsewidth_us(_pw[1]); 00184 _left_servo->pulsewidth_us(_pw[2]); 00185 }
Generated on Wed Jul 13 2022 02:33:16 by 1.7.2