Diff: main.cpp

Revision:

1:7d7dabfcd7ae

Parent:

0:9df42b937ac1

Child:

2:ee9eb5f76979

--- a/main.cpp	Tue Aug 28 01:25:47 2018 +0000
+++ b/main.cpp	Mon Oct 08 23:25:59 2018 +0000
@@ -1,45 +1,460 @@
 #include "mbed.h"
+#include "main.h"
+
+int Conf_Servos();
+int ConfTransmi();
+int RutColor();
+void MoverMotor (uint8_t motor, uint8_t grados);
+void MoverConjunto(uint8_t motores, uint8_t posicion);
+
+DigitalIn entrada (PE_15);
+DigitalOut S3 (PB_0);
+DigitalOut S2 (PE_0);
+
+PwmOut Servo1(PB_11);
+PwmOut Servo2(PB_10);
+PwmOut Servo3(PA_0);
+PwmOut Servo4(PE_14);
+PwmOut Servo5(PE_12);
+PwmOut Servo6(PE_10);
+PwmOut Servo7(PD_12);
+PwmOut Servo8(PD_13);
+
+
+Serial Transmi(USBTX, USBRX);
+Timer tiempo;
 
-Serial MESSE(USBTX, USBRX);
-int nume1=MESSE.getc();
-int nume2=MESSE.getc();
-int ope=MESSE.getc();
-int resultado=0;
+int inicio;
+char funcion;
+uint8_t motor_es;
+uint8_t grados_posicion;
+int cierre;
+float AnchoPulso;
+int valor;
+int color=0;
+int rojo=0;
+int azul=0;
+int verde=0;
+int Detectar();
+int tme=0;
+
+
+
+int main() {
+
+while (1){
+    
+Conf_Servos();
+ConfTransmi();
+
+Transmi.printf("\n Inicio. \n");
 
-int main(){
+inicio=Transmi.getc();
+if (inicio==0xFF)
+     {
+      funcion=Transmi.getc();
+      motor_es=Transmi.getc();
+      grados_posicion=Transmi.getc();
+      cierre=Transmi.getc();
+      
+      if (cierre==0xFD){
+
+switch (funcion){
+case 0x01:{
+
+MoverMotor(motor_es,grados_posicion);
+break;
+}
+case 0x02:{
+MoverConjunto(motor_es,grados_posicion);
+break;
+}
+case 0x03:{
+while (funcion==0x03){
+    RutColor();
+    }    
+    }
     
-    MESSE.printf("Ingresa el primer valor a operar""\n");
-    nume1=MESSE.getc();
-    MESSE.printf("Ingrese el segundo número a operar""\n");
-    nume2=MESSE.getc();
-    MESSE.printf("Ingrese 1 para sumar""\n""2 para restar""\n""3 para multiplicar""\n""4 para dividir""\n");
-    ope=MESSE.getc();
+default:{
+Transmi.printf("Funcion no definida. \n");
+        }
+    }
+}
+
+else {
+Transmi.printf("Error de codigo de cierre \n");
+        }
+     }
+     
+else {
+Transmi.printf("Error de codigo de inicio. \n");
+    }
+}
+
+}
+
+
+void MoverMotor(uint8_t motor, uint8_t grados){
+
+Transmi.printf("Se movera solo un motor. \n");
+if (grados <= 180){
+
+AnchoPulso=((grados*9.8)+535);
+
+
+switch (motor){
+
+case 0x01:{
+Servo1.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 1.\n");
+break;
+
+}
+
+case 0x02:{
+Servo2.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 2.\n");
+break;
+}
+
+case 0x03:{
+Servo3.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 3.\n");
+break;
+}
+
+case 0x04:{
+Servo4.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 4.\n");
+break;
+}
+
+case 0x05:{
+Servo5.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 5.\n");
+break;
+}
+case 0x06:{
+Servo6.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 6.\n");
+break;
+}
+
+case 0x07:{
+Servo7.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 7.\n");
+break;
+}
+
+case 0x08:{
+Servo8.pulsewidth_us(AnchoPulso);
+Transmi.printf("Se mueve el motor 8.\n");
+break;
+}
+
+default:{
+Transmi.printf("Motor no cargado. \n");
+    }
+  }
+
+}
+
+else {
+Transmi.printf("Valor de grados fuera de rango. \n");
+    }
+
+}
+
+
+void MoverConjunto(uint8_t motores,uint8_t posicion){
+Transmi.printf("Se movera un conjunto de motores. \n");
+
+if (posicion==0xDD){
     
-    switch (ope){
+    switch(motores){
+        case 1:{
+            Servo1.pulsewidth_us(550);
+            Servo2.pulsewidth_us(550);
+            Transmi.printf("Mover primer conjunto, ambos cerrados. \n");
+            break;
+            }
+            
+        case 2:{
+            Servo3.pulsewidth_us(550);
+            Servo4.pulsewidth_us(550);
+            Transmi.printf("Mover segundo conjunto, ambos cerrados. \n");
+            break;
+            }
         
+        case 3:{
+            Servo5.pulsewidth_us(550);
+            Servo6.pulsewidth_us(550);
+            Transmi.printf("Mover tercer conjunto, ambos cerrados. \n");
+            break;
+            }
         
-        case 1:
-        resultado=nume1+nume2;
-        MESSE.printf("EL resultado es %resultado");
-        break;
+        case 4:{
+            Servo7.pulsewidth_us(550);
+            Servo8.pulsewidth_us(550);
+            Transmi.printf("Mover cuarto conjunto, ambos cerrados. \n");
+            break;
+            }
+        
+        default: {
+            Transmi.printf("Motores no definidos. \n");
+            }
+         }
+     }
+     
+     else{
+     if (posicion==0xDA){
+    
+    switch(motores){
+        case 1:{
+            Servo1.pulsewidth_us(2100);
+            Servo2.pulsewidth_us(550);
+            Transmi.printf("Mover primer conjunto, uno abierto, dos cerrado. \n");
+            break;
+            }
+            
+        case 2:{
+            Servo3.pulsewidth_us(2100);
+            Servo4.pulsewidth_us(550);
+            Transmi.printf("Mover segundo conjunto, uno abierto, dos cerrado. \n");
+            break;
+            }
+        
+        case 3:{
+            Servo5.pulsewidth_us(2100);
+            Servo6.pulsewidth_us(550);
+            Transmi.printf("Mover tercer conjunto, uno abierto, dos cerrado. \n");
+            break;
+            }
         
-        case 2:
-        resultado=nume1-nume2;
-        MESSE.printf("EL resultado es %resultado");
-        break;
+        case 4:{
+            Servo7.pulsewidth_us(2100);
+            Servo8.pulsewidth_us(550);
+            Transmi.printf("Mover cuarto conjunto, uno abierto, dos cerrado. \n");
+            break;
+            }
+        
+        default: {
+            Transmi.printf("Motores no definidos. \n");
+            }
+         }
+     }
+     
+     
+     else{
+     
+     if (posicion==0x1D){
+    
+    switch(motores){
+        case 1:{
+            Servo1.pulsewidth_us(550);
+            Servo2.pulsewidth_us(2100);
+            Transmi.printf("Mover primer conjunto, uno cerrado, dos abierto. \n");
+            break;
+            }
+            
+        case 2:{
+            Servo3.pulsewidth_us(550);
+            Servo4.pulsewidth_us(2100);
+            Transmi.printf("Mover segundo conjunto, uno cerrado, dos abierto. \n");
+            break;
+            }
+        
+        case 3:{
+            Servo5.pulsewidth_us(550);
+            Servo6.pulsewidth_us(2100);
+            Transmi.printf("Mover tercer conjunto, uno cerrado, dos abierto. \n");
+            break;
+            }
+        
+        case 4:{
+            Servo7.pulsewidth_us(550);
+            Servo8.pulsewidth_us(2100);
+            Transmi.printf("Mover cuarto conjunto, uno cerrado, dos abierto. \n");
+            break;
+            }
         
-        case 3:
-        resultado=nume1*nume2;
-        MESSE.printf("EL resultado es ", resultado);
-        break;
+        default: {
+            Transmi.printf("Motores no definidos. \n");
+            }
+         }
+     }
+     else{
+     if (posicion==0x1A){
+    
+    switch(motores){
+        case 1:{
+            Servo1.pulsewidth_us(2100);
+            Servo2.pulsewidth_us(2100);
+            Transmi.printf("Mover primer conjunto, ambos abiertos. \n");
+            break;
+            }
+            
+        case 2:{
+            Servo3.pulsewidth_us(2100);
+            Servo4.pulsewidth_us(2100);
+            Transmi.printf("Mover segundo conjunto, ambos abiertos. \n");
+            break;
+            }
+        
+        case 3:{
+            Servo5.pulsewidth_us(2100);
+            Servo6.pulsewidth_us(2100);
+            Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
+            break;
+            }
+        
+        case 4:{
+            Servo7.pulsewidth_us(2100);
+            Servo8.pulsewidth_us(2100);
+            Transmi.printf("Mover tercer conjunto, ambos abiertos. \n");
+            break;
+            }
         
-        case 4:
-        resultado=nume1/nume2;
-        MESSE.printf("EL resultado es %resultado");
-        break;
-        
-        default:
-        MESSE.printf("Inválido");
+        default: {
+            Transmi.printf("Motores no definidos. \n");
+            }
+         }
+     }
+     
+     else{
+         Transmi.printf("Movimiento no definido. \n");
+         
+         }
         }
+      }
+   }
+     
+     
+}
+     
+
+
+int Conf_Servos(){
+    Servo1.period_ms(20);
+    Servo2.period_ms(20);
+    Servo3.period_ms(20);
+    Servo4.period_ms(20);
+    Servo5.period_ms(20);
+    Servo6.period_ms(20); 
+    Servo7.period_ms(20);
+    Servo8.period_ms(20);
+    return (0);
+    }
+    
+    
+int ConfTransmi(){
+    Transmi.baud(9600);
+    return(0);
+    }
+
+int RutColor(){
+    
+                S2=0;
+                S3=0;
+                rojo=Detectar();
+                              
+                S2=0;
+                S3=1;
+                azul=Detectar();
+               
+                S2=1;
+                S3=1;
+                verde=Detectar();
+                
+                if (rojo<azul & azul<verde){
+                    color=rojo;
+                    RutRojo();
+                    }
+                    
+                    else{
+                        if (azul<rojo & rojo<verde){
+                          color=azul;  
+                           RutAzul(); 
+                           }
+                           
+                            else{
+                                RutVerde();
+                                color=verde;
+                         }
+                    }
+                }       
+
+int Detectar(){
+    tme=0;
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    tiempo.start();
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    tiempo.stop();
+    tme=tiempo.read_us();
+    tiempo.reset();
+    return(tme);
+    
     
     }
+    
+    
+int RutRojo(){
+    MoverConjunto(1,0x1A);
+    MoverConjunto(3,0x1A);
+    wait (2);
+    
+    MoverConjunto(2,0xDD);
+    MoverConjunto(4,0xDD);
+    wait (2);
+    
+    MoverConjunto(1,0xDD);
+    MoverConjunto(3,0xDD);
+    wait (2);
+    
+    MoverConjunto(2,0x1A);
+    MoverConjunto(4,0x1A);
+    wait (2);
+    }
+    
+int RutAzul(){
+    
+    MoverConjunto(1,0x1A);
+    MoverConjunto(3,0x1A);
+    wait (2);
+    
+    MoverConjunto(2,0x1A);
+    MoverConjunto(4,0x1A);
+    wait (2);
+    
+    MoverConjunto(1,0xDD);
+    MoverConjunto(3,0xDD);
+    wait (2);
+    
+    MoverConjunto(2,0xDD);
+    MoverConjunto(4,0xDD);
+    wait (2);
+    }
+    
+int RutVerde(){
+    MoverConjunto(1,0x1A);
+    MoverConjunto(3,0xDD);
+    wait (2);
+    
+    MoverConjunto(2,0xDD);
+    MoverConjunto(4,0x1A);
+    wait (2);
+    
+    MoverConjunto(1,0x1A);
+    MoverConjunto(3,0xDD);
+    wait (2);
+    
+    MoverConjunto(2,0xDD);
+    MoverConjunto(4,0x1A);
+    wait (2);
+    }
\ No newline at end of file