File content as of revision 0:4a15e2d20446:

//Robo class implementation for controlling robotic arm
// Servos: A 1ms pulse is 0 degrees, 1.5ms is 90 degrees and a 2 ms pulse is approximately 180 degrees. 
// New timing pulses are sent to the servo every 20 ms.
// In theory .005556ms = 1 degree
// 5.56
// Currently .000010 = 1 increment

#include "robo.h"
#include "mbed.h"

#define PI 3.14159265

robo::robo(PinName s, PinName b, PinName e, PinName w, PinName c,PinName t){
    spin = new PwmOut(s);
    base = new PwmOut(b);
    elbow = new PwmOut(e);
    wrist = new PwmOut(w);
    claw = new PwmOut(c);
    trash = new PwmOut(t);
    spin->period_ms(20);
    base->period_ms(20);
    elbow->period_ms(20);
    wrist->period_ms(20);
    claw->period_ms(20);
    trash->period_ms(20);
}

void robo::write(float angle, int servo){
    //float oneDeg = 5.555556;
    float oneRad = 318.309886;
    float spinOffset = 1150;
    float baseOffset = 950;
    float elbowOffset = 1250;
    float wristOffset = 950;
    float clawOffset = 1000;
    

        switch(servo){
            case(0):
                spin->pulsewidth_us(angle*oneRad + spinOffset);
            break;
            case(1):
                base->pulsewidth_us(angle*oneRad + baseOffset);
            break;
            case(2):
                elbow->pulsewidth_us(angle*oneRad + elbowOffset);
            break;
            case(3):
                wrist->pulsewidth_us(angle*oneRad + wristOffset);
            break;
            case(4):
                claw->pulsewidth_us(angle*10 + clawOffset);
            break;
            case(5):
                trash->pulsewidth_us(angle*oneRad + 1000);
            break;            
        }
     
}

void robo::writeSpin(float angle){
    write(angle,0);
}
void robo::writeBase(float angle){
    write(angle,1);
    }
void robo::writeElbow(float angle){
    write(angle,2);
}
void robo::writeWrist(float angle){
    write(angle,3);
}
void robo::writeClaw(float angle){
    write(angle,4);
}
void robo::writeTrash(float angle){
    write(angle,5);
}




float* robo::calculateangle(float x1e, float y1e, float oe, float angles[]){      

//relative units
float a1 = 95;//mm's from length from base to elbow;
float a2 = 110;//mm's from length from elbow to wrist;
float a3 = 45;//mm's from length from wrist to claw;

float ce = cos(oe);
float se = sin(oe);

float o2 = -acos(  ((x1e-a3*ce)*(x1e-a3*ce) + (y1e-a3*se)*(y1e-a3*se) - a1*a1 - a2*a2) / (2*a1*a2)   );

float s2 = sin(o2);
float c2 = cos(o2);

float delta = a1*a1 + a2*a2 + 2*a1*a2*c2;
float det1 = (x1e-a3*ce)*(a1+a2*c2) - (y1e-a3*se)*(-a2*s2);
                                    
float o1 = acos(det1/delta);
float o3 = oe-o1-o2;

angles[0]= o1;
angles[1]= -o2; //Had to use negative sign to reconcile with arm configuation.
angles[2]= -o3; //Had to use negative sign to reconcile with arm configuation.

return angles;


}