File content as of revision 0:448502aece2e:

#include "mbed.h"
#include "TextLCD.h"
#include "TCS3200.h"
#include "Servo.h"

DigitalIn   F1(PC_8);
DigitalIn   F2(PC_6);
DigitalIn   F3(PC_5);
DigitalIn   K(D6);
DigitalOut M1(D2);
DigitalOut M2(D3);
DigitalOut I(D4);
Servo Servo1(D5);
TextLCD lcd(D13,D12,D11,D10,D9,D8,D7);
TCS3200 sensor(PB_2, PB_1,PB_15,PB_14, PB_13);
Serial pc(USBTX, USBRX);


void inicio(){Servo1.position(90);}

int lectura(){
  int v=3;
     
  return v;}
  
void Pos1(){
    if(K==0){pc.printf("E");inicio();}
    else{
    Servo1.position(92);
    M1=1; 
    wait(1);
    M1=0;
    I=0;
    M2=1;
    wait(1);
    M2=0;
    inicio();}}

void Pos2(){
    if(K==0){pc.printf("E");inicio();}
    else{
    Servo1.position(94);
    M1=1; 
    wait(1);
    M1=0;
    I=0;
    M2=1;
    wait(1);
    M2=0; 
    inicio();}}
    
void Pos3(){
    if(K==0){pc.printf("E");inicio();}
    else{
    Servo1.position(96);
    M1=1; 
    wait(1);
    M1=0;
    I=0;
    M2=1;
    wait(1);
    M2=0; 
    inicio();}}
    
void Pos4(){
    if(K==0){pc.printf("E");inicio();}
    else{
    Servo1.position(98);
    M1=1; 
    wait(1);
    M1=0;
    I=0;
    M2=1;
    wait(1);
    M2=0;
    inicio();}}
    
void Pos5(){
    if(K==0){pc.printf("E");inicio();}
    else{
    Servo1.position(100);
    M1=1; 
    wait(1);
    M1=0;
    I=0;
    M2=1;
    wait(1);
    M2=0;
    inicio();}}

void recoger(){
    inicio();
    M1=1; 
    wait(1);
    M1=0;
    I=1;
    M2=1;
    wait(1);
    M2=0;}
    
int main()
{pc.baud(9600);
int a=0,b=0,c=0,d=0,e=0;
inicio();
 while (true){
  int o=lectura();
  
  lcd.gotoxy(1,1);
  lcd.printf("P1:%d P2:%d P3:%d",a,b,c);
  lcd.gotoxy(1,2);
  lcd.printf("P4:%d P5:%d",d,e);
  
  pc.printf("T%d,%d,%d,%d,%dT",a,b,c,d,e);
  
  wait_ms(10);
  if(o==1){
      lcd.gotoxy(1,1);
      lcd.printf("Azul            ");
      lcd.gotoxy(1,2);
      lcd.printf("                ");
      pc.printf("A");
      recoger();
      Pos1();
      a++;}
      
  if(o==2){
      lcd.gotoxy(1,1);
      lcd.printf("Azul            ");
      lcd.gotoxy(1,2);
      lcd.printf("                ");
      pc.printf("B");
      recoger();
      Pos2();
      b++;}
      
  if(o==3){
      lcd.gotoxy(1,1);
      lcd.printf("Azul            ");
      lcd.gotoxy(1,2);
      lcd.printf("                ");
      pc.printf("C");
      recoger();
      Pos3();
      c++;}
      
  if(o==4){
      lcd.gotoxy(1,1);
      lcd.printf("Azul            ");
      lcd.gotoxy(1,2);
      lcd.printf("                ");
      pc.printf("D");
      recoger();
      Pos4();
      d++;}
      
  if(o==5){
      lcd.gotoxy(1,1);
      lcd.printf("Azul            ");
      lcd.gotoxy(1,2);
      lcd.printf("                ");
      pc.printf("F");
      recoger();
      Pos5();
      e++;}
   
     
    
}}