File content as of revision 3:8bc95fb7c59c:

/*Sistemas Embebidos
 Presentado a:
  Ingeniero Ferney Beltran 
 presentado por:
 John Vanegas 
 Josimar Hernandez
 Sebastian Aldana
 Sergio Fonseca
*/

#include "mbed.h"
#include "main.h"


int Conf_Servos();
int ConfTransmi();
void RutColor();
void Joy();
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);
AnalogIn analog_1(A0);
DigitalOut led(LED1);

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;

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");

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:{
  
    RutColor();
break;
    } 

case 0x04:{
    Joy();
    }

        

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(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 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 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;
            }
        
        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;
            }
        
        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);
    }

void  RutColor(){
    
                S2=0;
                S3=0;
                rojo=Detectar();
                              
                S2=0;
                S3=1;
                azul=Detectar();
               
                S2=1;
                S3=1;
                verde=Detectar();
                
                if ((rojo>300)&(rojo<460) & (azul>100)&(azul<160) & (verde>200)&(verde<310)){
                    
                    Transmi.printf("AZUL \n");
            Servo1.pulsewidth_us(1025);
            wait (0.5);
            Servo2.pulsewidth_us(1319);
            wait (0.5);     
            Servo1.pulsewidth_us(1412);
            wait (0.5);     
            Servo7.pulsewidth_us(1025);
            wait (0.5);
            Servo8.pulsewidth_us(1809);
            wait (0.5);
            Servo7.pulsewidth_us(1417);
            wait (0.5);
            Servo2.pulsewidth_us(1515);
            Servo8.pulsewidth_us(1515);
            wait (0.5);
            Servo3.pulsewidth_us(1221);
            wait (0.5);
            Servo4.pulsewidth_us(1613);
            wait (0.5);
            Servo3.pulsewidth_us(1613);
            wait (0.5);
            Servo5.pulsewidth_us(731);
            wait (0.5);
            Servo6.pulsewidth_us(1025);
            wait (0.5);
            Servo5.pulsewidth_us(1025);
            wait (0.5);
            Servo4.pulsewidth_us(1417);
            Servo6.pulsewidth_us(1417);
            wait (0.5);
                    }
                    
                    else{
                        if ((rojo>70)&(rojo<80) & (azul>170)&(azul<220) & (verde>250)&(verde<300)){
                            
                            Transmi.printf("ROJO\n");
                                  Servo1.pulsewidth_us(1025);
            wait (0.5);
            Servo2.pulsewidth_us(1711);
            wait (0.5);     
            Servo1.pulsewidth_us(1412);
            wait (0.5);     
            Servo7.pulsewidth_us(1025);
            wait (0.5);
            Servo8.pulsewidth_us(1319);
            wait (0.5);
            Servo7.pulsewidth_us(1417);
            wait (0.5);
            Servo2.pulsewidth_us(1515);
            Servo8.pulsewidth_us(1515);
            wait (0.5);
            Servo3.pulsewidth_us(1221);
            wait (0.5);
            Servo4.pulsewidth_us(1025);
            wait (0.5);
            Servo3.pulsewidth_us(1613);
            wait (0.5);
            Servo5.pulsewidth_us(731);
            wait (0.5);
            Servo6.pulsewidth_us(1613);
            wait (0.5);
            Servo5.pulsewidth_us(1025);
            wait (0.5);
            Servo4.pulsewidth_us(1417);
            Servo6.pulsewidth_us(1417);
            wait (0.5); 
                            }
                           
                            else{
                                
                                if ((rojo>90)&(rojo<120) & (azul>121)&(azul<170) & (verde>100)&(verde<110)){
                            
                           Transmi.printf("VERDE\n");
                                Servo1.pulsewidth_us(700);
                                wait (1);
                                Servo3.pulsewidth_us(700);
                                wait (1);
                                Servo5.pulsewidth_us(700);
                                wait (1);
                                Servo7.pulsewidth_us(700);
                                wait (1);
                                Servo3.pulsewidth_us(700);
                                wait (1);
                                Servo1.pulsewidth_us(1500);
                                wait (1);
                                Servo3.pulsewidth_us(1500);
                                wait (1);
                                Servo5.pulsewidth_us(1500);
                                wait (1);
                                Servo7.pulsewidth_us(1500);
                                wait (1);
                                  
                                
                            }
                            else{
                            Transmi.printf("color no valido. \n");}
                         }
                    }
                }       

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);
    
}

void Joy(){
    
    
    
    
    float medir_r;
    float medir_v;
  
    
    printf("MANDO\n");

    while(funcion==4) {

        medir_r = analog_1.read();

        medir_v = medir_r * 3300; 

        if (medir_v < 1000) {             
              
            Servo1.pulsewidth_us(1025);
            Servo2.pulsewidth_us(1319);
            wait (0.3);     
            Servo1.pulsewidth_us(1412);
            wait (0.3);
                 
            Servo7.pulsewidth_us(1025);
            Servo8.pulsewidth_us(1809);
            wait (0.3);
            Servo7.pulsewidth_us(1417);
            wait (0.3);
            
            Servo2.pulsewidth_us(1515);
            Servo8.pulsewidth_us(1515);
            wait (0.3);
            Servo3.pulsewidth_us(1221);
            Servo4.pulsewidth_us(1613);
            wait (0.3);
            Servo3.pulsewidth_us(1613);
            wait (0.3);
            
            Servo5.pulsewidth_us(731);
            Servo6.pulsewidth_us(1025);
            wait (0.3);
            Servo5.pulsewidth_us(1025);
            wait (0.3);
            
            Servo4.pulsewidth_us(1417);
            Servo6.pulsewidth_us(1417);
            wait (0.3);
                 
        } else {
            if (medir_v > 2000){
            wait (0.3);
            Servo2.pulsewidth_us(1711);
            wait (0.3);     
            Servo1.pulsewidth_us(1412);
            wait (0.3);     
            Servo7.pulsewidth_us(1025);
            wait (0.3);
            Servo8.pulsewidth_us(1319);
            wait (0.3);
            Servo7.pulsewidth_us(1417);
            wait (0.3);
            Servo2.pulsewidth_us(1515);
            Servo8.pulsewidth_us(1515);
            wait (0.3);
            Servo3.pulsewidth_us(1221);
            wait (0.3);
            Servo4.pulsewidth_us(1025);
            wait (0.3);
            Servo3.pulsewidth_us(1613);
            wait (0.3);
            Servo5.pulsewidth_us(731);
            wait (0.3);
            Servo6.pulsewidth_us(1613);
            wait (0.3);
            Servo5.pulsewidth_us(1025);
            wait (0.3);
            Servo4.pulsewidth_us(1417);
            Servo6.pulsewidth_us(1417);
            wait (0.3); 
                
        }
        else{    
        wait (1.0);}
        }
        }
        }