Revision 1:d2b7780ca6b3, committed 2017-07-04

Comitter:

eencae

Date:

Tue Jul 04 16:13:12 2017 +0000

Parent:

0:49b8e971fa83

Commit message:

Initial commit

Changed in this revision

--- a/MeArm.cpp	Tue Nov 04 20:54:49 2014 +0000
+++ b/MeArm.cpp	Tue Jul 04 16:13:12 2017 +0000
@@ -1,164 +1,185 @@
 /*
 @file MeArm.cpp
-
-@brief Member functions implementations
-
 */
 #include "mbed.h"
 #include "MeArm.h"
 
-MeArm::MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin)
+MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin)
 {
-    // set up pins as required
-    base = new PwmOut(basePin);
-    shoulder = new PwmOut(shoulderPin);
-    elbow = new PwmOut(elbowPin);
-    gripper = new PwmOut(gripperPin);
-    // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
-    // can do it anyway to be sure. All channels share same period so just need to set one of them
-    base->period(1.0/50.0);  // servos are typically 50 Hz
-    // setup USB serial for debug/comms
-    serial = new Serial(USBTX,USBRX);
-    // leds used for debug
-    leds = new BusOut(LED4,LED3,LED2,LED1);
+    // PwmOut objects for servos
+    _middle_servo = new PwmOut(middle_pin);
+    _left_servo = new PwmOut(left_pin);
+    _right_servo = new PwmOut(right_pin);
+    _claw_servo = new PwmOut(claw_pin);
+
+    // set link lengths (mm)
+    _a[0] = 15.0;
+    _a[1] = 80.0;
+    _a[2] = 80.0;
+    _a[3] = 65.0;
+    
+    // default calibration values, will need changing for each MeArm   
+    _claw_cal[0] = 2200;
+    _claw_cal[1] = 1400;
+    //middle
+    _theta_1_cal[0] = 1650;
+    _theta_1_cal[1] = 2700;
+    // right
+    _theta_2_cal[0] = 2000;
+    _theta_2_cal[1] = 1000;
+    //left 
+    _theta_3_cal[0] = 1900;
+    _theta_3_cal[1] = 850;
+    
+    // height of shoulder servo off the floor (including gears under gripper)
+    _base_height = 30.0;
+
+    // set PWM period - all channels share so just need to set one
+    _middle_servo->period_ms(20);
+
 }
 
-void MeArm::setTarget(float x,float y,float z)
+MeArm::~MeArm()
 {
-    target.x = x;
-    target.y = y;
-    target.z = z;    
-    
-    // TODO - add checks for valid target
+    delete _middle_servo, _left_servo, _right_servo, _claw_servo;
 }
 
