File content as of revision 1:29fc95839893:

#include "mbed.h"
#include "ctype.h"
#include "string.h"
#include "esp_at.h"
#include "esp_socket.h"
#include "hcsr04.h"

//config conexion wifi
#define nombreWifi "RAJAJ"
#define contraWifi "ANCM2211"
#define enlaceHTLM "luckystrike051.pythonanywhere.com"
#define puertoHTLM 80

//estados del bot
#define STANDBY_COCINA 4
#define TRANSPORT_IDA 5
#define STANDBY_MESA 2
#define TRANSPORT_VUELTA 3
#define CLEANUP 6

//estados motor
#define STANDBY 1
#define FOWARD 2
#define TURN_RIGHT 3
#define TURN_LEFT 4
#define HALF_TURN 5

// distancia a la que el utrasonico detiene el bot
#define DISTANCIA_STOP 11

//estados del modulo wifi
enum{
    PROC_POST_OPEN_STATE,
    PROC_POST_OPEN_REQUEST,
    PROC_POST_OPEN_HCSR,
    PROC_POST_SEND_STATE,
    PROC_POST_SEND_REQUEST,
    PROC_POST_SEND_HCSR,
    PROC_POST_RECV_STATE,
    PROC_POST_RECV_REQUEST,
    PROC_POST_RECV_HCSR,
    PROC_POST_CLOSE_STATE,
    PROC_POST_CLOSE_REQUEST,
    PROC_POST_CLOSE_HCSR,
};

#define ON 1
#define OFF 0

Serial DBG(USBTX, USBRX); // tx, rx

//temperatura
AnalogIn   TEMPIN(A0);

//pines para fines varios
DigitalOut RED(PTE2);
DigitalOut GREEN(PTE3);
DigitalOut BLUE(PTE4);

//pines motor
DigitalOut ENAB_1 (PTC9);
DigitalOut ENAB_2 (PTC8);
DigitalOut DER_1 (PTA5);
DigitalOut IZQ_1 (PTA4);
DigitalOut DER_2 (PTA12);
DigitalOut IZQ_2 (PTD4);

DigitalOut S0(PTC17);
DigitalOut S1(PTC16);
DigitalOut S2(PTC13);
DigitalOut S3(PTC12);
InterruptIn INPUT(PTD0);
DigitalOut OE(PTD5);

DigitalIn INFRA_DER(PTE21);
DigitalIn INFRA_IZQ(PTE20);

DigitalIn PULSADOR(PTC4);

Timer cont;
Ticker seg;
Ticker seg_m;
Ticker seg_send;

#define SYSTEM_TICKER_TIC_DEF   (0.01) //10mseg    
#define SYSTEM_TICKER_1_SEG     ( 1 / SYSTEM_TICKER_TIC_DEF)
Ticker system_ticker;

HCSR04 sensor(PTA17, PTA16);

int util, distancia;

unsigned int segundos = 0, segundos_m = 0, segundos_send = 0;
int j = 0, mem = 0;
unsigned short proc_post_tic = 0;

char estado_ASPI = STANDBY_COCINA;
char est_motor = STANDBY;
char est_ultra = OFF; 

bool IsFoodReady = false;
bool msg_post = false;
bool IsOnLine = true;
bool IsHCSRFree = true;
bool IsHCSRWork = true;

void proc_post (void);
void proc_post_tic_cb ();
void conta();
void conta_m();
void conta_send();
void Ascend();
void Desend();
void ME_ASPI();
void MOTORR();
void INFRAROJOS();
void ULTRASONICO();
void TEMPERATURA();

int colores[4][2] = {{207, 290}, //IDA       3   0
                    {300, 390},//VUELTA      5   1
                    {400, 500},//TERMINAL    7   2
                    {600, 660}//COCINA       12  3
                    }; // 0-ida  1-vuelta 2-terminal 4-cocina

unsigned char string_est_aspi[155];
unsigned char buffer_data_http[255];
unsigned char buffer_memo[150];
char mes_send[50];

