File content as of revision 0:ee5445096838:

#include "mbed.h"
#include "SI1143.h"
#include "TextLCD.h"

#define PI 3.14159265

DigitalOut led1(LED1);
SI1143 sensor(p28, p27);
PwmOut spin(p21);
PwmOut shoulder(p22);
PwmOut elbow(p25);
PwmOut grip(p26);
TextLCD lcd(p15, p16, p17, p23, p19, p24);
Serial pc(USBTX, USBRX);
 
int main()
{
    //LED sensor, bottom-left, top-left, bottom-right
    int s1,s2,s3;
    double sense1,sense2,sense3;
    
    //distance between sensors in cm
    double diff = 5;
    
    //sensed and stored coordinates
    double xs,ys,zs;
    double x=45,y=5,z=50;
    
    //arbitrary position for arm
    double stx=60,sty=25,stz=100;
    
    //length of arm segment
    double len12=7.5, len23=15;
    
    //polar distance
    double r;
    
    //check values
    double root, tproot, wproot;
    int timer=0;
    
    //motor 1,2,3
    double angle_sp,angle_sh,angle_el;
    
    //Setup
    lcd.cls();
    led1 = 0;
    grip.pulsewidth_us(700);
    sensor.bias(4,20);
    wait(1);
    
    while(1)
    {
        //Read each led sensor
        s1 = sensor.get_ps1(4);
        s2 = sensor.get_ps2(4);
        s3 = sensor.get_ps3(4);
        sense1 = /*1000 - */1500/s1;
        sense2 = /*1000 - */1500/s2;
        sense3 = /*1000 - */1200/s3;
        
        //Control for gripper
        if (((s1>800) || (s2>600) || (s3>600)) && !timer)
        {
            timer=20;
            if (!led1)
            {
                led1 = 1;
                grip.pulsewidth_us(1350);
            }
            else
            {
                led1 = 0;
                grip.pulsewidth_us(700);
            }
        }
        timer--;
        if (timer < 0) timer=0;
        
        //Set outer limit
        if (sense1>110) sense1 = 110;
        if (sense2>110) sense2 = 110;
        if (sense3>110) sense3 = 110;
        
        //Trilateration for x
        xs = 40 + ((pow(sense1,2) - pow(sense3,2) + pow(diff,2)) / (2*diff));
        if (xs<5) xs = 10;
        if (xs>95) xs = 90;
        
        //Trilateration for y
        ys = -sty + ((pow(sense1,2) - pow(sense2,2) + pow(diff,2)) / (2*diff));
        if (ys<-50) ys = -40;
        if (ys>50) ys = 40;
        
        //Trilateration for z
        root = pow(sense1+60,2) - pow(y,2) - pow(x,2);
        zs = sqrt(root);
        if (zs>70) zs = 70;
        
        //Tolerance level to store x,y,z
        if (abs(x-xs) <= 7) x = xs;
        if (abs(y-ys) <= 10) y = ys;
        if (abs(z-zs) <= 6) z = zs;
        
        //Used for debugging
        //x=60;
        //z=15;
        //for(y=-90;y<=90;y+=10){wait(1);
        
        //Angle for spin motor, converted into degrees
        angle_sp = atan((stz - z)/(x - stx)) * 180/PI;
        if (angle_sp < 0)
            angle_sp += 180;
        angle_sp = 180 - angle_sp;
        
        //Polar distance and arcosine wrapping
        r = sqrt(pow(x - stx,2) + pow(stz - z,2));
        wproot = 2*pow(r,2) - pow(len12,2) - pow(len23,2);
        while (wproot > 225)
            wproot -= 225;
        while (wproot < -225)
            wproot += 225;
        
        //Angles for elbow motor first, then shoulder motor
        angle_el = acos(wproot/(2*len12*len23));
        tproot = len12 + len23*cos(angle_el);
        angle_sh = (-r*len23*sin(angle_el) + y*tproot)/(y*len23*sin(angle_el) + r*tproot);
        
        //Angles for shoulder and elbow motor, converted into degrees
        angle_sh = 180 - (angle_sh * 180/PI);
        angle_el = 90 + (angle_el * 180/PI);
        
        //Covert degrees to pulses to appropriate motors
        spin.pulsewidth_us(800 + (angle_sp*20)/3);
        shoulder.pulsewidth_us(800 + (angle_sh*20)/3);
        elbow.pulsewidth_us(2300 - (angle_el*95)/9);
        
        //Debug info
        printf("%.1f %.1f %.1f\r\n",xs,ys,zs);
        //printf("%.1f %.1f %.1f\r\n",x,y,z);
        //printf("%d %d %d\r\n",s1,s2,s3);
        lcd.cls();
        lcd.printf("sp:%.0f sh:%.0f\nel:%.0f ",angle_sp,180-angle_sh,angle_el);
        lcd.printf("%.0f,%.0f,%.0f",x,y,z);//}
        //printf("%.2f %.2f %.2f\r\n\n",angle_sp,angle_sh,angle_el);
    }
}