File content as of revision 0:a53b6ec3fbb3:

/*******************************************************************/
/*                                                                 */
/*                Synchronisation                                  */
/*                                                                 */
/*  Procédures de synchronisation des piles du pont Bacalan        */
/*                                                                 */
/*  Creation : 3R MONTAUBAN                                        */
/*******************************************************************/

#include "Synchronisation.h"

S16 Ecart_Synchro_Avant ;
F32 Cumul_Ecarts_Synchro_F32 ;
F32 Correction_Synchro_F32 ;

/**     void    vSynchro_Initialise ( void )
*       Procédure appelée à chaque mise en route en mode automatique (synchronisé)
*/
void    vSynchro_Initialise ( void )
    {// Initialisation des variables de synchronisation
    Ecart_Synchro_Avant = 0 ;
    Cumul_Ecarts_Synchro_F32 = 0.0 ;
    Correction_Synchro_F32 = (F32) Correction_Synchro ;
    }

void    vSynchronise ( U8 Mode_Synchronisation, U8 Sens )
    {
    F32 Correction_F32 ;
    
    switch ( Mode_Synchronisation )
        {
        case CUSTOM :
            {
            // Ecrivez ici votre code personnalisé pour la synchronisation des piles du pont
            Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
            Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
            break ;
            }
        case DEUX_VITESSES :
            {
            // Calcul de l'écart de synchronisation
            Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
            Ecart_Synchro_Avant = Ecart_Synchronisation ;
                 
            if ( abs( Ecart_Synchronisation ) < Defaut_Mineur_Synchro )
                {// Pas d'écart de synchronisation significatif: meme vitesse pour les 2 rives
                Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
                Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
                }
            
            else if ( Sens == MONTE )
                {// Le pont monte, et l'ecart de synchronisation nécessite une correction de vitesse
                if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )        
                    {// Montée et Hauteur_RD > Hauteur_RG
                    Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
                    Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
                    }
                else if ( Ecart_Synchronisation < -Defaut_Majeur_Synchro )  
                    {// Montée et Hauteur_RD < Hauteur_RG
                    Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
                    Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
                    }
        
                }
            else if ( Sens == DESCEND )
                {// Le pont descend, et l'ecart de synchronisation nécessite une correction de vitesse
                if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )            
                    {// Descente et Hauteur_RD > Hauteur_RG
                    Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
                    Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
                    }
                else if ( Ecart_Synchronisation < (-Defaut_Majeur_Synchro) )    
                    {// Descente et Hauteur_RD < Hauteur_RG
                    Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
                    Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
                    }
                }
            break ;
            }
            
        case RD_SUIT_RG :
            {
            // Calcul de l'écart de synchronisation
            if ( Sens == MONTE )
                {
                Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
                }
            else
                {
                Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
                }
            Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
                            
            Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
            
            // Anticipation
            Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
            
            // Proportionnel
            Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
            
            // Intégral
            Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
            
            // Dérivé
            Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
            
            Consigne_Vitesse_RD = (S16) Correction_F32 ;
            
            Ecart_Synchro_Avant = Ecart_Synchronisation ;
            break ;
            }
            
        case RG_SUIT_RD :
            {
            // Calcul de l'écart de synchronisation
            if ( Sens == MONTE )
                {
                Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
                }
            else
                {
                Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
                }
            
            Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
                            
            Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
            
            // Anticipation
            Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
            
            // Proportionnel
            Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
            
            // Intégral
            Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
            
            // Dérivé
            Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
            
            Consigne_Vitesse_RG = (S16) Correction_F32 ;
            
            Ecart_Synchro_Avant = Ecart_Synchronisation ;
            break ;
            }
        case AUCUN :
        default :
            {
            // Pas de correction de vitesse pour la synchronisation
            Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
            Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
            }
        }
    }