File content as of revision 1:c162c077430f:

#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
DigitalIn pb(p8);

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;

int new_pb = 0;
int old_pb = 0;
bool state = 0;


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

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

float shoulderPosition;
float sh;
//float shoulder;
//
Thread threadLight;
Thread threadRotate;

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

int main()
{   

    pb.mode(PullUp);

    while(1){
            
                new_pb=pb;
                
                    if (state == 0){                             
                            
                            led4=0;
                                                    
                         
                             
                             if((new_pb == 0) && (old_pb==1)){
                                    
                                        state =1;
                                            
                                              }
                                                                
                                }
                      else {
                                 
                                 led4=1;
                           //      stroke();
                                 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();
                                
                                     
                             if((new_pb == 0) && (old_pb==1)){
                                    
                                        state =0;
                                            
                                              }                                                               
                                
                                }          
                                
                    old_pb = new_pb;     
                                
            }



}


void rotate(){
    
    // shoulder=0.37f;
     wrist = 0.1f;
     elbow = 0.10f;
     
   // elbow = 0.00f;
    //while(doRotate){
    
    
for (float i = 0.44f; i > 0.0f; i-=0.01f) {// Forward cycle
                
                led3=0;
                shoulder=i;
                wait_ms(100);
               // pc.printf("shoulder %.2f  \n",i);
                
                                  }
                                  
                                  
for (float j = 0; j < 0.34f; j+=0.01f) {  // Return cycle take reading
                
                led3=0;
                shoulder=j;
                wait_ms(100);
                
                if(newRedValue <= 0.615     ){//abs(newRedValue-oldRedValue)>=0.03){  //newRedValue <= oldRedValue - .02 ){
 
                    shoulderPosition = shoulder;
                    
                 }
               
          //     pc.printf("shoulder %.2f \n",j);
                
                                  }
                                  
 for (float j = 0.34; j < 0.50f; j+=0.01f) {  // Return cycle 
                
                led3=1;
                shoulder=j;
                wait_ms(100);
                
        //        pc.printf("shoulder %.2f \n",j);
                
                                  }                                 
     

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

    
    dunk();
    
    threadRotate.terminate();
   

   //} 
    
    
}


void stroke(){
    
   shoulder = shoulderPosition-.10;
   
    Thread::wait(2000);
    
    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.50f;
        
        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("Red (%.2f), old red (%.2f), new red (%.2f), shoul(%.2f)\n, Shoulder position %.2f \n", red_value, oldRedValue, newRedValue,sh ,shoulderPosition);
        //The above code displays the red, green, and blue values read in by the color sensor.
        //wait(0.5);
       
         
         pc.printf("new Red Value %.2f  old Red value %.2f shoulder  %.2f \n",newRedValue,oldRedValue,shoulderPosition);
       //    pc.printf("%.2f\n",newRedValue);
         
         
         Thread::wait(300);
        oldRedValue = newRedValue;
         
    
    
}
    
    
    
}