Synchronisation.cpp

00001 /*******************************************************************/
00002 /*                                                                 */
00003 /*                Synchronisation                                  */
00004 /*                                                                 */
00005 /*  Procédures de synchronisation des piles du pont Bacalan        */
00006 /*                                                                 */
00007 /*  Creation : 3R MONTAUBAN                                        */
00008 /*******************************************************************/
00009 
00010 #include "Synchronisation.h"
00011 
00012 S16 Ecart_Synchro_Avant ;
00013 F32 Cumul_Ecarts_Synchro_F32 ;
00014 F32 Correction_Synchro_F32 ;
00015 
00016 /**     void    vSynchro_Initialise ( void )
00017 *       Procédure appelée à chaque mise en route en mode automatique (synchronisé)
00018 */
00019 void    vSynchro_Initialise ( void )
00020     {// Initialisation des variables de synchronisation
00021     Ecart_Synchro_Avant = 0 ;
00022     Cumul_Ecarts_Synchro_F32 = 0.0 ;
00023     Correction_Synchro_F32 = (F32) Correction_Synchro ;
00024     }
00025 
00026 void    vSynchronise ( U8 Mode_Synchronisation, U8 Sens )
00027     {
00028     F32 Correction_F32 ;
00029     
00030     switch ( Mode_Synchronisation )
00031         {
00032         case CUSTOM :
00033             {
00034             // Ecrivez ici votre code personnalisé pour la synchronisation des piles du pont
00035             Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00036             Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00037             break ;
00038             }
00039         case DEUX_VITESSES :
00040             {
00041             // Calcul de l'écart de synchronisation
00042             Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
00043             Ecart_Synchro_Avant = Ecart_Synchronisation ;
00044                  
00045             if ( abs( Ecart_Synchronisation ) < Defaut_Mineur_Synchro )
00046                 {// Pas d'écart de synchronisation significatif: meme vitesse pour les 2 rives
00047                 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00048                 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00049                 }
00050             
00051             else if ( Sens == MONTE )
00052                 {// Le pont monte, et l'ecart de synchronisation nécessite une correction de vitesse
00053                 if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )        
00054                     {// Montée et Hauteur_RD > Hauteur_RG
00055                     Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
00056                     Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00057                     }
00058                 else if ( Ecart_Synchronisation < -Defaut_Majeur_Synchro )  
00059                     {// Montée et Hauteur_RD < Hauteur_RG
00060                     Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00061                     Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
00062                     }
00063         
00064                 }
00065             else if ( Sens == DESCEND )
00066                 {// Le pont descend, et l'ecart de synchronisation nécessite une correction de vitesse
00067                 if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )            
00068                     {// Descente et Hauteur_RD > Hauteur_RG
00069                     Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00070                     Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
00071                     }
00072                 else if ( Ecart_Synchronisation < (-Defaut_Majeur_Synchro) )    
00073                     {// Descente et Hauteur_RD < Hauteur_RG
00074                     Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
00075                     Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00076                     }
00077                 }
00078             break ;
00079             }
00080             
00081         case RD_SUIT_RG :
00082             {
00083             // Calcul de l'écart de synchronisation
00084             if ( Sens == MONTE )
00085                 {
00086                 Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
00087                 }
00088             else
00089                 {
00090                 Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
00091                 }
00092             Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
00093                             
00094             Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00095             
00096             // Anticipation
00097             Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
00098             
00099             // Proportionnel
00100             Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
00101             
00102             // Intégral
00103             Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
00104             
00105             // Dérivé
00106             Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
00107             
00108             Consigne_Vitesse_RD = (S16) Correction_F32 ;
00109             
00110             Ecart_Synchro_Avant = Ecart_Synchronisation ;
00111             break ;
00112             }
00113             
00114         case RG_SUIT_RD :
00115             {
00116             // Calcul de l'écart de synchronisation
00117             if ( Sens == MONTE )
00118                 {
00119                 Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
00120                 }
00121             else
00122                 {
00123                 Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
00124                 }
00125             
00126             Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
00127                             
00128             Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00129             
00130             // Anticipation
00131             Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
00132             
00133             // Proportionnel
00134             Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
00135             
00136             // Intégral
00137             Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
00138             
00139             // Dérivé
00140             Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
00141             
00142             Consigne_Vitesse_RG = (S16) Correction_F32 ;
00143             
00144             Ecart_Synchro_Avant = Ecart_Synchronisation ;
00145             break ;
00146             }
00147         case AUCUN :
00148         default :
00149             {
00150             // Pas de correction de vitesse pour la synchronisation
00151             Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00152             Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00153             }
00154         }
00155     }