-/// TODO - add checks for valid angles
-void MeArm::moveBaseServo(float angle)
+void MeArm::set_claw_calibration_values(int open_val, int closed_val)
 {
-    // arm facing forward is defined as 90 degrees, angle increases counter-clockwise
-    int pulseWidthMicroSeconds = 575.0 + 985.0*angle/90.0;
-    serial->printf("Base Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    base->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    _claw_cal[0] = closed_val;
+    _claw_cal[1] = open_val;
 }
 
-void MeArm::moveShoulderServo(float angle)
+void MeArm::set_middle_calibration_values(int zero, int ninety)
+{
+    _theta_1_cal[0] = zero;
+    _theta_1_cal[1] = ninety;
+}
+void MeArm::set_right_calibration_values(int zero, int ninety)
 {
-    // angle relative to horizontal
-    int pulseWidthMicroSeconds = 2705.0 - 1097.0*angle/90.0;
-    serial->printf("Shoulder Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    shoulder->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    _theta_2_cal[0] = zero;
+    _theta_2_cal[1] = ninety;
+}
+void MeArm::set_left_calibration_values(int zero, int ninety)
+{
+    _theta_3_cal[0] = zero;
+    _theta_3_cal[1] = ninety;
 }
 
-void MeArm::moveElbowServo(float angle)
+void MeArm::set_claw(float value)
 {
-    // angle relative to horizontal
-    int pulseWidthMicroSeconds = 1910.0 - 1095.0*angle/90.0;
-    serial->printf("Elbow Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    elbow->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    // open - closed value
+    int range = _claw_cal[1]  -_claw_cal[0];
+
+    // closed + val * range
+    int pw = _claw_cal[0] + int(range*value);
+
+    _claw_servo->pulsewidth_us(pw);
 }
 
-void MeArm::moveServos()
+void MeArm::goto_xyz(float x, float y, float z)
 {
-    moveBaseServo(thetaBase);    
-    moveShoulderServo(thetaShoulder);    
-    moveElbowServo(thetaElbow);    
+    // set position
+    _x=x;
+    _y=y;
+    _z=z - _base_height;
+    
+    //printf("x = %f y = %f z = %f\n",x,y,z);
+
+    // do inverse kinematics
+    calc_middle_angle();
+    calc_right_angle();
+    calc_left_angle();
+    
+    // convert to pulse-width
+    calc_pw();
+    
+    // update servo position
+    move_servos();    
 }
 
-// calculate angle in DEGREES
-float MeArm::cosineRule(float adj1, float adj2, float opp)
+// base servo
+void MeArm::calc_middle_angle()
+{
+    _theta[0] = RAD2DEG * atan2( _y , _x);
+    //printf("theta[0] = %f\n",_theta[0]);
+}
+
+// "elbow" servo
+void MeArm::calc_left_angle()
 {
-    // cosine rule
-    float tmp = (adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2);
-    //println("tmp = " + tmp);
+// for the elbow, we use the 2-link planar simplification,
+    // so work out the direct distance (radius) from the shoulder to the target in xy
+    // then subtract the shoulder and gripper offsets to get the 'x' value
+    float d = sqrt( _x*_x + _y*_y );
+    float x_bar = d - _a[0] - _a[3];
 
-    // check for valid angle
-    if ( fabs(tmp) <= 1.0 ) {
-        return RAD2DEG*acos(tmp);  
-    } else {
-        error(1);
+    float D = (x_bar*x_bar + _z*_z - _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[2]*_a[1]);
+
+    if ( D*D > 1.0) {   // if this is the case, we will get complex roots and hence invalid solution
+        printf("Error!\n");
+        exit(1);
     }
 
-    return -1.0;  // will never get here due to infinite loop
-}
+    // from IK derivation
+    // we want the -ve root to get the 'elbow' up solution
+    _theta[2] = RAD2DEG*atan2( -sqrt(1-D*D) , D);
+    //printf("theta[2] = %f\n",_theta[2]);
+
 
-void MeArm::error(int code)
-{
-    // flash error code in infinite loop
-    while(1) {
-        leds->write(code);
-        wait_ms(100);
-        leds->write(0);
-        wait_ms(100);
-    }
+    // we don't actually control the elbow angle in the MeArm
+    // we need to convert to the angle that we do control
+    _theta[2] = -(_theta[1] + _theta[2]);
+    //printf("theta[2] = %f\n",_theta[2]);
+
 }
 
-void MeArm::solveInverseKinematics()
+// shoulder servo
+void MeArm::calc_right_angle()
 {
-    serial->printf("#################################\n");
-    serial->printf("\nx = %f y = %f z = %f\n\n",target.x,target.y,target.z);
-    
-    float maxReach = 220.0;  // distance when arm outstretched (mm)
+    // for the shoulder, we use the 2-link planar simplification,
+    // so work out the direct distance (radius) from the shoulder to the target in xy
+    // then subtract the shoulder and gripper offsets to get the 'x' value
 
-    // rudimentary checks if target is within reach - won't cover all possibilities - need to catch more later
-    if (target.z > 140.0 || target.z < 25.0 || target.y < 0 || target.y > maxReach || target.x < -maxReach || target.x > maxReach ) {
-        error(2);  // hang and flash error code
-    }
+    float d = sqrt( _x*_x + _y*_y );
+    float x_bar = d - _a[0] - _a[3];
 
-    // distance to target in x-y plane
-    float rTarget = sqrt(target.x*target.x + target.y*target.y);
-    serial->printf("Distance to target = %f mm\n",rTarget);
+    // from IK derivation
+    float E = (x_bar*x_bar + _z*_z + _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[1]);
+    float r = sqrt( x_bar*x_bar + _z*_z );
 
-    // if target out of reach
-    if (rTarget > maxReach ) {
-        error(3); // hang and flash error code
+    if (E > r) {  // if this is the case, we will get complex roots and hence invalid solution
+        printf("Error!\n");
+        exit(1);
     }
 
-    // distance to wrist position in x-y plane
-    float rWrist = rTarget - L3;
-    serial->printf("Distance to wrist = %f mm\n",rWrist);
-
-    // shoulder offset from centre of rotation of base
-    float rShoulder = L0;
-    serial->printf("Distance to shoulder = %f mm\n",rShoulder);
+    // from IK derivation
+    // we want the +ve root to get the 'elbow' up solution
+    _theta[1] = RAD2DEG*( atan2(sqrt( r*r - E*E ),E) + atan2(_z,x_bar) );
+    //printf("theta[1] = %f\n",_theta[1]);
 
-    // direct distance from shoulder to wrist
-    float Dsw = sqrt( (rWrist - rShoulder)*(rWrist - rShoulder) + (target.z-BASE_HEIGHT)*(target.z-BASE_HEIGHT) );
-    serial->printf("Direct distance from shoulder to wrist = %f mm\n",Dsw);
-
-    // angle from shoulder to wrist
-    float thetaSw;
-    // angle from shoulder to wrist - with check
-    if (fabs((rWrist - rShoulder)/Dsw) <= 1.0) {
-        thetaSw = RAD2DEG*acos((rWrist - rShoulder)/Dsw);
-    } else {
-      error(4); 
-    }
+}
 
-    // if target is below base, then reflect the angle
-    if (target.z < BASE_HEIGHT)
-        thetaSw*=-1.0;
-
-    serial->printf("Angle from shoulder to wrist = %f degree\n" , thetaSw);
+void MeArm::calc_pw(){
+    
+    // 90 - 0
+    float range = (_theta_1_cal[1] - _theta_1_cal[0])/90.0;
+    _pw[0] = _theta_1_cal[0] + int(range*_theta[0]);
     
-    // elbow internal angle
-    float beta = cosineRule(L1, L2, Dsw);
-    serial->printf("Elbow internal angle = %f degrees\n",beta);
+    range = (_theta_2_cal[1] - _theta_2_cal[0])/90.0;
+    _pw[1] = _theta_2_cal[0] + int(range*_theta[1]);
+    
+    range = (_theta_3_cal[1] - _theta_3_cal[0])/90.0;
+    _pw[2] = _theta_3_cal[0] + int(range*_theta[2]);
+    
+    //printf("pw[0] = %i pw[1] = %i pw[2] = %i\n",_pw[0],_pw[1],_pw[2]);
+    
+}
 
-    // shoulder angle from horizontal
-    thetaShoulder = thetaSw + cosineRule(Dsw, L1, L2);
-    serial->printf("Shoulder servo angle = %f degrees\n",thetaShoulder);
-
-    // convert to servo angle
-    thetaElbow = 180.0 - thetaShoulder - beta;
-    serial->printf("Elbow servo angle = %f\n",thetaElbow);
+void MeArm::move_servos() {
     
-    // calculate angle from top view
-    thetaBase = RAD2DEG*atan2(target.y, target.x);
-    serial->printf("Base servo angle = %f degrees\n",thetaBase);
-}
+    //printf("Move servos\n");
+    _middle_servo->pulsewidth_us(_pw[0]);
+    _right_servo->pulsewidth_us(_pw[1]); 
+    _left_servo->pulsewidth_us(_pw[2]);    
+}
\ No newline at end of file

--- a/MeArm.h	Tue Nov 04 20:54:49 2014 +0000
+++ b/MeArm.h	Tue Jul 04 16:13:12 2017 +0000
@@ -1,132 +1,155 @@
 /**
 @file MeArm.h
-
-@brief Header file containing member functions and variables
-
 */
 
 #ifndef MEARM_H
 #define MEARM_H
 
-// used to convert RADIANS to DEGREES
-#define RAD2DEG 57.2957795
-// length of base to shoulder,upper arm,forearm and gripper (mm)
-#define L0 18.0
-#define L1 80.0
-#define L2 80.0
-#define L3 55.0
-// height of base of ground (mm)
-#define BASE_HEIGHT 60.0
-
-// struct used to store coordinates in 3D space
-typedef struct vector_s {
-    float x;
-    float y;
-    float z;
-} vector;
+// used to convert RADIANS<->DEGREES
+#define PI 3.1415926536
+#define RAD2DEG (180.0/PI)
+#define DEG2RAD (PI/180.0)
 
 #include "mbed.h"
 
 /**
-@brief mbed library for controlling the phenoptix #MeArm http://www.phenoptix.com/collections/mearm
-@brief The library solves the inverse kinematic equations of the arm to position the gripper at a
-@brief specified [X,Y,Z] cordinate. From the perspective of the arm base, the X-direction is left-to-right
-@brief (centered at 0). The Y-direction is forwards (centered at 0) and up/down is the Z-direction (0 level
-@brief with the base servo).
+@brief Library for controlling the MeArm (https://shop.mime.co.uk)
 
-@brief Revision 0.1
+@brief Revision 1.1
 
 @author Craig A. Evans
-@date   October 2014
- *
- * Example:
- * @code
+@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()
 {
-    arm.setTarget(50.0,100.0,120.0);  // set x,y,z co-ordinate in mm
-    arm.solveInverseKinematics();  // solve for servo angles
-    arm.moveServos();   // update servos
-
-    while (1) {
+    //                             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
 
- * @endcode
- */
+*/
+
 class MeArm
 {
 
 public:
     /** Create a MeArm object connected to the specified pins. MUST be PWM-enabled pins.
     *
-    * @param base Pin connected to the signal line on the base rotation servo
-    * @param shoulder Pin connected to the signal line on the shoulder servo)
-    * @param elbow Pin connected to the signal line on the elbow servo
-    * @param gripper Pin connected to the signal line on the gripper servo
+    * @param Pin for middle (base servo)
+    * @param Pin for left servo
+    * @param Pin for right servo
+    * @param Pin for claw servo
     */
-    MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin);
+    MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin);
+
+    /** Destructor */
+    ~MeArm();
 
-    /**  Set base angle of arm. Forwards is 90 degrees, angles increase counter-clockwise.
+    /** Calibrate the claw servo
     *
-    * @param angle Angle in degrees
+    * @param pulse-width value (us) when claw is open
+    * @param pulse-width value (us) when claw is closed
     */
-    void moveBaseServo(float angle);
-    /**  Set shoulder angle of arm. Horizontal is 0 degrees, angles increase clockwise.
+    void set_claw_calibration_values(int open_val, int closed_val);
+
+    /** Calibrate the middle servo
     *
-    * @param angle Angle in degrees
+    * @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 moveShoulderServo(float angle);
-    /**  Set elbow angle of arm. Horizontal is 0 degrees, angles increase counter-clockwise.
-    *    This is NOT the internal elbow angle, but the angle of the elbow servo from horizontal.
+    void set_middle_calibration_values(int zero, int ninety);
+
+    /** Calibrate the left servo
     *
-    * @param angle Angle in degrees
+    * @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 moveElbowServo(float angle);
-    /**  Set target co-ordinate of gripper in [x,y,z]. Distances in mm.
+    void set_left_calibration_values(int zero, int ninety);
+
+    /** Calibrate the left servo
     *
-    * @param x left-to-right (centred on 0)
-    * @param y forwards
-    * @param z up-and-down
+    * @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 setTarget(float x,float y,float z);
-    /**  Solve inverse kinematics of arm to get the servo angles
+    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 solveInverseKinematics();
-    /**  Move servos to calculated angles
-    */
-    void moveServos();
+    void goto_xyz(float x, float y, float z);
 
 private:
-    // calculate angle using cosine rule
-    float cosineRule(float adj1, float adj2, float opp);
-    // hang in infinite loop flashing code on LEDs
-    void error(int code);
+
+    PwmOut *_middle_servo;
+    PwmOut *_left_servo;
+    PwmOut *_right_servo;
+    PwmOut *_claw_servo;
 
-public:
+    float _a[4];            // array for link lengths
+    float _base_height;     // height of base off floor
+
+    float _theta[3];    // joint angles
+    int _pw[3];
 
-private:  // private variables
-    // pins for servos
-    PwmOut* base;
-    PwmOut* shoulder;
-    PwmOut* elbow;
-    PwmOut* gripper;
-    Serial* serial;  // for comms
-    BusOut* leds;    // for debug
-    // angles
-    float thetaBase;
-    float thetaShoulder;
-    float thetaElbow;
-    // vector to store 3D coordinate of target
-    vector target;
+    // 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
\ No newline at end of file