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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MeArm.cpp Source File

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 }