File content as of revision 0:bdd172e29b8b:

#include "mbed.h"
#include "Servo.h"
#include "rtos.h"

Servo shoulder(p26); // pwm
Servo elbow(p25); // pwm
Servo wrist(p24); // pwm
Servo hand(p23); // pwm

I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
Serial pc(USBTX, USBRX);    //Used to view the colors that are read in
 
int sensor_addr = 41 << 1;

float clear_value;
float red_value;
float blue_value;
float green_value;

float newRedValue;
float oldRedValue;


 
DigitalOut green(LED1);
DigitalOut led4(LED4);
DigitalOut led(p11);

void stroke();
void dunk();
void rotate();
void readLight();

float shoulderPosition;
//
Thread threadLight;
Thread threadRotate;

void blinky() {
        led4 = !led4;
        //wait(0.2);
    }

int main()
{   

threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds) 
threadRotate.start(rotate);// to stop thread thread_name.terminate();







}

void rotate(){
    wrist = 0.0f;
    elbow = 0.30f;
    
    //while(doRotate){

    float count = 1.0f;
   
           while (count>=0.0f) {
         
             
        float sCount = count/2;
        
        if(newRedValue <= oldRedValue - .04 ){
        shoulderPosition = 0.5f + sCount;
        
        }
        shoulder = 0.0f + sCount;;
        count-= 0.01f;
    
       // wait(.05);
        Thread::wait(50);

    }
   
   
    while (count<=1.0f) {
       
        float sCount = count/2;
        
        
        shoulder = 0.0f + sCount;
        count+= 0.01f;
        Thread::wait(50);
        
        //wait(.05);

    }
    

    
    
    threadLight.terminate();
    
     dunk();
    
    threadRotate.terminate();
   

   //} 
    
    
}

void stroke(){
    
    shoulder = shoulderPosition;
    
    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
        float wCount =  i/1.5;
        float eCount = i/2;
        //hand = 1.5f ;
        wrist = 0.0f + wCount;
        elbow =  0.19f + eCount;//wCount; //0.47f;
        wait(.05);
        //printf("wCount is %f \n", wCount);
//        printf("eCount is %f \n", eCount);


    }
    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
        float wCount =  i/1.5;
        float eCount = i/2;
        //hand = 1.5f ;
        wrist = 0.33f - wCount;
        elbow =  0.44f - eCount;//wCount; //0.47f;
        //printf("wCount is %f \n", wCount);
//        printf("eCount is %f \n", eCount);
        wait(.05);

    } 
}

void dunk(){
        
        shoulder = 0.3f;
        
        float wristIN;
        float elbowIN;
   
    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
        float wCount =  i/1.5;
        float eCount = i/2;
        //hand = 1.5f ;
        wristIN = 0.2f - wCount;//equals.33f at end
        //elbowIN =  0.5f + eCount;//equals .44f at the end
        //wait(.5);
        wrist = wristIN;
        elbow = 0.3f;
        //
//     printf("wCount is %f \n", wCount);
//       printf("eCount is %f \n", eCount);
     wait(.05);

    }
    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
        float wCount =  i/1.5;
        float eCount = i/2;
        //hand = 1.5f ;
        //wait(.5);
        wrist = wristIN + wCount;
        //elbow = wristIN - eCount;//wCount; //0.47f;
//        printf("wCount is %f \n", wCount);
//        printf("eCount is %f \n", eCount);
    wait(.05);

    } 
    
    stroke();
    
}


void readLight () {
    
    
    pc.baud(9600);
    green = 1; // off
    
    // Connect to the Color sensor and verify
    
    i2c.frequency(200000);
    
    char id_regval[1] = {146};
    char data[1] = {0};
    i2c.write(sensor_addr,id_regval,1, true);
    i2c.read(sensor_addr,data,1,false);
    
    if (data[0]==68) {
        green = 0;
        wait (2); 
        green = 1;
        } else {
        green = 1; 
    }
    
    // Initialize color sensor
    
    char timing_register[2] = {129,0};
    i2c.write(sensor_addr,timing_register,2,false);
    
    char control_register[2] = {143,0};
    i2c.write(sensor_addr,control_register,2,false);
    
    char enable_register[2] = {128,3};
    i2c.write(sensor_addr,enable_register,2,false);
    
    // Read data from color sensor (Clear/Red/Green/Blue)
    led = 1;
  while (true) { 
        char clear_reg[1] = {148};
        char clear_data[2] = {0,0};
        i2c.write(sensor_addr,clear_reg,1, true);
        i2c.read(sensor_addr,clear_data,2, false);
        
         clear_value = ((int)clear_data[1] << 8) | clear_data[0];
        
        char red_reg[1] = {150};
        char red_data[2] = {0,0};
        i2c.write(sensor_addr,red_reg,1, true);
        i2c.read(sensor_addr,red_data,2, false);
        
         red_value = ((int)red_data[1] << 8) | red_data[0];
        
        char green_reg[1] = {152};
        char green_data[2] = {0,0};
        i2c.write(sensor_addr,green_reg,1, true);
        i2c.read(sensor_addr,green_data,2, false);
        
         green_value = ((int)green_data[1] << 8) | green_data[0];
        
        char blue_reg[1] = {154};
        char blue_data[2] = {0,0};
        i2c.write(sensor_addr,blue_reg,1, true);
        i2c.read(sensor_addr,blue_data,2, false);
        
         blue_value = ((int)blue_data[1] << 8) | blue_data[0];
        
        // print sensor readings
        red_value = red_value / clear_value;
        newRedValue = red_value;
        green_value = green_value/clear_value;
        blue_value = blue_value/clear_value;
        pc.printf("Clear (%.2f), Red (%.2f), old red (%.2f), new red (%.2f)\n, Shoulder position %.2f \n", clear_value, red_value, oldRedValue, newRedValue, shoulderPosition);
        //The above code displays the red, green, and blue values read in by the color sensor.
        //wait(0.5);
         Thread::wait(300);
         
         oldRedValue = newRedValue;
    
    
}
    
    
    
}