File content as of revision 0:49b8e971fa83:

/*
@file MeArm.cpp

@brief Member functions implementations

*/
#include "mbed.h"
#include "MeArm.h"

MeArm::MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin)
{
    // 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);
}

void MeArm::setTarget(float x,float y,float z)
{
    target.x = x;
    target.y = y;
    target.z = z;    
    
    // TODO - add checks for valid target
}

/// TODO - add checks for valid angles
void MeArm::moveBaseServo(float angle)
{
    // 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
}

void MeArm::moveShoulderServo(float angle)
{
    // 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
}

void MeArm::moveElbowServo(float angle)
{
    // 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
}

void MeArm::moveServos()
{
    moveBaseServo(thetaBase);    
    moveShoulderServo(thetaShoulder);    
    moveElbowServo(thetaElbow);    
}

// calculate angle in DEGREES
float MeArm::cosineRule(float adj1, float adj2, float opp)
{
    // cosine rule
    float tmp = (adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2);
    //println("tmp = " + tmp);

    // check for valid angle
    if ( fabs(tmp) <= 1.0 ) {
        return RAD2DEG*acos(tmp);  
    } else {
        error(1);
    }

    return -1.0;  // will never get here due to infinite loop
}

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

void MeArm::solveInverseKinematics()
{
    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)

    // 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
    }

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

    // if target out of reach
    if (rTarget > maxReach ) {
        error(3); // hang and flash error code
    }

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

    // 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);
    
    // elbow internal angle
    float beta = cosineRule(L1, L2, Dsw);
    serial->printf("Elbow internal angle = %f degrees\n",beta);

    // 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);
    
    // calculate angle from top view
    thetaBase = RAD2DEG*atan2(target.y, target.x);
    serial->printf("Base servo angle = %f degrees\n",thetaBase);
}