timones.cpp

00001 //
00002 // RUTINAS DE CONTROL DE TIMONES
00003 //
00004 
00005 #include "declaraciones.h"
00006 
00007 
00008 extern volatile st_datos_servo datos_servo;      // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00009 extern volatile st_datos_servo datos_otro_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
00010 extern int cons_timon_babor;            // consigna % de timon babor bajado
00011 extern int cons_timon_estribor;         // consigna % de timon estribor bajado
00012 extern long desviacion_servo;
00013 extern int16_t torque_servo;            // torque del servo
00014 extern float diferencia_rumbo;          // Diferencia al Rumbo deseado
00015 extern volatile int pos_objetivo_servo;
00016 extern volatile bool f_actualiza_tim_trans;     // Pone flag para actualizar timones y transmision
00017 
00018 
00019 
00020 
00021 /*******************************************************************************
00022 // devuelve diferencia del rumbo actual al fijado
00023 // si es positiva debe girar a estribor, si negativa a babor
00024 *******************************************************************************/
00025 float dif_rumbo(float rumbo_actual, float rumbo_deseado){
00026 
00027   float dif = rumbo_deseado - rumbo_actual;
00028   if(dif < -180.0)
00029     dif += 360;
00030   else if(dif > 180)
00031     dif -= 360;
00032   dif = constrain(dif, -180.0, 180.0);
00033     
00034   return (dif);
00035 }
00036 
00037 
00038 
00039 #define VELOCIDAD_MAX 70.0
00040 #define HISTERESIS_RUMBO 0.5
00041 #define FACTOR_INTEGRAL_TIMON 0.01   //0.3 para 100% en 30s
00042 
00043 /*******************************************************************************
00044 // devuelve el valor maximo de timón en % según la velocidad
00045 *******************************************************************************/
00046 float max_timon(float velocidad){
00047 
00048   velocidad = constrain(velocidad, 0.0, VELOCIDAD_MAX);
00049   float max = 80 * (velocidad * velocidad)/(VELOCIDAD_MAX * VELOCIDAD_MAX);
00050   max = constrain(100-max, 20.0, 100.0);
00051 
00052   return max;
00053 }
00054 
00055 
00056 
00057 
00058 
00059 /*******************************************************************************
00060 // actualiza posición de los timones en piloto automatico segun rumbo actual, deseado, velocidad y tiempo
00061 *******************************************************************************/
00062 void pos_timones_p_automatico( float rumbo_actual, float rumbo_deseado, float velocidad){
00063   static float rumbo_anterior = 0.0;
00064   static float tiempo = 0.0;
00065   static long tiempo_ms = 0;
00066 
00067   if(rumbo_deseado != rumbo_anterior){
00068     tiempo = 0.0;
00069     tiempo_ms = millis();
00070     rumbo_anterior = rumbo_deseado;  
00071   }
00072   else{ // imncrementa tiempo
00073     tiempo += ((millis()-tiempo_ms)/1000.0);
00074   }
00075   diferencia_rumbo = dif_rumbo(rumbo_actual, rumbo_deseado);
00076   if( abs(diferencia_rumbo) > HISTERESIS_RUMBO){
00077     // Recalcula valores para los timones
00078     float timon = abs(diferencia_rumbo)/180.0*100.0;             // Valor proporcional
00079     timon += abs(diferencia_rumbo)*(tiempo/10)*FACTOR_INTEGRAL_TIMON; // Valor Integral
00080     if(diferencia_rumbo > 0.0){  // Rumbo a estribor, baja timón de estribor
00081       timon = constrain(timon, datos_servo.trim_timon_estr, max_timon(velocidad)); // Ajusta al valor máximo de acuerdo con la velocidad y el offset
00082       cons_timon_babor = datos_servo.trim_timon_bab;  // pone a valor mínimo
00083       cons_timon_estribor = timon;
00084     }
00085     else { // Rumbo a babor, baja timón de babor
00086       timon = constrain(timon, datos_servo.trim_timon_bab, max_timon(velocidad)); // Ajusta al valor máximo de acuerdo con la velocidad y el offset
00087       cons_timon_estribor = datos_servo.trim_timon_estr;  // pone a valor mínimo
00088       cons_timon_babor = timon;      
00089     }
00090   }
00091   else{
00092     cons_timon_babor = datos_servo.trim_timon_bab;  // pone a valor mínimo
00093     cons_timon_estribor = datos_servo.trim_timon_estr;      // pone a valor mínimo
00094   }
00095 }
00096 
00097 
00098 
00099 /*******************************************************************************
00100  * SINCRONIZA SERVOS ENTRE ELLOS SEGUN SU ESTADO
00101  ******************************************************************************/
00102 void sincroniza_servos(void){
00103 
00104     if(datos_otro_servo.estado != DIR_REPOSO){
00105         pos_objetivo_servo = (int16_t)datos_otro_servo.posicion_mando;
00106         printf("pos_objetivo_servo=%d\r\n", pos_objetivo_servo);
00107         datos_servo.rumbo = datos_otro_servo.rumbo;
00108     }
00109 }
00110 
00111 
00112 
00113 int rumbosPiloto[]={0,30,0,270,300,0,10,120,330,350};
00114 int rumbosRumboFijo[]={0,179,0,181,0,179,0,181,0,179};
00115 //int rumbosRumboFijo[]={0,5,235,355,10,0,245,0,350,150};
00116 
00117 /*******************************************************************************
00118  * PROCESA PILOTO AUTOMATICO SI ESTA CONECTADO, PROCESA CADA PERIODO (25ms)
00119  ******************************************************************************/
00120 void procesa_piloto_automatico(){
00121 static int cnt_torque_max = 0;
00122 
00123     if(datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_RUMBO){
00124       if(torque_servo >= TORQUE_MAX_PILOTO_AUTO){
00125         if(++cnt_torque_max>10){  // Salida de piloto automatico
00126           datos_servo.estado = DIR_REPOSO;
00127           printf("Sale de PILOTO por torque maximo\r\n");
00128         }
00129       }
00130       else
00131         cnt_torque_max = 0;
00132     }
00133     if(datos_servo.estado == DIR_PILOTO){
00134       int i = (millis()/10000L)%10;
00135       datos_servo.rumbo_piloto_auto = rumbosPiloto[i];
00136       pos_timones_p_automatico( datos_servo.rumbo, datos_servo.rumbo_piloto_auto, datos_servo.velocidad);
00137     }
00138     else if(datos_servo.estado == DIR_RUMBO){
00139       int i = (millis()/20000L)%10;
00140 //      DEBUG_PORT.println(i);
00141       datos_servo.rumbo_fijado = rumbosRumboFijo[i];
00142       pos_timones_p_automatico( datos_servo.rumbo, datos_servo.rumbo_fijado, datos_servo.velocidad);
00143     }
00144     else{
00145 //      pos_objetivo_servo = 0;
00146       actualiza_timones();  // MANUAL
00147     }
00148     if(datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_RUMBO){
00149       long difTimones = datos_servo.timon_babor - datos_servo.timon_estribor;
00150       pos_objetivo_servo -= (pos_objetivo_servo-((difTimones * DESVIACION_MAXIMA) / 100))/10;
00151     }
00152 }
00153 
00154 
00155 //***************************************************************
00156 // * ACTUALIZA TIMONES
00157 //***************************************************************
00158 void actualiza_timones(){   // ACTUALIZA TIMONES
00159 
00160   // ACTIVA TIMONES SEGUN DESVIACION
00161   if(desviacion_servo >= 40){
00162     cons_timon_babor = map(abs(desviacion_servo-40), 0, DESVIACION_MAXIMA, datos_servo.trim_timon_bab, 100);
00163     cons_timon_estribor = datos_servo.trim_timon_estr;
00164     if(cons_timon_babor <= TIMONES_HIST)
00165       cons_timon_babor = datos_servo.trim_timon_bab;
00166   }
00167   else if(desviacion_servo <= -40){
00168     cons_timon_estribor = map(abs(desviacion_servo+40), 0, DESVIACION_MAXIMA, datos_servo.trim_timon_estr, 100);
00169     cons_timon_babor = datos_servo.trim_timon_bab;
00170      if(cons_timon_estribor <= TIMONES_HIST)
00171       cons_timon_estribor = datos_servo.trim_timon_estr;
00172   }
00173   else{
00174       cons_timon_babor = datos_servo.trim_timon_bab;
00175       cons_timon_estribor = datos_servo.trim_timon_estr;      
00176   }
00177 }
00178 
00179 
00180 #define SALTO_TIMON 5   //5% cada 250ms
00181 
00182 
00183 //***************************************************************
00184 // Pocesa activación y posición timones, llamada cada 250ms
00185 //***************************************************************
00186 void  procesa_posicion_timones(void){
00187 
00188 
00189 //    printf("cons_timon_babor:%d timon_babor:%d\r\n", cons_timon_babor, datos_servo.timon_babor );
00190 //    printf("cons_timon_estribor:%d timon_estribor:%d\r\n", cons_timon_estribor, datos_servo.timon_estribor );
00191 
00192     datos_servo.timon_babor = cons_timon_babor;
00193     datos_servo.timon_estribor = cons_timon_estribor;
00194     
00195 /*
00196     if(abs(cons_timon_babor - datos_servo.timon_babor) > TIMONES_HIST){
00197         printf("dif=%d\r\n", abs(cons_timon_babor - datos_servo.timon_babor) );
00198         // Si la diferencia es mayor que la histeresis, mueve timon
00199         if(cons_timon_babor > datos_servo.timon_babor){
00200             if(datos_servo.timon_estribor <= datos_servo.trim_timon_estr)
00201             datos_servo.timon_babor += SALTO_TIMON; // SALTO_TIMON% cada 250ms
00202         }
00203         else{
00204             datos_servo.timon_babor -= SALTO_TIMON; // -SALTO_TIMON% cada 250ms
00205             if(datos_servo.timon_babor > 127)
00206                 datos_servo.timon_babor=0;
00207         }
00208     }
00209     else{
00210         // para movimiento del timon
00211         datos_servo.timon_babor = cons_timon_babor;
00212     }
00213         
00214     if(abs(cons_timon_estribor - datos_servo.timon_estribor) > TIMONES_HIST){
00215         // Si la diferencia es mayor que la histeresis, mueve timon
00216         if(cons_timon_estribor > datos_servo.timon_estribor){
00217             if(datos_servo.timon_babor <= datos_servo.trim_timon_bab)
00218                 datos_servo.timon_estribor += SALTO_TIMON; // SALTO_TIMON% cada 100ms
00219         }
00220         else{
00221             datos_servo.timon_estribor -= SALTO_TIMON; // -SALTO_TIMON% cada 100ms
00222             if(datos_servo.timon_estribor > 127)
00223                 datos_servo.timon_estribor=0;
00224         }
00225     }
00226     else{
00227         // para movimiento del timon
00228         datos_servo.timon_estribor = cons_timon_estribor;
00229     }
00230 */
00231 
00232 }
00233