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
- Committer:
- eencae
- Date:
- 2017-07-04
- Revision:
- 1:d2b7780ca6b3
- Parent:
- 0:49b8e971fa83
File content as of revision 1:d2b7780ca6b3:
/** @file MeArm.h */ #ifndef MEARM_H #define MEARM_H // used to convert RADIANS<->DEGREES #define PI 3.1415926536 #define RAD2DEG (180.0/PI) #define DEG2RAD (PI/180.0) #include "mbed.h" /** @brief Library for controlling the MeArm (https://shop.mime.co.uk) @brief Revision 1.1 @author Craig A. Evans @date June 2017 @code #include "mbed.h" #include "MeArm.h" // create MeArm object - base, shoulder, elbow, gripper MeArm arm(p21,p22,p23,p24); AnalogIn m_pot(p17); AnalogIn l_pot(p18); AnalogIn r_pot(p19); AnalogIn c_pot(p20); int main() { // open , closed arm.set_claw_calibration_values(1450,2160); // 0 , 90 arm.set_middle_calibration_values(1650,2700); arm.set_right_calibration_values(2000,1000); arm.set_left_calibration_values(1870,850); while(1) { float x = 100.0 + 100.0*m_pot.read(); float y = -140.0 + 280.0*l_pot.read(); float z = 150.0*r_pot.read(); float claw_val = c_pot.read(); // 0.0 to 1.0 arm.set_claw(claw_val); arm.goto_xyz(x,y,z); wait(0.2); } } @endcode */ class MeArm { public: /** Create a MeArm object connected to the specified pins. MUST be PWM-enabled pins. * * @param Pin for middle (base servo) * @param Pin for left servo * @param Pin for right servo * @param Pin for claw servo */ MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin); /** Destructor */ ~MeArm(); /** Calibrate the claw servo * * @param pulse-width value (us) when claw is open * @param pulse-width value (us) when claw is closed */ void set_claw_calibration_values(int open_val, int closed_val); /** Calibrate the middle servo * * @param pulse-width value (us) when base is facing forward (0 degrees) * @param pulse-width value (us) when base is facing left (90 degrees) */ void set_middle_calibration_values(int zero, int ninety); /** Calibrate the left servo * * @param pulse-width value (us) when left servo horn is horizontal (0 degrees) * @param pulse-width value (us) when left servo horn is vertical (90 degrees) */ void set_left_calibration_values(int zero, int ninety); /** Calibrate the left servo * * @param pulse-width value (us) when right servo horn is horizontal (0 degrees) * @param pulse-width value (us) when right servo horn is vertical (90 degrees) */ void set_right_calibration_values(int zero, int ninety); /** Control the claw * * @param Value - range of 0.0 (closed) to 1.0 (open) */ void set_claw(float value); /** Move end-effector to position. Positions outside of workspace will result in error and code exiting. * * @param x - x coordinate (mm) forwards * @param y - y coordinate (mm) left (> 0 mm) and right (< 0 mm) * @param z - z coordinate (mm) up (must be > 0mm) */ void goto_xyz(float x, float y, float z); private: PwmOut *_middle_servo; PwmOut *_left_servo; PwmOut *_right_servo; PwmOut *_claw_servo; float _a[4]; // array for link lengths float _base_height; // height of base off floor float _theta[3]; // joint angles int _pw[3]; // calibration values int _theta_1_cal[2]; int _theta_2_cal[2]; int _theta_3_cal[2]; int _claw_cal[2]; // target position float _x; float _y; float _z; //IK functions void calc_middle_angle(); void calc_left_angle(); void calc_right_angle(); void calc_pw(); void move_servos(); }; #endif