Le Pont 3R / Mbed 2 deprecated Le_Pont_V10116

Version initiale

Dependencies:   mbed

Fork of Le_Pont_V10116 by SAGNES Christophe

Synchronisation.cpp@2:a10c8133d71c, 2018-03-29 (annotated)

Committer:
CS
Date:
Thu Mar 29 15:41:22 2018 +0000
Revision:
2:a10c8133d71c
Parent:
0:8b3c6f593515 
Publish of "Le_Pont_V11003" in the team folder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CS 0:8b3c6f593515 1 /*******************************************************************/
CS 0:8b3c6f593515 2 /* */
CS 0:8b3c6f593515 3 /* Synchronisation */
CS 0:8b3c6f593515 4 /* */
CS 0:8b3c6f593515 5 /* Procédures de synchronisation des piles du pont Bacalan */
CS 0:8b3c6f593515 6 /* */
CS 0:8b3c6f593515 7 /* Creation : 3R MONTAUBAN */
CS 0:8b3c6f593515 8 /*******************************************************************/
CS 0:8b3c6f593515 9
CS 0:8b3c6f593515 10 #include "Synchronisation.h"
CS 0:8b3c6f593515 11
CS 0:8b3c6f593515 12 S16 Ecart_Synchro_Avant ;
CS 0:8b3c6f593515 13 F32 Cumul_Ecarts_Synchro_F32 ;
CS 0:8b3c6f593515 14 F32 Correction_Synchro_F32 ;
CS 0:8b3c6f593515 15
CS 0:8b3c6f593515 16 /** void vSynchro_Initialise ( void )
CS 0:8b3c6f593515 17 * Procédure appelée à chaque mise en route en mode automatique (synchronisé)
CS 0:8b3c6f593515 18 */
CS 0:8b3c6f593515 19 void vSynchro_Initialise ( void )
CS 0:8b3c6f593515 20 {// Initialisation des variables de synchronisation
CS 0:8b3c6f593515 21 Ecart_Synchro_Avant = 0 ;
CS 0:8b3c6f593515 22 Cumul_Ecarts_Synchro_F32 = 0.0 ;
CS 0:8b3c6f593515 23 Correction_Synchro_F32 = (F32) Correction_Synchro ;
CS 0:8b3c6f593515 24 }
CS 0:8b3c6f593515 25
CS 0:8b3c6f593515 26 void vSynchronise ( U8 Mode_Synchronisation, U8 Sens )
CS 0:8b3c6f593515 27 {
CS 0:8b3c6f593515 28 F32 Correction_F32 ;
CS 0:8b3c6f593515 29
CS 0:8b3c6f593515 30 switch ( Mode_Synchronisation )
CS 0:8b3c6f593515 31 {
CS 0:8b3c6f593515 32 case CUSTOM :
CS 0:8b3c6f593515 33 {
CS 0:8b3c6f593515 34 // Ecrivez ici votre code personnalisé pour la synchronisation des piles du pont
CS 0:8b3c6f593515 35 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 36 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 37 break ;
CS 0:8b3c6f593515 38 }
CS 0:8b3c6f593515 39 case DEUX_VITESSES :
CS 0:8b3c6f593515 40 {
CS 0:8b3c6f593515 41 // Calcul de l'écart de synchronisation
CS 0:8b3c6f593515 42 Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
CS 0:8b3c6f593515 43 Ecart_Synchro_Avant = Ecart_Synchronisation ;
CS 0:8b3c6f593515 44
CS 0:8b3c6f593515 45 if ( abs( Ecart_Synchronisation ) < Defaut_Mineur_Synchro )
CS 0:8b3c6f593515 46 {// Pas d'écart de synchronisation significatif: meme vitesse pour les 2 rives
CS 0:8b3c6f593515 47 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 48 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 49 }
CS 0:8b3c6f593515 50
CS 0:8b3c6f593515 51 else if ( Sens == MONTE )
CS 0:8b3c6f593515 52 {// Le pont monte, et l'ecart de synchronisation nécessite une correction de vitesse
CS 0:8b3c6f593515 53 if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )
CS 0:8b3c6f593515 54 {// Montée et Hauteur_RD > Hauteur_RG
CS 0:8b3c6f593515 55 Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
CS 0:8b3c6f593515 56 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 57 }
CS 0:8b3c6f593515 58 else if ( Ecart_Synchronisation < -Defaut_Majeur_Synchro )
CS 0:8b3c6f593515 59 {// Montée et Hauteur_RD < Hauteur_RG
CS 0:8b3c6f593515 60 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 61 Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
CS 0:8b3c6f593515 62 }
CS 0:8b3c6f593515 63
CS 0:8b3c6f593515 64 }
CS 0:8b3c6f593515 65 else if ( Sens == DESCEND )
CS 0:8b3c6f593515 66 {// Le pont descend, et l'ecart de synchronisation nécessite une correction de vitesse
CS 0:8b3c6f593515 67 if ( Ecart_Synchronisation > Defaut_Majeur_Synchro )
CS 0:8b3c6f593515 68 {// Descente et Hauteur_RD > Hauteur_RG
CS 0:8b3c6f593515 69 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 70 Consigne_Vitesse_RG = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
CS 0:8b3c6f593515 71 }
CS 0:8b3c6f593515 72 else if ( Ecart_Synchronisation < (-Defaut_Majeur_Synchro) )
CS 0:8b3c6f593515 73 {// Descente et Hauteur_RD < Hauteur_RG
CS 0:8b3c6f593515 74 Consigne_Vitesse_RD = (S16)( (F32)Consigne_Vitesse_Auto * Correction_Synchro_F32 / 100.0 ) ;
CS 0:8b3c6f593515 75 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 76 }
CS 0:8b3c6f593515 77 }
CS 0:8b3c6f593515 78 break ;
CS 0:8b3c6f593515 79 }
CS 0:8b3c6f593515 80
CS 0:8b3c6f593515 81 case RD_SUIT_RG :
CS 0:8b3c6f593515 82 {
CS 0:8b3c6f593515 83 // Calcul de l'écart de synchronisation
CS 0:8b3c6f593515 84 if ( Sens == MONTE )
CS 0:8b3c6f593515 85 {
CS 0:8b3c6f593515 86 Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
CS 0:8b3c6f593515 87 }
CS 0:8b3c6f593515 88 else
CS 0:8b3c6f593515 89 {
CS 0:8b3c6f593515 90 Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
CS 0:8b3c6f593515 91 }
CS 0:8b3c6f593515 92 Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
CS 0:8b3c6f593515 93
CS 0:8b3c6f593515 94 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 95
CS 0:8b3c6f593515 96 // Anticipation
CS 0:8b3c6f593515 97 Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
CS 0:8b3c6f593515 98
CS 0:8b3c6f593515 99 // Proportionnel
CS 0:8b3c6f593515 100 Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
CS 0:8b3c6f593515 101
CS 0:8b3c6f593515 102 // Intégral
CS 0:8b3c6f593515 103 Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
CS 0:8b3c6f593515 104
CS 0:8b3c6f593515 105 // Dérivé
CS 0:8b3c6f593515 106 Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
CS 0:8b3c6f593515 107
CS 0:8b3c6f593515 108 Consigne_Vitesse_RD = (S16) Correction_F32 ;
CS 0:8b3c6f593515 109
CS 0:8b3c6f593515 110 Ecart_Synchro_Avant = Ecart_Synchronisation ;
CS 0:8b3c6f593515 111 break ;
CS 0:8b3c6f593515 112 }
CS 0:8b3c6f593515 113
CS 0:8b3c6f593515 114 case RG_SUIT_RD :
CS 0:8b3c6f593515 115 {
CS 0:8b3c6f593515 116 // Calcul de l'écart de synchronisation
CS 0:8b3c6f593515 117 if ( Sens == MONTE )
CS 0:8b3c6f593515 118 {
CS 0:8b3c6f593515 119 Ecart_Synchronisation = ( Hauteur_RD - Hauteur_RG + Ecart_Synchro_Avant ) / 2 ;
CS 0:8b3c6f593515 120 }
CS 0:8b3c6f593515 121 else
CS 0:8b3c6f593515 122 {
CS 0:8b3c6f593515 123 Ecart_Synchronisation = ( Hauteur_RG - Hauteur_RD + Ecart_Synchro_Avant ) / 2 ;
CS 0:8b3c6f593515 124 }
CS 0:8b3c6f593515 125
CS 0:8b3c6f593515 126 Cumul_Ecarts_Synchro_F32 = Cumul_Ecarts_Synchro_F32 + (F32) Ecart_Synchronisation ;
CS 0:8b3c6f593515 127
CS 0:8b3c6f593515 128 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 129
CS 0:8b3c6f593515 130 // Anticipation
CS 0:8b3c6f593515 131 Correction_F32 = (F32) Consigne_Vitesse_Auto * (F32) Anticipation_Synchro / 100.0 ;
CS 0:8b3c6f593515 132
CS 0:8b3c6f593515 133 // Proportionnel
CS 0:8b3c6f593515 134 Correction_F32 = Correction_F32 + (F32) KP_Synchro * (F32) Ecart_Synchronisation / 100.0 ;
CS 0:8b3c6f593515 135
CS 0:8b3c6f593515 136 // Intégral
CS 0:8b3c6f593515 137 Correction_F32 = Correction_F32 + (F32) KI_Synchro * (F32) Cumul_Ecarts_Synchro_F32 / 100.0 ;
CS 0:8b3c6f593515 138
CS 0:8b3c6f593515 139 // Dérivé
CS 0:8b3c6f593515 140 Correction_F32 = Correction_F32 + (F32) KD_Synchro * ( (F32) Ecart_Synchronisation - (F32) Ecart_Synchro_Avant ) / 100.0 ;
CS 0:8b3c6f593515 141
CS 0:8b3c6f593515 142 Consigne_Vitesse_RG = (S16) Correction_F32 ;
CS 0:8b3c6f593515 143
CS 0:8b3c6f593515 144 Ecart_Synchro_Avant = Ecart_Synchronisation ;
CS 0:8b3c6f593515 145 break ;
CS 0:8b3c6f593515 146 }
CS 0:8b3c6f593515 147 case AUCUN :
CS 0:8b3c6f593515 148 default :
CS 0:8b3c6f593515 149 {
CS 0:8b3c6f593515 150 // Pas de correction de vitesse pour la synchronisation
CS 0:8b3c6f593515 151 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 152 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
CS 0:8b3c6f593515 153 }
CS 0:8b3c6f593515 154 }
CS 0:8b3c6f593515 155 }