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.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?

UserRevisionLine numberNew 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