File content as of revision 8:b4465148d206:

#include "declaraciones.h"


extern volatile st_datos_servo datos_servo;          // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
extern volatile st_datos_servo datos_otro_servo;     // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
extern long desviacion_servo;
extern volatile int pos_actual_servo;
extern volatile int pos_objetivo_servo;
extern byte pulsadores;                     // Estado pulsadores del mando
extern byte estado_old;        // ultimo estado enviado



/*****************************************************
* PROCESA CAMBIO DE ESTADO
* De acuerdo con la lectura del servo y el estado recibido del mando
*****************************************************/
void procesa_estado(void)       // PROCESA CAMBIO DE ESTADO
{
    static long timer_recto = millis();
    static long timer_rumbo = millis();

    if(datos_otro_servo.estado != DIR_REPOSO) { // Si el otro servo está activo
        if((datos_otro_servo.estado == DIR_END_PILOT && (datos_servo.estado==DIR_RUMBO || datos_servo.estado == DIR_PILOTO)) ) {
            datos_servo.estado = DIR_REPOSO;
            printf("En REPOSO estado otro:%d estado:%d\r\n", datos_otro_servo.estado, datos_servo.estado);
        }
        else if( (datos_otro_servo.estado == DIR_RUMBO || datos_otro_servo.estado == DIR_PILOTO) && (abs(desviacion_servo) >= 50) )
                datos_servo.estado = DIR_END_PILOT;   // PARA FINALIZAR PILOTO AUTOMATICO DEL OTRO MANDO
        else {
            datos_servo.estado = DIR_REPOSO;
        }
    } else { // SI EL OTRO SERVO ESTÁ EN REPOSO
        if(datos_servo.estado == DIR_END_PILOT) {
            datos_servo.estado = DIR_REPOSO;
            pos_objetivo_servo = 0;
//        } else if(datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_REPOSO) {
        }
        else if( (datos_servo.estado == DIR_REPOSO && abs(desviacion_servo) >= 50) ||
                 ( (datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO) && abs(desviacion_servo) >= 90) )
            {
                datos_servo.estado = DIR_MASTER;
                printf("De PILOTO a MASTER por desviacion_servo\r\n");
            }
        else if(datos_servo.estado == DIR_MASTER) { // Si está en master pasa a reposo si el mando se deja recto durante 1s
            if(abs(desviacion_servo) < 50) {
                if((millis() - timer_recto) > 1000) {
                    datos_servo.estado = DIR_REPOSO;
                    printf("De MASTER a REPOSO por tiempo centrado\r\n");
//                    envia_posicion_mando(datos_servo.posicion_mando);
                }
            }
            else {
                timer_recto = millis();
            }
        }
    }


    // PROCESA ACTIVACION O DESACTIVACION DE RUMBO
    if(pulsadores & MASK_PULS_RUMBO == MASK_PULS_RUMBO) { // Si pulsado rumbo
        if(timer_rumbo==0)
            timer_rumbo = millis();
    } else { // Se ha soltado pulsador
        if(timer_rumbo>0) {
            long tiempo = millis() - timer_rumbo;
            printf("tiempo:%d \r\n", tiempo);
            printf("datos_servo.estado:%d \r\n", datos_servo.estado);
            printf("datos_otro_servo.estado:%d\r\n", datos_otro_servo.estado);

            if(datos_otro_servo.estado == DIR_REPOSO && datos_servo.estado == DIR_REPOSO) {   // Los dos en reposo para activa RUMBO o PILOTO
                if(tiempo > 50 && tiempo <= 2000) {  // Pulsacion corta
                    datos_servo.estado = DIR_RUMBO;
                    escribe_parametro_servo(1, 1, 10);
                    escribe_parametro_servo(1, 2, 2000);
                    printf("en RUMBO\r\n");
                } else if(tiempo > 2000 && tiempo < 8000) {    // Pulsacion larga
                    datos_servo.estado = DIR_PILOTO;
                    escribe_parametro_servo(1, 1, 10);
                    escribe_parametro_servo(1, 2, 2000);
                    printf("en PILOTO\r\n");
                }
            } else {
                if(datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO) {
                    datos_servo.estado = DIR_REPOSO;
                    printf("en REPOSO\r\n");
                    pos_objetivo_servo = 0;
                } else if(datos_otro_servo.estado == DIR_RUMBO || datos_otro_servo.estado == DIR_PILOTO) {
                    datos_servo.estado = DIR_END_PILOT;   // PARA FINALIZAR PILOTO AUTOMATICO DEL OTRO MANDO
                    printf("en END_PILOT\r\n");
                }
            }
            timer_rumbo = 0;
        }
    }



    // PROCESA EL CAMBIO DE ESTADO
    if((estado_old == DIR_RUMBO || estado_old == DIR_PILOTO) && (datos_servo.estado == DIR_REPOSO || datos_servo.estado == DIR_MASTER)) {
        pos_objetivo_servo = 0;
        escribe_parametro_servo(1, 1, 50);  // velocidad de respuesta del servo
        escribe_parametro_servo(1, 2, 10000);
    }
    if(estado_old != datos_servo.estado) {
//        envia_posicion_mando(datos_servo.posicion_mando); // SI CAMBIO ESTADO ENVIA POSICION Y ESTADO
        estado_old = datos_servo.estado;  // Actualiza ultimo estado enviado
    }

}