MeArm.h

00001 /**
00002 @file MeArm.h
00003 */
00004 
00005 #ifndef MEARM_H
00006 #define MEARM_H
00007 
00008 // used to convert RADIANS<->DEGREES
00009 #define PI 3.1415926536
00010 #define RAD2DEG (180.0/PI)
00011 #define DEG2RAD (PI/180.0)
00012 
00013 #include "mbed.h"
00014 
00015 /**
00016 @brief Library for controlling the MeArm (https://shop.mime.co.uk)
00017 
00018 @brief Revision 1.1
00019 
00020 @author Craig A. Evans
00021 @date   June 2017
00022 
00023 @code
00024 #include "mbed.h"
00025 #include "MeArm.h"
00026 
00027 // create MeArm object - base, shoulder, elbow, gripper
00028 MeArm arm(p21,p22,p23,p24);
00029 
00030 AnalogIn m_pot(p17);
00031 AnalogIn l_pot(p18);
00032 AnalogIn r_pot(p19);
00033 AnalogIn c_pot(p20);
00034 
00035 int main()
00036 {
00037     //                             open , closed
00038     arm.set_claw_calibration_values(1450,2160);
00039     //                                   0 , 90
00040     arm.set_middle_calibration_values(1650,2700);
00041     arm.set_right_calibration_values(2000,1000);
00042     arm.set_left_calibration_values(1870,850);
00043 
00044     while(1) {
00045         
00046         float x = 100.0 + 100.0*m_pot.read();
00047         float y = -140.0 + 280.0*l_pot.read();
00048         float z = 150.0*r_pot.read();   
00049         float claw_val = c_pot.read();   // 0.0 to 1.0
00050         
00051         arm.set_claw(claw_val);
00052         arm.goto_xyz(x,y,z);
00053 
00054         wait(0.2);      
00055     }    
00056 }
00057 @endcode
00058 
00059 */
00060 
00061 class MeArm
00062 {
00063 
00064 public:
00065     /** Create a MeArm object connected to the specified pins. MUST be PWM-enabled pins.
00066     *
00067     * @param Pin for middle (base servo)
00068     * @param Pin for left servo
00069     * @param Pin for right servo
00070     * @param Pin for claw servo
00071     */
00072     MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin);
00073 
00074     /** Destructor */
00075     ~MeArm();
00076 
00077     /** Calibrate the claw servo
00078     *
00079     * @param pulse-width value (us) when claw is open
00080     * @param pulse-width value (us) when claw is closed
00081     */
00082     void set_claw_calibration_values(int open_val, int closed_val);
00083 
00084     /** Calibrate the middle servo
00085     *
00086     * @param pulse-width value (us) when base is facing forward (0 degrees)
00087     * @param pulse-width value (us) when base is facing left (90 degrees)
00088     */
00089     void set_middle_calibration_values(int zero, int ninety);
00090 
00091     /** Calibrate the left servo
00092     *
00093     * @param pulse-width value (us) when left servo horn is horizontal (0 degrees)
00094     * @param pulse-width value (us) when left servo horn is vertical (90 degrees)
00095     */
00096     void set_left_calibration_values(int zero, int ninety);
00097 
00098     /** Calibrate the left servo
00099     *
00100     * @param pulse-width value (us) when right servo horn is horizontal (0 degrees)
00101     * @param pulse-width value (us) when right servo horn is vertical (90 degrees)
00102     */
00103     void set_right_calibration_values(int zero, int ninety);
00104 
00105     /** Control the claw
00106     *
00107     * @param Value - range of 0.0 (closed) to 1.0 (open)
00108     */
00109     void set_claw(float value);
00110 
00111     /** Move end-effector to position. Positions outside of workspace will result in error and code exiting.
00112     *
00113     * @param x - x coordinate (mm) forwards
00114     * @param y - y coordinate (mm) left (> 0 mm) and right (< 0 mm)
00115     * @param z - z coordinate (mm) up (must be > 0mm)
00116     */
00117     void goto_xyz(float x, float y, float z);
00118 
00119 private:
00120 
00121     PwmOut *_middle_servo;
00122     PwmOut *_left_servo;
00123     PwmOut *_right_servo;
00124     PwmOut *_claw_servo;
00125 
00126     float _a[4];            // array for link lengths
00127     float _base_height;     // height of base off floor
00128 
00129     float _theta[3];    // joint angles
00130     int _pw[3];
00131 
00132     // calibration values
00133     int _theta_1_cal[2];
00134     int _theta_2_cal[2];
00135     int _theta_3_cal[2];
00136     int _claw_cal[2];
00137 
00138     // target position
00139     float _x;
00140     float _y;
00141     float _z;
00142 
00143     //IK functions
00144     void calc_middle_angle();
00145     void calc_left_angle();
00146     void calc_right_angle();
00147 
00148     void calc_pw();
00149 
00150     void move_servos();
00151 
00152 
00153 };
00154 
00155 #endif