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
- Committer:
- eencae
- Date:
- 2017-07-04
- Revision:
- 1:d2b7780ca6b3
- Parent:
- 0:49b8e971fa83
File content as of revision 1:d2b7780ca6b3:
/* @file MeArm.cpp */ #include "mbed.h" #include "MeArm.h" MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin) { // PwmOut objects for servos _middle_servo = new PwmOut(middle_pin); _left_servo = new PwmOut(left_pin); _right_servo = new PwmOut(right_pin); _claw_servo = new PwmOut(claw_pin); // set link lengths (mm) _a[0] = 15.0; _a[1] = 80.0; _a[2] = 80.0; _a[3] = 65.0; // default calibration values, will need changing for each MeArm _claw_cal[0] = 2200; _claw_cal[1] = 1400; //middle _theta_1_cal[0] = 1650; _theta_1_cal[1] = 2700; // right _theta_2_cal[0] = 2000; _theta_2_cal[1] = 1000; //left _theta_3_cal[0] = 1900; _theta_3_cal[1] = 850; // height of shoulder servo off the floor (including gears under gripper) _base_height = 30.0; // set PWM period - all channels share so just need to set one _middle_servo->period_ms(20); } MeArm::~MeArm() { delete _middle_servo, _left_servo, _right_servo, _claw_servo; } void MeArm::set_claw_calibration_values(int open_val, int closed_val) { _claw_cal[0] = closed_val; _claw_cal[1] = open_val; } void MeArm::set_middle_calibration_values(int zero, int ninety) { _theta_1_cal[0] = zero; _theta_1_cal[1] = ninety; } void MeArm::set_right_calibration_values(int zero, int ninety) { _theta_2_cal[0] = zero; _theta_2_cal[1] = ninety; } void MeArm::set_left_calibration_values(int zero, int ninety) { _theta_3_cal[0] = zero; _theta_3_cal[1] = ninety; } void MeArm::set_claw(float value) { // open - closed value int range = _claw_cal[1] -_claw_cal[0]; // closed + val * range int pw = _claw_cal[0] + int(range*value); _claw_servo->pulsewidth_us(pw); } void MeArm::goto_xyz(float x, float y, float z) { // set position _x=x; _y=y; _z=z - _base_height; //printf("x = %f y = %f z = %f\n",x,y,z); // do inverse kinematics calc_middle_angle(); calc_right_angle(); calc_left_angle(); // convert to pulse-width calc_pw(); // update servo position move_servos(); } // base servo void MeArm::calc_middle_angle() { _theta[0] = RAD2DEG * atan2( _y , _x); //printf("theta[0] = %f\n",_theta[0]); } // "elbow" servo void MeArm::calc_left_angle() { // for the elbow, we use the 2-link planar simplification, // so work out the direct distance (radius) from the shoulder to the target in xy // then subtract the shoulder and gripper offsets to get the 'x' value float d = sqrt( _x*_x + _y*_y ); float x_bar = d - _a[0] - _a[3]; float D = (x_bar*x_bar + _z*_z - _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[2]*_a[1]); if ( D*D > 1.0) { // if this is the case, we will get complex roots and hence invalid solution printf("Error!\n"); exit(1); } // from IK derivation // we want the -ve root to get the 'elbow' up solution _theta[2] = RAD2DEG*atan2( -sqrt(1-D*D) , D); //printf("theta[2] = %f\n",_theta[2]); // we don't actually control the elbow angle in the MeArm // we need to convert to the angle that we do control _theta[2] = -(_theta[1] + _theta[2]); //printf("theta[2] = %f\n",_theta[2]); } // shoulder servo void MeArm::calc_right_angle() { // for the shoulder, we use the 2-link planar simplification, // so work out the direct distance (radius) from the shoulder to the target in xy // then subtract the shoulder and gripper offsets to get the 'x' value float d = sqrt( _x*_x + _y*_y ); float x_bar = d - _a[0] - _a[3]; // from IK derivation float E = (x_bar*x_bar + _z*_z + _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[1]); float r = sqrt( x_bar*x_bar + _z*_z ); if (E > r) { // if this is the case, we will get complex roots and hence invalid solution printf("Error!\n"); exit(1); } // from IK derivation // we want the +ve root to get the 'elbow' up solution _theta[1] = RAD2DEG*( atan2(sqrt( r*r - E*E ),E) + atan2(_z,x_bar) ); //printf("theta[1] = %f\n",_theta[1]); } void MeArm::calc_pw(){ // 90 - 0 float range = (_theta_1_cal[1] - _theta_1_cal[0])/90.0; _pw[0] = _theta_1_cal[0] + int(range*_theta[0]); range = (_theta_2_cal[1] - _theta_2_cal[0])/90.0; _pw[1] = _theta_2_cal[0] + int(range*_theta[1]); range = (_theta_3_cal[1] - _theta_3_cal[0])/90.0; _pw[2] = _theta_3_cal[0] + int(range*_theta[2]); //printf("pw[0] = %i pw[1] = %i pw[2] = %i\n",_pw[0],_pw[1],_pw[2]); } void MeArm::move_servos() { //printf("Move servos\n"); _middle_servo->pulsewidth_us(_pw[0]); _right_servo->pulsewidth_us(_pw[1]); _left_servo->pulsewidth_us(_pw[2]); }