File content as of revision 1:d2b7780ca6b3:

/*
@file MeArm.cpp
*/
#include "mbed.h"
#include "MeArm.h"

MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin)
{
    // 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);

}

MeArm::~MeArm()
{
    delete _middle_servo, _left_servo, _right_servo, _claw_servo;
}

void MeArm::set_claw_calibration_values(int open_val, int closed_val)
{
    _claw_cal[0] = closed_val;
    _claw_cal[1] = open_val;
}

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)
{
    _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::set_claw(float value)
{
    // 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::goto_xyz(float x, float y, float z)
{
    // 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();    
}

// 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()
{
// 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];

    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);
    }

    // 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]);


    // 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]);

}

// shoulder servo
void MeArm::calc_right_angle()
{
    // 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

    float d = sqrt( _x*_x + _y*_y );
    float x_bar = d - _a[0] - _a[3];

    // 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 (E > r) {  // if this is the case, we will get complex roots and hence invalid solution
        printf("Error!\n");
        exit(1);
    }

    // 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]);

}

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]);
    
    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]);
    
}

void MeArm::move_servos() {
    
    //printf("Move servos\n");
    _middle_servo->pulsewidth_us(_pw[0]);
    _right_servo->pulsewidth_us(_pw[1]); 
    _left_servo->pulsewidth_us(_pw[2]);    
}