File content as of revision 0:4a15e2d20446:

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

#define PI 3.14159265
#include "mbed.h"

robo arm(p21, p22, p23, p24, p25, p26);
Serial myserial(p28, p27);

float deg = 180/PI;
float rad = PI/180;
float openclaw = 0;
float closeclaw = 100;

char xin;
char yin;
char cords[3];
int x;
int y;
int z;
int zin;

int main() {

//Start Arm in standard position.//
arm.writeBase(90*rad);   
wait(0.2);
arm.writeElbow(90*rad); 
wait(0.2);
arm.writeWrist(90*rad);
wait(0.2);
arm.writeClaw(openclaw);
wait(0.2);


while (1) {

    myserial.scanf("%s", &cords); //scan a string with the coordinates of the object;
        
    xin=cords[0];
    zin=cords[1];
        
    x= (int)xin;   //x will be distance sent by amigobot's camera. (It is y in the image, but x in the algorithm)
    y= -70;       //height form the base of the arm to the top of the trash in mm. Right now it is hardcoded.
    z= (int)zin; //z will be the horizontal distance. Right now its not being used.

    float * angles;  //Initialize angles vector where the calculatiosn will be stored.
    angles = (float *)malloc(3 * sizeof(float));
    angles[0] = 0;
    angles[1] = 0;
    angles[2] = 0;

    float oe = -90*rad;
    angles = arm.calculateangle(x, y, oe, angles);// convert positions to angles and store in angles variable

    //store the angles into a printable variable (Haven't had luck printing array entries)
    float baseangle = angles[0];
    float elbowangle = angles[1];
    float wristangle = angles[2];

    // Pick up Object
    arm.writeBase(baseangle);   //move arm based on calculated angles
    wait(0.05);
    arm.writeElbow(elbowangle); //move arm based on calculated angles
    wait(0.05);
    arm.writeWrist(wristangle); //move arm based on calculated angles
    wait(1.5);
    arm.writeClaw(closeclaw);   //close the claw before moving to object
    wait(1);

    // Deposit Object in bin behind it
    arm.writeBase(70*rad);
    wait(0.2);
    arm.writeElbow(0*rad);
    wait(0.7);
    arm.writeElbow(-90*rad);
    wait(0.7);
    arm.writeWrist(0*rad);
    wait(1);

    // Let go of Object
    arm.writeClaw(openclaw);
    wait(1.5);

    //Return to initial position
    arm.writeBase(90*rad);
    wait(0.2);
    arm.writeElbow(0*rad);
    wait(1);
    arm.writeElbow(90*rad); 
    wait(0.2);
    arm.writeWrist(90*rad);
    wait(0.2);      
    //free(angles);
}//end of while     
}//end main