estado.cpp

00001 #include "declaraciones.h"
00002 
00003 
00004 extern volatile st_datos_servo datos_servo;          // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00005 extern volatile st_datos_servo datos_otro_servo;     // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
00006 extern long desviacion_servo;
00007 extern volatile int pos_actual_servo;
00008 extern volatile int pos_objetivo_servo;
00009 extern byte pulsadores;                     // Estado pulsadores del mando
00010 extern byte estado_old;        // ultimo estado enviado
00011 
00012 
00013 
00014 /*****************************************************
00015 * PROCESA CAMBIO DE ESTADO
00016 * De acuerdo con la lectura del servo y el estado recibido del mando
00017 *****************************************************/
00018 void procesa_estado(void)       // PROCESA CAMBIO DE ESTADO
00019 {
00020     static long timer_recto = millis();
00021     static long timer_rumbo = millis();
00022 
00023     if(datos_otro_servo.estado != DIR_REPOSO) { // Si el otro servo está activo
00024         if((datos_otro_servo.estado == DIR_END_PILOT && (datos_servo.estado==DIR_RUMBO || datos_servo.estado == DIR_PILOTO)) ) {
00025             datos_servo.estado = DIR_REPOSO;
00026             printf("En REPOSO estado otro:%d estado:%d\r\n", datos_otro_servo.estado, datos_servo.estado);
00027         }
00028         else if( (datos_otro_servo.estado == DIR_RUMBO || datos_otro_servo.estado == DIR_PILOTO) && (abs(desviacion_servo) >= 50) )
00029                 datos_servo.estado = DIR_END_PILOT;   // PARA FINALIZAR PILOTO AUTOMATICO DEL OTRO MANDO
00030         else {
00031             datos_servo.estado = DIR_REPOSO;
00032         }
00033     } else { // SI EL OTRO SERVO ESTÁ EN REPOSO
00034         if(datos_servo.estado == DIR_END_PILOT) {
00035             datos_servo.estado = DIR_REPOSO;
00036             pos_objetivo_servo = 0;
00037 //        } else if(datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_REPOSO) {
00038         }
00039         else if( (datos_servo.estado == DIR_REPOSO && abs(desviacion_servo) >= 50) ||
00040                  ( (datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO) && abs(desviacion_servo) >= 90) )
00041             {
00042                 datos_servo.estado = DIR_MASTER;
00043                 printf("De PILOTO a MASTER por desviacion_servo\r\n");
00044             }
00045         else if(datos_servo.estado == DIR_MASTER) { // Si está en master pasa a reposo si el mando se deja recto durante 1s
00046             if(abs(desviacion_servo) < 50) {
00047                 if((millis() - timer_recto) > 1000) {
00048                     datos_servo.estado = DIR_REPOSO;
00049                     printf("De MASTER a REPOSO por tiempo centrado\r\n");
00050 //                    envia_posicion_mando(datos_servo.posicion_mando);
00051                 }
00052             }
00053             else {
00054                 timer_recto = millis();
00055             }
00056         }
00057     }
00058 
00059 
00060     // PROCESA ACTIVACION O DESACTIVACION DE RUMBO
00061     if(pulsadores & MASK_PULS_RUMBO == MASK_PULS_RUMBO) { // Si pulsado rumbo
00062         if(timer_rumbo==0)
00063             timer_rumbo = millis();
00064     } else { // Se ha soltado pulsador
00065         if(timer_rumbo>0) {
00066             long tiempo = millis() - timer_rumbo;
00067             printf("tiempo:%d \r\n", tiempo);
00068             printf("datos_servo.estado:%d \r\n", datos_servo.estado);
00069             printf("datos_otro_servo.estado:%d\r\n", datos_otro_servo.estado);
00070 
00071             if(datos_otro_servo.estado == DIR_REPOSO && datos_servo.estado == DIR_REPOSO) {   // Los dos en reposo para activa RUMBO o PILOTO
00072                 if(tiempo > 50 && tiempo <= 2000) {  // Pulsacion corta
00073                     datos_servo.estado = DIR_RUMBO;
00074                     escribe_parametro_servo(1, 1, 10);
00075                     escribe_parametro_servo(1, 2, 2000);
00076                     printf("en RUMBO\r\n");
00077                 } else if(tiempo > 2000 && tiempo < 8000) {    // Pulsacion larga
00078                     datos_servo.estado = DIR_PILOTO;
00079                     escribe_parametro_servo(1, 1, 10);
00080                     escribe_parametro_servo(1, 2, 2000);
00081                     printf("en PILOTO\r\n");
00082                 }
00083             } else {
00084                 if(datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO) {
00085                     datos_servo.estado = DIR_REPOSO;
00086                     printf("en REPOSO\r\n");
00087                     pos_objetivo_servo = 0;
00088                 } else if(datos_otro_servo.estado == DIR_RUMBO || datos_otro_servo.estado == DIR_PILOTO) {
00089                     datos_servo.estado = DIR_END_PILOT;   // PARA FINALIZAR PILOTO AUTOMATICO DEL OTRO MANDO
00090                     printf("en END_PILOT\r\n");
00091                 }
00092             }
00093             timer_rumbo = 0;
00094         }
00095     }
00096 
00097 
00098 
00099     // PROCESA EL CAMBIO DE ESTADO
00100     if((estado_old == DIR_RUMBO || estado_old == DIR_PILOTO) && (datos_servo.estado == DIR_REPOSO || datos_servo.estado == DIR_MASTER)) {
00101         pos_objetivo_servo = 0;
00102         escribe_parametro_servo(1, 1, 50);  // velocidad de respuesta del servo
00103         escribe_parametro_servo(1, 2, 10000);
00104     }
00105     if(estado_old != datos_servo.estado) {
00106 //        envia_posicion_mando(datos_servo.posicion_mando); // SI CAMBIO ESTADO ENVIA POSICION Y ESTADO
00107         estado_old = datos_servo.estado;  // Actualiza ultimo estado enviado
00108     }
00109 
00110 }
00111