Version initiale
Dependencies: mbed
Fork of Le_Pont_V10116 by
Synchronisation.cpp
- Committer:
- CS
- Date:
- 2018-03-29
- Revision:
- 2:a10c8133d71c
- Parent:
- 0:8b3c6f593515
File content as of revision 2:a10c8133d71c:
/*******************************************************************/ /* */ /* 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 ; } } }