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