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.h@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.h |
eencae | 0:49b8e971fa83 | 3 | */ |
eencae | 0:49b8e971fa83 | 4 | |
eencae | 0:49b8e971fa83 | 5 | #ifndef MEARM_H |
eencae | 0:49b8e971fa83 | 6 | #define MEARM_H |
eencae | 0:49b8e971fa83 | 7 | |
eencae | 1:d2b7780ca6b3 | 8 | // used to convert RADIANS<->DEGREES |
eencae | 1:d2b7780ca6b3 | 9 | #define PI 3.1415926536 |
eencae | 1:d2b7780ca6b3 | 10 | #define RAD2DEG (180.0/PI) |
eencae | 1:d2b7780ca6b3 | 11 | #define DEG2RAD (PI/180.0) |
eencae | 0:49b8e971fa83 | 12 | |
eencae | 0:49b8e971fa83 | 13 | #include "mbed.h" |
eencae | 0:49b8e971fa83 | 14 | |
eencae | 0:49b8e971fa83 | 15 | /** |
eencae | 1:d2b7780ca6b3 | 16 | @brief Library for controlling the MeArm (https://shop.mime.co.uk) |
eencae | 0:49b8e971fa83 | 17 | |
eencae | 1:d2b7780ca6b3 | 18 | @brief Revision 1.1 |
eencae | 0:49b8e971fa83 | 19 | |
eencae | 0:49b8e971fa83 | 20 | @author Craig A. Evans |
eencae | 1:d2b7780ca6b3 | 21 | @date June 2017 |
eencae | 0:49b8e971fa83 | 22 | |
eencae | 1:d2b7780ca6b3 | 23 | @code |
eencae | 0:49b8e971fa83 | 24 | #include "mbed.h" |
eencae | 0:49b8e971fa83 | 25 | #include "MeArm.h" |
eencae | 0:49b8e971fa83 | 26 | |
eencae | 0:49b8e971fa83 | 27 | // create MeArm object - base, shoulder, elbow, gripper |
eencae | 0:49b8e971fa83 | 28 | MeArm arm(p21,p22,p23,p24); |
eencae | 0:49b8e971fa83 | 29 | |
eencae | 1:d2b7780ca6b3 | 30 | AnalogIn m_pot(p17); |
eencae | 1:d2b7780ca6b3 | 31 | AnalogIn l_pot(p18); |
eencae | 1:d2b7780ca6b3 | 32 | AnalogIn r_pot(p19); |
eencae | 1:d2b7780ca6b3 | 33 | AnalogIn c_pot(p20); |
eencae | 1:d2b7780ca6b3 | 34 | |
eencae | 0:49b8e971fa83 | 35 | int main() |
eencae | 0:49b8e971fa83 | 36 | { |
eencae | 1:d2b7780ca6b3 | 37 | // open , closed |
eencae | 1:d2b7780ca6b3 | 38 | arm.set_claw_calibration_values(1450,2160); |
eencae | 1:d2b7780ca6b3 | 39 | // 0 , 90 |
eencae | 1:d2b7780ca6b3 | 40 | arm.set_middle_calibration_values(1650,2700); |
eencae | 1:d2b7780ca6b3 | 41 | arm.set_right_calibration_values(2000,1000); |
eencae | 1:d2b7780ca6b3 | 42 | arm.set_left_calibration_values(1870,850); |
eencae | 0:49b8e971fa83 | 43 | |
eencae | 1:d2b7780ca6b3 | 44 | while(1) { |
eencae | 1:d2b7780ca6b3 | 45 | |
eencae | 1:d2b7780ca6b3 | 46 | float x = 100.0 + 100.0*m_pot.read(); |
eencae | 1:d2b7780ca6b3 | 47 | float y = -140.0 + 280.0*l_pot.read(); |
eencae | 1:d2b7780ca6b3 | 48 | float z = 150.0*r_pot.read(); |
eencae | 1:d2b7780ca6b3 | 49 | float claw_val = c_pot.read(); // 0.0 to 1.0 |
eencae | 1:d2b7780ca6b3 | 50 | |
eencae | 1:d2b7780ca6b3 | 51 | arm.set_claw(claw_val); |
eencae | 1:d2b7780ca6b3 | 52 | arm.goto_xyz(x,y,z); |
eencae | 0:49b8e971fa83 | 53 | |
eencae | 1:d2b7780ca6b3 | 54 | wait(0.2); |
eencae | 1:d2b7780ca6b3 | 55 | } |
eencae | 0:49b8e971fa83 | 56 | } |
eencae | 1:d2b7780ca6b3 | 57 | @endcode |
eencae | 0:49b8e971fa83 | 58 | |
eencae | 1:d2b7780ca6b3 | 59 | */ |
eencae | 1:d2b7780ca6b3 | 60 | |
eencae | 0:49b8e971fa83 | 61 | class MeArm |
eencae | 0:49b8e971fa83 | 62 | { |
eencae | 0:49b8e971fa83 | 63 | |
eencae | 0:49b8e971fa83 | 64 | public: |
eencae | 0:49b8e971fa83 | 65 | /** Create a MeArm object connected to the specified pins. MUST be PWM-enabled pins. |
eencae | 0:49b8e971fa83 | 66 | * |
eencae | 1:d2b7780ca6b3 | 67 | * @param Pin for middle (base servo) |
eencae | 1:d2b7780ca6b3 | 68 | * @param Pin for left servo |
eencae | 1:d2b7780ca6b3 | 69 | * @param Pin for right servo |
eencae | 1:d2b7780ca6b3 | 70 | * @param Pin for claw servo |
eencae | 0:49b8e971fa83 | 71 | */ |
eencae | 1:d2b7780ca6b3 | 72 | MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin); |
eencae | 1:d2b7780ca6b3 | 73 | |
eencae | 1:d2b7780ca6b3 | 74 | /** Destructor */ |
eencae | 1:d2b7780ca6b3 | 75 | ~MeArm(); |
eencae | 0:49b8e971fa83 | 76 | |
eencae | 1:d2b7780ca6b3 | 77 | /** Calibrate the claw servo |
eencae | 0:49b8e971fa83 | 78 | * |
eencae | 1:d2b7780ca6b3 | 79 | * @param pulse-width value (us) when claw is open |
eencae | 1:d2b7780ca6b3 | 80 | * @param pulse-width value (us) when claw is closed |
eencae | 0:49b8e971fa83 | 81 | */ |
eencae | 1:d2b7780ca6b3 | 82 | void set_claw_calibration_values(int open_val, int closed_val); |
eencae | 1:d2b7780ca6b3 | 83 | |
eencae | 1:d2b7780ca6b3 | 84 | /** Calibrate the middle servo |
eencae | 0:49b8e971fa83 | 85 | * |
eencae | 1:d2b7780ca6b3 | 86 | * @param pulse-width value (us) when base is facing forward (0 degrees) |
eencae | 1:d2b7780ca6b3 | 87 | * @param pulse-width value (us) when base is facing left (90 degrees) |
eencae | 0:49b8e971fa83 | 88 | */ |
eencae | 1:d2b7780ca6b3 | 89 | void set_middle_calibration_values(int zero, int ninety); |
eencae | 1:d2b7780ca6b3 | 90 | |
eencae | 1:d2b7780ca6b3 | 91 | /** Calibrate the left servo |
eencae | 0:49b8e971fa83 | 92 | * |
eencae | 1:d2b7780ca6b3 | 93 | * @param pulse-width value (us) when left servo horn is horizontal (0 degrees) |
eencae | 1:d2b7780ca6b3 | 94 | * @param pulse-width value (us) when left servo horn is vertical (90 degrees) |
eencae | 0:49b8e971fa83 | 95 | */ |
eencae | 1:d2b7780ca6b3 | 96 | void set_left_calibration_values(int zero, int ninety); |
eencae | 1:d2b7780ca6b3 | 97 | |
eencae | 1:d2b7780ca6b3 | 98 | /** Calibrate the left servo |
eencae | 0:49b8e971fa83 | 99 | * |
eencae | 1:d2b7780ca6b3 | 100 | * @param pulse-width value (us) when right servo horn is horizontal (0 degrees) |
eencae | 1:d2b7780ca6b3 | 101 | * @param pulse-width value (us) when right servo horn is vertical (90 degrees) |
eencae | 1:d2b7780ca6b3 | 102 | */ |
eencae | 1:d2b7780ca6b3 | 103 | void set_right_calibration_values(int zero, int ninety); |
eencae | 1:d2b7780ca6b3 | 104 | |
eencae | 1:d2b7780ca6b3 | 105 | /** Control the claw |
eencae | 1:d2b7780ca6b3 | 106 | * |
eencae | 1:d2b7780ca6b3 | 107 | * @param Value - range of 0.0 (closed) to 1.0 (open) |
eencae | 0:49b8e971fa83 | 108 | */ |
eencae | 1:d2b7780ca6b3 | 109 | void set_claw(float value); |
eencae | 1:d2b7780ca6b3 | 110 | |
eencae | 1:d2b7780ca6b3 | 111 | /** Move end-effector to position. Positions outside of workspace will result in error and code exiting. |
eencae | 1:d2b7780ca6b3 | 112 | * |
eencae | 1:d2b7780ca6b3 | 113 | * @param x - x coordinate (mm) forwards |
eencae | 1:d2b7780ca6b3 | 114 | * @param y - y coordinate (mm) left (> 0 mm) and right (< 0 mm) |
eencae | 1:d2b7780ca6b3 | 115 | * @param z - z coordinate (mm) up (must be > 0mm) |
eencae | 0:49b8e971fa83 | 116 | */ |
eencae | 1:d2b7780ca6b3 | 117 | void goto_xyz(float x, float y, float z); |
eencae | 0:49b8e971fa83 | 118 | |
eencae | 0:49b8e971fa83 | 119 | private: |
eencae | 1:d2b7780ca6b3 | 120 | |
eencae | 1:d2b7780ca6b3 | 121 | PwmOut *_middle_servo; |
eencae | 1:d2b7780ca6b3 | 122 | PwmOut *_left_servo; |
eencae | 1:d2b7780ca6b3 | 123 | PwmOut *_right_servo; |
eencae | 1:d2b7780ca6b3 | 124 | PwmOut *_claw_servo; |
eencae | 0:49b8e971fa83 | 125 | |
eencae | 1:d2b7780ca6b3 | 126 | float _a[4]; // array for link lengths |
eencae | 1:d2b7780ca6b3 | 127 | float _base_height; // height of base off floor |
eencae | 1:d2b7780ca6b3 | 128 | |
eencae | 1:d2b7780ca6b3 | 129 | float _theta[3]; // joint angles |
eencae | 1:d2b7780ca6b3 | 130 | int _pw[3]; |
eencae | 0:49b8e971fa83 | 131 | |
eencae | 1:d2b7780ca6b3 | 132 | // calibration values |
eencae | 1:d2b7780ca6b3 | 133 | int _theta_1_cal[2]; |
eencae | 1:d2b7780ca6b3 | 134 | int _theta_2_cal[2]; |
eencae | 1:d2b7780ca6b3 | 135 | int _theta_3_cal[2]; |
eencae | 1:d2b7780ca6b3 | 136 | int _claw_cal[2]; |
eencae | 1:d2b7780ca6b3 | 137 | |
eencae | 1:d2b7780ca6b3 | 138 | // target position |
eencae | 1:d2b7780ca6b3 | 139 | float _x; |
eencae | 1:d2b7780ca6b3 | 140 | float _y; |
eencae | 1:d2b7780ca6b3 | 141 | float _z; |
eencae | 1:d2b7780ca6b3 | 142 | |
eencae | 1:d2b7780ca6b3 | 143 | //IK functions |
eencae | 1:d2b7780ca6b3 | 144 | void calc_middle_angle(); |
eencae | 1:d2b7780ca6b3 | 145 | void calc_left_angle(); |
eencae | 1:d2b7780ca6b3 | 146 | void calc_right_angle(); |
eencae | 1:d2b7780ca6b3 | 147 | |
eencae | 1:d2b7780ca6b3 | 148 | void calc_pw(); |
eencae | 1:d2b7780ca6b3 | 149 | |
eencae | 1:d2b7780ca6b3 | 150 | void move_servos(); |
eencae | 1:d2b7780ca6b3 | 151 | |
eencae | 1:d2b7780ca6b3 | 152 | |
eencae | 0:49b8e971fa83 | 153 | }; |
eencae | 0:49b8e971fa83 | 154 | |
eencae | 0:49b8e971fa83 | 155 | #endif |