int main() {    
    RED = OFF;
    GREEN = OFF;
    BLUE = OFF; 
    
    seg.attach(&conta, 1.0);
    seg_m.attach(&conta_m, 1.0);
    seg_send.attach(&conta_send, 1.0);
    //seg_us.attach(&conta_us, 1.0);
    system_ticker.attach(&proc_post_tic_cb, SYSTEM_TICKER_TIC_DEF);
    
    (void)WiFi_init(nombreWifi, contraWifi);
    
    OE = 1;
        
    //Escala a 2%
    S0 = 0;                     
    S1 = 1;       
    
    //Filtro
    S2 = 0;
    S3 = 1;
    
    INPUT.rise(&Ascend);
    INPUT.fall(&Desend);
    
    msg_post = true;
    while(1) {
    
    //DBG.printf("%d_", segundos_stop);
    
    (void)WiFi_Step();
       
    proc_post();
    
    if(IsOnLine){
        INFRAROJOS();
    }
    
    ME_ASPI();
    
    ULTRASONICO();
    
    MOTORR();
    
    TEMPERATURA();
    
    wait(0.5);
    
    //DBG.printf("%d - %d - %d \r\n", distancia, segundos_stop, (int)IsHCSRFree);
    
    }
}

void ME_ASPI(){
    
    switch(estado_ASPI){
        case STANDBY_COCINA:
            j = 0;
            sprintf((char*)string_est_aspi,"ESPERANDO EN COCINA");    

            if(IsFoodReady){
                estado_ASPI = TRANSPORT_IDA; 
            }
        break;
        case TRANSPORT_IDA:
            OE = 0;
            
            ULTRASONICO();
            
            sprintf((char*)string_est_aspi,"LLEVANDO PEDIDO A MESA");

            if( util >= colores[0][0] && util <= colores[0][1]){
                
                if(segundos == 0 && j < strlen((char*)buffer_memo)){ 
                    
                    if(buffer_memo[j] == '0'){
                        
                        if(buffer_memo[j+1] == 'V'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 4;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                        if(buffer_memo[j+1] == 'I'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 0;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                        if(buffer_memo[j+1] == 'L'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 6;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                        
                    }
                    if(buffer_memo[j] == '1'){
                        
                        if(buffer_memo[j+1] == 'V'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 4;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                        if(buffer_memo[j+1] == 'I'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 0;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                        if(buffer_memo[j+1] == 'L'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 6;
                            segundos = 10;
                            j++;j++;
                            break;
                        }
                    
                    }
                }
            
            }
            if( util >= colores[2][0] && util <= colores[2][1]){
                estado_ASPI = STANDBY_MESA;
            }   
        break;
        case STANDBY_MESA:
            OE = 1;
            
            sprintf((char*)string_est_aspi,"ESPERANDO EN MESA");
            if(PULSADOR == 1){
                est_motor = HALF_TURN;
                segundos_m = 10;
            } 
            if(segundos_m == 0 && est_motor == HALF_TURN){
                estado_ASPI = TRANSPORT_VUELTA;
            }
        break;
        case TRANSPORT_VUELTA:
            OE = 0;
            
            sprintf((char*)string_est_aspi,"VOLVIENDO A COCINA");

            if( util >= colores[1][0] && util <= colores[1][1]){
                
                if(segundos == 0 && j >= 0){ 
                    if(buffer_memo[j-2] == '0'){
                           
                        if(buffer_memo[j-1] == 'V'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 4;
                            segundos = 5;
                            j--;j--;
                            break;    
                        }
                        if(buffer_memo[j-1] == 'I'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 0;
                            segundos = 5;
                            j--;j--;
                            break;  
                        }
                        if(buffer_memo[j-1] == 'L'){
                            
                            est_motor = TURN_LEFT;
                            segundos_m = 6;
                            segundos = 5;
                            j--;j--;
                            break;   
                        }
                        
                    }
                    if(buffer_memo[j-2] == '1'){
                         
                        if(buffer_memo[j-1] == 'V'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 4;
                            segundos = 10;
                            j--;j--; 
                            break; 
                        }
                        if(buffer_memo[j-1] == 'I'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 0;
                            segundos = 10;
                            j--;j--;
                            break;  
                        }
                        if(buffer_memo[j-1] == 'L'){
                            
                            est_motor = TURN_RIGHT;
                            segundos_m = 6;
                            segundos = 10;
                            j--;j--;
                            break;   
                        }
                        
                    }
                }
            
            }
        if( util >= colores[3][0] && util <= colores[3][1]){
            est_motor = HALF_TURN;
            segundos_m = 10;
        } 
        if(segundos_m == 0 && est_motor == HALF_TURN){
            estado_ASPI = CLEANUP;
        }
        break;
        case CLEANUP:
            OE = 1;
            
            int a = sizeof((char*)buffer_data_http);
            int i = 0;
            
            for(i = 0; i < a; i++){
                buffer_data_http[i] = 0;
                buffer_memo[i] = 0;
                
                /* limpiar buffer*/
            }
            if(i == a){
                 IsFoodReady = false;  
            }
            if(IsFoodReady == false){
                estado_ASPI = STANDBY_COCINA;
            }
        break;
        
    }
}

void INFRAROJOS(){
    
    if(estado_ASPI == TRANSPORT_IDA || estado_ASPI == TRANSPORT_VUELTA){
        if(INFRA_DER == 0 && INFRA_IZQ != 0){
            
            est_motor = TURN_LEFT;
            segundos_m = 2;
        }
        if(INFRA_DER != 0 && INFRA_IZQ == 0){
            
            est_motor = TURN_RIGHT;
            segundos_m = 2;
        }
        if(INFRA_DER == 0 && INFRA_IZQ == 0){
            
            est_motor = FOWARD;
        }
        if(INFRA_DER != 0 && INFRA_IZQ != 0){
            
            est_motor = STANDBY;
        }
    }
}

void TEMPERATURA(){
    if(TEMPIN >= 0.65f){
        RED = OFF;
        GREEN = OFF;
        BLUE = ON;
    } 
    if(TEMPIN < 0.65f && TEMPIN >= 0.55f){
        RED = OFF;
        GREEN = ON;
        BLUE = OFF;  
    }  
    if(TEMPIN < 0.55f && TEMPIN >= 0.25f){
        RED = OFF;
        GREEN = ON;
        BLUE = ON; 
    }
    if(TEMPIN < 0.25f && TEMPIN >= 0.15f){
        RED = ON;
        GREEN = OFF;
        BLUE = OFF;
    }
    if(TEMPIN < 0.15f){
        RED = ON;
        GREEN = OFF;
        BLUE = ON;
    }
}


void MOTORR(){
    if(IsHCSRFree){
    
    switch(est_motor){
        default:
        case STANDBY:
        ENAB_1 = 0;
        ENAB_2 = 0;
        DER_1 = 0;
        IZQ_1 = 0;
        DER_2 = 0;
        IZQ_2 = 0;
        break;
        case FOWARD:
        
        ENAB_1 = 1;
        ENAB_2 = 1;
        DER_1 = 1;
        IZQ_1 = 0;
        DER_2 = 0;
        IZQ_2 = 1;
        break;
        case TURN_RIGHT:
            
            if(segundos_m > 0){
            IsOnLine = false;
            ENAB_1 = 1;
            ENAB_2 = 1;
            DER_1 = 1;
            IZQ_1 = 0;
            DER_2 = 0;
            IZQ_2 = 0;
            }else{
                IsOnLine = true;
            }
        break;
        case TURN_LEFT:
            
            if(segundos_m > 0){
            IsOnLine = false;
            ENAB_1 = 1;
            ENAB_2 = 1;
            DER_1 = 0;
            IZQ_1 = 0;
            DER_2 = 0;
            IZQ_2 = 1;
            }else{
                IsOnLine = true;
            }
        break;
        case HALF_TURN:
            ;
            if(segundos_m > 0){
            IsOnLine = false;
            ENAB_1 = 1;
            ENAB_2 = 1;
            DER_1 = 1;
            IZQ_1 = 0;
            DER_2 = 1;
            IZQ_2 = 0;
            }else{
                IsOnLine = true;
            }
        break;
        }
        
    }else{
        ENAB_1 = 0;
        ENAB_2 = 0;
        DER_1 = 0;
        IZQ_1 = 0;
        DER_2 = 0;
        IZQ_2 = 0;
    }
}

void ULTRASONICO(){
    if(estado_ASPI == TRANSPORT_IDA || estado_ASPI == TRANSPORT_VUELTA){
    
    distancia = sensor.distance();
    if(distancia > DISTANCIA_STOP){
        IsHCSRFree = true;
        est_ultra = OFF;
        
    }else{
        IsHCSRFree = false;
        est_ultra = ON;
        
    }
    }else{
        IsHCSRFree = true;  
    }
    
}


void proc_post (void){
    int res ;
    static unsigned char proc_post_step = PROC_POST_OPEN_STATE;
    
    if( proc_post_tic )
        return;
        
    proc_post_tic = SYSTEM_TICKER_1_SEG;
    
    if(!has_ip){ 
        proc_post_step = PROC_POST_OPEN_STATE;
        return;
    }
    
    char buff_send[255];
    switch(proc_post_step){
        default:
        case PROC_POST_OPEN_STATE:
            if( sock_connect ( SOCKET_TCP, enlaceHTLM, puertoHTLM, 10000) == 0 ){
                proc_post_tic = 1 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_SEND_STATE;          
            }else{
                proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
                proc_post_step = PROC_POST_OPEN_STATE;
                sock_close();
            }
            break;
            
        case PROC_POST_SEND_STATE: 
            
            sprintf(buff_send, "GET /estadorobotactual?estadorobot=%s HTTP/1.1\r\nHost: luckystrike051.pythonanywhere.com\r\nConnection: Close\r\n\r\n", string_est_aspi);
        
            res = sock_send ( (unsigned char *)buff_send,
                                strlen(buff_send), 25000);
                                
            DBG.printf("proc_post: send: (%d)%s\r\n", res, buff_send);
            if( res > 0 ){
                proc_post_tic = SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_RECV_STATE;
                DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV\r\n", __LINE__ );
            }else{
                proc_post_tic = 2 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_CLOSE_STATE;
                DBG.printf("-proc_posrt: -> [%d] PROC_POST_CLOSE\r\n", __LINE__ );            
            }
            break;  
            
        case PROC_POST_RECV_STATE:
            res = sock_recv ( buffer_data_http, 250, 500 );
            
            if(estado_ASPI != STANDBY_COCINA || estado_ASPI != TRANSPORT_VUELTA){
                segundos_send = 40;   
            }else{
                segundos_send = 0;
            }
            
            DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV:%d\r\n", __LINE__, res );   
            
            if( res > 0 ){
                if (strstr ((char*)buffer_data_http, "200 OK")){
                    DBG.printf("proc_posrt: -> [%d] 200 OK\r\n", __LINE__ );
                }
            }
            proc_post_tic = SYSTEM_TICKER_1_SEG; 
            proc_post_step = PROC_POST_CLOSE_STATE;
            DBG.printf("proc_posrt: -> [%d] PROC_POST_CLOSE_STATE\r\n", __LINE__ );   
            break;
        
        case PROC_POST_CLOSE_STATE:
            
            for(int i = 0; i < 255; i++){
                buffer_data_http[i] = 0;
                buff_send[i] = 0;
                
            }
            
            
            proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
            proc_post_step = PROC_POST_OPEN_REQUEST;
            
            break;
            
        case PROC_POST_OPEN_REQUEST:
            if(estado_ASPI == STANDBY_COCINA || estado_ASPI == TRANSPORT_VUELTA && segundos == 0){
            if( sock_connect ( SOCKET_TCP, enlaceHTLM, puertoHTLM, 10000) == 0 ){
                proc_post_tic = 1 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_SEND_REQUEST;          
            }else{
                
                proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
                proc_post_step = PROC_POST_OPEN_REQUEST;
                sock_close();
            }
            }else{
                proc_post_step = PROC_POST_OPEN_HCSR;
            }
            break;
            
        case PROC_POST_SEND_REQUEST:
               
                sprintf(buff_send, "GET /showubicacionrobotactual HTTP/1.1\r\nHost: luckystrike051.pythonanywhere.com\r\nConnection: Close\r\n\r\n");   
            
            res = sock_send ( (unsigned char *)buff_send,
                                strlen(buff_send), 25000);
            DBG.printf("proc_post: send: (%d)%s\r\n", res, buff_send);
            if( res > 0 ){
                proc_post_tic = SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_RECV_REQUEST;
                DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV_REQUEST\r\n", __LINE__ );
            }else{
                proc_post_tic = 2 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_CLOSE_REQUEST;
                DBG.printf("-proc_posrt: -> [%d] PROC_POST_CLOSE_REQUEST\r\n", __LINE__ );            
            }
            break; 
        
        case PROC_POST_RECV_REQUEST:
            res = sock_recv ( buffer_data_http, 250, 500 );
            DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV:%d\r\n", __LINE__, res );
            
            for(int i = 0; i < strlen((char*)buffer_data_http); i++){
                
                if(buffer_data_http[i] == 'E'){
                    
                    for(int a = i; a < strlen((char*)buffer_data_http); a++){
                        buffer_memo[a-i] = buffer_data_http[a+1];
                        
                        if(buffer_data_http[a] == 'T'){
                            IsFoodReady = true;
                            i = a;
                            break;
                        }
                    }
                }
            }    
            
            if( res > 0 ){
                if (strstr ((char*)buffer_data_http, "200 OK")){
                    DBG.printf("proc_posrt: -> [%d] 200 OK\r\n", __LINE__ );
                }
            }
            proc_post_tic = SYSTEM_TICKER_1_SEG; 
            proc_post_step = PROC_POST_CLOSE_REQUEST;
            DBG.printf("proc_posrt: -> [%d] PROC_POST_CLOSE\r\n", __LINE__ );   
            break;
        
        case PROC_POST_CLOSE_REQUEST:
            
            proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
            
            proc_post_step = PROC_POST_OPEN_HCSR;
            
            break;
            
        case PROC_POST_OPEN_HCSR:
            if(IsHCSRFree == false){
            if( sock_connect ( SOCKET_TCP, enlaceHTLM, puertoHTLM, 10000) == 0 ){
                proc_post_tic = 1 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_SEND_HCSR;          
            }else{
                
                proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
                proc_post_step = PROC_POST_OPEN_HCSR;
                sock_close();
            }
            }else{
                proc_post_step = PROC_POST_OPEN_STATE;
            }
            break;
            
        case PROC_POST_SEND_HCSR: 
            
            sprintf(buff_send, "GET /estadorobotactual?estadorobot=ME TRABE CUANDO ESTABA %s HTTP/1.1\r\nHost: luckystrike051.pythonanywhere.com\r\nConnection: Close\r\n\r\n", string_est_aspi);
        
            
            res = sock_send ( (unsigned char *)buff_send,
                                strlen(buff_send), 25000);
            DBG.printf("proc_post: send: (%d)%s\r\n", res, buff_send);
            if( res > 0 ){
                proc_post_tic = SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_RECV_HCSR;
                DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV\r\n", __LINE__ );
            }else{
                proc_post_tic = 2 * SYSTEM_TICKER_1_SEG; 
                proc_post_step = PROC_POST_CLOSE_HCSR;
                DBG.printf("-proc_posrt: -> [%d] PROC_POST_CLOSE\r\n", __LINE__ );            
            }
            break;  
            
        case PROC_POST_RECV_HCSR:
            res = sock_recv ( buffer_data_http, 250, 500 );
            
            DBG.printf("proc_posrt: -> [%d] PROC_POST_RECV:%d\r\n", __LINE__, res );   
            
            if( res > 0 ){
                if (strstr ((char*)buffer_data_http, "200 OK")){
                    DBG.printf("proc_posrt: -> [%d] 200 OK\r\n", __LINE__ );
                }
            }
            proc_post_tic = SYSTEM_TICKER_1_SEG; 
            proc_post_step = PROC_POST_CLOSE_HCSR;
            DBG.printf("proc_posrt: -> [%d] PROC_POST_CLOSE_STATE\r\n", __LINE__ );   
            break;
        
        case PROC_POST_CLOSE_HCSR:
            
            proc_post_tic = 10 * SYSTEM_TICKER_1_SEG;
            proc_post_step = PROC_POST_OPEN_STATE;
            
            break;
             
    } 
}

//contador para no sobreponer colores 
void conta(){
    
    if(segundos > 0){
    segundos--;
    }
}

//contador motores
void conta_m(){
    if(IsHCSRFree){
        if(segundos_m > 0){
            segundos_m--;
        }
    }
}

//contador para no sobreponer colores 
void conta_send(){
    //DBG.printf("%d_", segundos);
    if(segundos_send > 0){
    segundos_send--;
    }
}

// contador de tic de wifi
void proc_post_tic_cb (){
    if (proc_post_tic)
        proc_post_tic-- ;
}   

//contador de duty de sensor de color (start)
void Ascend(){
    cont.start();
}

//contador de duty de sensor de color (stop n store)
void Desend(){
    cont.stop();
    util = cont.read_us();
    cont.reset();
}