Craig Evans / MeArm

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@1:d2b7780ca6b3, 2017-07-04 (annotated)

Committer:
eencae
Date:
Tue Jul 04 16:13:12 2017 +0000
Revision:
1:d2b7780ca6b3
Parent:
0:49b8e971fa83 
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:49b8e971fa83 1 /*
eencae 0:49b8e971fa83 2 @file MeArm.cpp
eencae 0:49b8e971fa83 3 */
eencae 0:49b8e971fa83 4 #include "mbed.h"
eencae 0:49b8e971fa83 5 #include "MeArm.h"
eencae 0:49b8e971fa83 6
eencae 1:d2b7780ca6b3 7 MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin)
eencae 0:49b8e971fa83 8 {
eencae 1:d2b7780ca6b3 9 // PwmOut objects for servos
eencae 1:d2b7780ca6b3 10 _middle_servo = new PwmOut(middle_pin);
eencae 1:d2b7780ca6b3 11 _left_servo = new PwmOut(left_pin);
eencae 1:d2b7780ca6b3 12 _right_servo = new PwmOut(right_pin);
eencae 1:d2b7780ca6b3 13 _claw_servo = new PwmOut(claw_pin);
eencae 1:d2b7780ca6b3 14
eencae 1:d2b7780ca6b3 15 // set link lengths (mm)
eencae 1:d2b7780ca6b3 16 _a[0] = 15.0;
eencae 1:d2b7780ca6b3 17 _a[1] = 80.0;
eencae 1:d2b7780ca6b3 18 _a[2] = 80.0;
eencae 1:d2b7780ca6b3 19 _a[3] = 65.0;
eencae 1:d2b7780ca6b3 20
eencae 1:d2b7780ca6b3 21 // default calibration values, will need changing for each MeArm
eencae 1:d2b7780ca6b3 22 _claw_cal[0] = 2200;
eencae 1:d2b7780ca6b3 23 _claw_cal[1] = 1400;
eencae 1:d2b7780ca6b3 24 //middle
eencae 1:d2b7780ca6b3 25 _theta_1_cal[0] = 1650;
eencae 1:d2b7780ca6b3 26 _theta_1_cal[1] = 2700;
eencae 1:d2b7780ca6b3 27 // right
eencae 1:d2b7780ca6b3 28 _theta_2_cal[0] = 2000;
eencae 1:d2b7780ca6b3 29 _theta_2_cal[1] = 1000;
eencae 1:d2b7780ca6b3 30 //left
eencae 1:d2b7780ca6b3 31 _theta_3_cal[0] = 1900;
eencae 1:d2b7780ca6b3 32 _theta_3_cal[1] = 850;
eencae 1:d2b7780ca6b3 33
eencae 1:d2b7780ca6b3 34 // height of shoulder servo off the floor (including gears under gripper)
eencae 1:d2b7780ca6b3 35 _base_height = 30.0;
eencae 1:d2b7780ca6b3 36
eencae 1:d2b7780ca6b3 37 // set PWM period - all channels share so just need to set one
eencae 1:d2b7780ca6b3 38 _middle_servo->period_ms(20);
eencae 1:d2b7780ca6b3 39
eencae 0:49b8e971fa83 40 }
eencae 0:49b8e971fa83 41
eencae 1:d2b7780ca6b3 42 MeArm::~MeArm()
eencae 0:49b8e971fa83 43 {
eencae 1:d2b7780ca6b3 44 delete _middle_servo, _left_servo, _right_servo, _claw_servo;
eencae 0:49b8e971fa83 45 }
eencae 0:49b8e971fa83 46
eencae 1:d2b7780ca6b3 47 void MeArm::set_claw_calibration_values(int open_val, int closed_val)
eencae 0:49b8e971fa83 48 {
eencae 1:d2b7780ca6b3 49 _claw_cal[0] = closed_val;
eencae 1:d2b7780ca6b3 50 _claw_cal[1] = open_val;
eencae 0:49b8e971fa83 51 }
eencae 0:49b8e971fa83 52
eencae 1:d2b7780ca6b3 53 void MeArm::set_middle_calibration_values(int zero, int ninety)
eencae 1:d2b7780ca6b3 54 {
eencae 1:d2b7780ca6b3 55 _theta_1_cal[0] = zero;
eencae 1:d2b7780ca6b3 56 _theta_1_cal[1] = ninety;
eencae 1:d2b7780ca6b3 57 }
eencae 1:d2b7780ca6b3 58 void MeArm::set_right_calibration_values(int zero, int ninety)
eencae 0:49b8e971fa83 59 {
eencae 1:d2b7780ca6b3 60 _theta_2_cal[0] = zero;
eencae 1:d2b7780ca6b3 61 _theta_2_cal[1] = ninety;
eencae 1:d2b7780ca6b3 62 }
eencae 1:d2b7780ca6b3 63 void MeArm::set_left_calibration_values(int zero, int ninety)
eencae 1:d2b7780ca6b3 64 {
eencae 1:d2b7780ca6b3 65 _theta_3_cal[0] = zero;
eencae 1:d2b7780ca6b3 66 _theta_3_cal[1] = ninety;
eencae 0:49b8e971fa83 67 }
eencae 0:49b8e971fa83 68
eencae 1:d2b7780ca6b3 69 void MeArm::set_claw(float value)
eencae 0:49b8e971fa83 70 {
eencae 1:d2b7780ca6b3 71 // open - closed value
eencae 1:d2b7780ca6b3 72 int range = _claw_cal[1] -_claw_cal[0];
eencae 1:d2b7780ca6b3 73
eencae 1:d2b7780ca6b3 74 // closed + val * range
eencae 1:d2b7780ca6b3 75 int pw = _claw_cal[0] + int(range*value);
eencae 1:d2b7780ca6b3 76
eencae 1:d2b7780ca6b3 77 _claw_servo->pulsewidth_us(pw);
eencae 0:49b8e971fa83 78 }
eencae 0:49b8e971fa83 79
eencae 1:d2b7780ca6b3 80 void MeArm::goto_xyz(float x, float y, float z)
eencae 0:49b8e971fa83 81 {
eencae 1:d2b7780ca6b3 82 // set position
eencae 1:d2b7780ca6b3 83 _x=x;
eencae 1:d2b7780ca6b3 84 _y=y;
eencae 1:d2b7780ca6b3 85 _z=z - _base_height;
eencae 1:d2b7780ca6b3 86
eencae 1:d2b7780ca6b3 87 //printf("x = %f y = %f z = %f\n",x,y,z);
eencae 1:d2b7780ca6b3 88
eencae 1:d2b7780ca6b3 89 // do inverse kinematics
eencae 1:d2b7780ca6b3 90 calc_middle_angle();
eencae 1:d2b7780ca6b3 91 calc_right_angle();
eencae 1:d2b7780ca6b3 92 calc_left_angle();
eencae 1:d2b7780ca6b3 93
eencae 1:d2b7780ca6b3 94 // convert to pulse-width
eencae 1:d2b7780ca6b3 95 calc_pw();
eencae 1:d2b7780ca6b3 96
eencae 1:d2b7780ca6b3 97 // update servo position
eencae 1:d2b7780ca6b3 98 move_servos();
eencae 0:49b8e971fa83 99 }
eencae 0:49b8e971fa83 100
eencae 1:d2b7780ca6b3 101 // base servo
eencae 1:d2b7780ca6b3 102 void MeArm::calc_middle_angle()
eencae 1:d2b7780ca6b3 103 {
eencae 1:d2b7780ca6b3 104 _theta[0] = RAD2DEG * atan2( _y , _x);
eencae 1:d2b7780ca6b3 105 //printf("theta[0] = %f\n",_theta[0]);
eencae 1:d2b7780ca6b3 106 }
eencae 1:d2b7780ca6b3 107
eencae 1:d2b7780ca6b3 108 // "elbow" servo
eencae 1:d2b7780ca6b3 109 void MeArm::calc_left_angle()
eencae 0:49b8e971fa83 110 {
eencae 1:d2b7780ca6b3 111 // for the elbow, we use the 2-link planar simplification,
eencae 1:d2b7780ca6b3 112 // so work out the direct distance (radius) from the shoulder to the target in xy
eencae 1:d2b7780ca6b3 113 // then subtract the shoulder and gripper offsets to get the 'x' value
eencae 1:d2b7780ca6b3 114 float d = sqrt( _x*_x + _y*_y );
eencae 1:d2b7780ca6b3 115 float x_bar = d - _a[0] - _a[3];
eencae 0:49b8e971fa83 116
eencae 1:d2b7780ca6b3 117 float D = (x_bar*x_bar + _z*_z - _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[2]*_a[1]);
eencae 1:d2b7780ca6b3 118
eencae 1:d2b7780ca6b3 119 if ( D*D > 1.0) { // if this is the case, we will get complex roots and hence invalid solution
eencae 1:d2b7780ca6b3 120 printf("Error!\n");
eencae 1:d2b7780ca6b3 121 exit(1);
eencae 0:49b8e971fa83 122 }
eencae 0:49b8e971fa83 123
eencae 1:d2b7780ca6b3 124 // from IK derivation
eencae 1:d2b7780ca6b3 125 // we want the -ve root to get the 'elbow' up solution
eencae 1:d2b7780ca6b3 126 _theta[2] = RAD2DEG*atan2( -sqrt(1-D*D) , D);
eencae 1:d2b7780ca6b3 127 //printf("theta[2] = %f\n",_theta[2]);
eencae 1:d2b7780ca6b3 128
eencae 0:49b8e971fa83 129
eencae 1:d2b7780ca6b3 130 // we don't actually control the elbow angle in the MeArm
eencae 1:d2b7780ca6b3 131 // we need to convert to the angle that we do control
eencae 1:d2b7780ca6b3 132 _theta[2] = -(_theta[1] + _theta[2]);
eencae 1:d2b7780ca6b3 133 //printf("theta[2] = %f\n",_theta[2]);
eencae 1:d2b7780ca6b3 134
eencae 0:49b8e971fa83 135 }
eencae 0:49b8e971fa83 136
eencae 1:d2b7780ca6b3 137 // shoulder servo
eencae 1:d2b7780ca6b3 138 void MeArm::calc_right_angle()
eencae 0:49b8e971fa83 139 {
eencae 1:d2b7780ca6b3 140 // for the shoulder, we use the 2-link planar simplification,
eencae 1:d2b7780ca6b3 141 // so work out the direct distance (radius) from the shoulder to the target in xy
eencae 1:d2b7780ca6b3 142 // then subtract the shoulder and gripper offsets to get the 'x' value
eencae 0:49b8e971fa83 143
eencae 1:d2b7780ca6b3 144 float d = sqrt( _x*_x + _y*_y );
eencae 1:d2b7780ca6b3 145 float x_bar = d - _a[0] - _a[3];
eencae 0:49b8e971fa83 146
eencae 1:d2b7780ca6b3 147 // from IK derivation
eencae 1:d2b7780ca6b3 148 float E = (x_bar*x_bar + _z*_z + _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[1]);
eencae 1:d2b7780ca6b3 149 float r = sqrt( x_bar*x_bar + _z*_z );
eencae 0:49b8e971fa83 150
eencae 1:d2b7780ca6b3 151 if (E > r) { // if this is the case, we will get complex roots and hence invalid solution
eencae 1:d2b7780ca6b3 152 printf("Error!\n");
eencae 1:d2b7780ca6b3 153 exit(1);
eencae 0:49b8e971fa83 154 }
eencae 0:49b8e971fa83 155
eencae 1:d2b7780ca6b3 156 // from IK derivation
eencae 1:d2b7780ca6b3 157 // we want the +ve root to get the 'elbow' up solution
eencae 1:d2b7780ca6b3 158 _theta[1] = RAD2DEG*( atan2(sqrt( r*r - E*E ),E) + atan2(_z,x_bar) );
eencae 1:d2b7780ca6b3 159 //printf("theta[1] = %f\n",_theta[1]);
eencae 0:49b8e971fa83 160
eencae 1:d2b7780ca6b3 161 }
eencae 0:49b8e971fa83 162
eencae 1:d2b7780ca6b3 163 void MeArm::calc_pw(){
eencae 1:d2b7780ca6b3 164
eencae 1:d2b7780ca6b3 165 // 90 - 0
eencae 1:d2b7780ca6b3 166 float range = (_theta_1_cal[1] - _theta_1_cal[0])/90.0;
eencae 1:d2b7780ca6b3 167 _pw[0] = _theta_1_cal[0] + int(range*_theta[0]);
eencae 0:49b8e971fa83 168
eencae 1:d2b7780ca6b3 169 range = (_theta_2_cal[1] - _theta_2_cal[0])/90.0;
eencae 1:d2b7780ca6b3 170 _pw[1] = _theta_2_cal[0] + int(range*_theta[1]);
eencae 1:d2b7780ca6b3 171
eencae 1:d2b7780ca6b3 172 range = (_theta_3_cal[1] - _theta_3_cal[0])/90.0;
eencae 1:d2b7780ca6b3 173 _pw[2] = _theta_3_cal[0] + int(range*_theta[2]);
eencae 1:d2b7780ca6b3 174
eencae 1:d2b7780ca6b3 175 //printf("pw[0] = %i pw[1] = %i pw[2] = %i\n",_pw[0],_pw[1],_pw[2]);
eencae 1:d2b7780ca6b3 176
eencae 1:d2b7780ca6b3 177 }
eencae 0:49b8e971fa83 178
eencae 1:d2b7780ca6b3 179 void MeArm::move_servos() {
eencae 0:49b8e971fa83 180
eencae 1:d2b7780ca6b3 181 //printf("Move servos\n");
eencae 1:d2b7780ca6b3 182 _middle_servo->pulsewidth_us(_pw[0]);
eencae 1:d2b7780ca6b3 183 _right_servo->pulsewidth_us(_pw[1]);
eencae 1:d2b7780ca6b3 184 _left_servo->pulsewidth_us(_pw[2]);
eencae 1:d2b7780ca6b3 185 }