Diff: main.cpp

Revision:

0:448502aece2e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 15 17:08:00 2018 +0000
@@ -0,0 +1,168 @@
+#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++;}
+   
+     
+    
+}}