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?
User | Revision | Line number | New 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 | } |