Le Pont 3R
Version initiale
Dependencies: mbed
Fork of
Le_Pont_V10116
by 
SAGNES Christophe
Embed:
(wiki syntax)
Show/hide line numbers
Gemma.cpp
00001 //****************************************************************** 00002 // 00003 // Gemma 00004 // 00005 // Logiciel de gestion des modes de marche et d'arret du Pont Bacalan 00006 // 00007 // 00008 // 00009 //******************************************************************** 00010 00011 #include "Gemma.h" 00012 00013 Serial PC3(USBTX, USBRX) ; 00014 // Chronomètre 00015 Timer Chrono_Param ; 00016 AnalogOut Consigne_aout (p18) ; 00017 00018 U8 No_Parametre ; 00019 S16 *pParametres_Console_RG ; 00020 S16 *pParametres_Console_RD ; 00021 00022 void vGemma_Init () 00023 { 00024 Chrono_Param.start () ; 00025 pParametres_Console_RG = &Param_Version_RG ; 00026 pParametres_Console_RD = &Param_Version_RD ; 00027 No_Parametre = 0 ; 00028 Consigne_aout.write ( 0.0 ) ; 00029 } 00030 00031 void vGemma ( Pilote* pRive_Droite , Pilote* pRive_Gauche ) 00032 { 00033 // Fonctionnement des differents modes 00034 PC3.printf("\r\n Mode: %i", Mode_Fonctionnement) ; 00035 if ( Mode_Fonctionnement == MODE_MANUEL ) 00036 // En mode manuel, pas d'action maintenue, les 2 rives sont indépendantes 00037 { 00038 //Raz des parametres de synchronisation 00039 vSynchro_Initialise ( ) ; 00040 00041 if ( BTN_Monte_RD ) 00042 {// Montée rive droite 00043 if ( ( Hauteur_RD - Hauteur_RG ) < Defaut_Critique_Manu_mm ) 00044 { 00045 PC3.printf("\t Monte RD") ; 00046 pRive_Droite->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu ) ; 00047 } 00048 else 00049 { 00050 pRive_Droite->Arret() ; 00051 } 00052 00053 BTN_Descend_RD = 0 ; 00054 } 00055 else if ( BTN_Descend_RD ) 00056 {// Descente rive droite 00057 if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Manu_mm ) 00058 { 00059 PC3.printf("\t Descend RD") ; 00060 pRive_Droite->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu ) ; 00061 } 00062 else 00063 { 00064 pRive_Droite->Arret() ; 00065 } 00066 BTN_Monte_RD = 0 ; 00067 } 00068 else 00069 {// Arrêt rive droite 00070 pRive_Droite->Arret() ; 00071 BTN_Descend_RD = 0 ; 00072 BTN_Monte_RD = 0 ; 00073 //PC3.printf("\r\n Arret RD") ; 00074 } 00075 if ( BTN_Monte_RG ) 00076 {// Montée rive gauche 00077 if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Manu_mm ) 00078 { 00079 PC3.printf("\t Monte RG") ; 00080 pRive_Gauche->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu ) ; 00081 Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ; 00082 } 00083 else 00084 { 00085 pRive_Gauche->Arret() ; 00086 } 00087 BTN_Descend_RG = 0 ; 00088 } 00089 else if ( BTN_Descend_RG ) 00090 {// Descente rive gauche 00091 if ( ( Hauteur_RD - Hauteur_RG ) < Defaut_Critique_Manu_mm ) 00092 { 00093 PC3.printf("\t Descend RG") ; 00094 pRive_Gauche->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu ) ; 00095 Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ; 00096 } 00097 else 00098 { 00099 pRive_Gauche->Arret() ; 00100 } 00101 BTN_Monte_RG = 0 ; 00102 } 00103 else 00104 {// Arret rive gauche 00105 pRive_Gauche->Arret() ; 00106 BTN_Monte_RG = 0 ; 00107 BTN_Descend_RG = 0 ; 00108 Consigne_aout.write ( 0.0 ) ; 00109 //PC3.printf("\r\n Arret RG") ; 00110 } 00111 if ( BTN_Frein ) 00112 {// Liberation des freins 00113 if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD ) 00114 { 00115 if (pRive_Gauche->Etat_Frein == SERRE ) pRive_Gauche->Frein( DESSERRE ) ; 00116 if (pRive_Droite->Etat_Frein == SERRE ) pRive_Droite->Frein( DESSERRE ) ; 00117 } 00118 } 00119 else 00120 {// Serrage des freins 00121 if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD ) 00122 { 00123 if (pRive_Gauche->Etat_Frein == DESSERRE ) pRive_Gauche->Frein( SERRE ) ; 00124 if (pRive_Droite->Etat_Frein == DESSERRE ) pRive_Droite->Frein( SERRE ) ; 00125 } 00126 } 00127 } 00128 00129 else if ( Mode_Fonctionnement == MODE_AUTOMATIQUE ) 00130 {// En mode automatique, les rives sont synchronisées, avec consigne de vitesse et hauteur cible 00131 if ( BTN_Arret ) 00132 {// Arret du pont 00133 BTN_Monte = 0 ; 00134 BTN_Descend = 0 ; 00135 BTN_Arret = 0 ; 00136 pRive_Droite->Arret() ; 00137 pRive_Gauche->Arret() ; 00138 Consigne_aout.write ( 0.0 ) ; 00139 00140 vSynchro_Initialise ( ) ; 00141 00142 //PC3.printf("\r\n Btn Arret") ; 00143 } 00144 else 00145 {// Vérification de l'ecart de synchronisation: différence de hauteur entre droite et gauche 00146 if ( abs( Hauteur_RD - Hauteur_RG ) > Defaut_Critique_Synchro ) 00147 {// Si l'ecart depasse le seuil critique: on arrete tout! 00148 BTN_Arret = 0 ; 00149 BTN_Descend = 0 ; 00150 BTN_Monte = 0 ; 00151 pRive_Droite->Arret() ; 00152 pRive_Gauche->Arret() ; 00153 Consigne_aout.write ( 0.0 ) ; 00154 PC3.printf("\r\n Defaut Critique") ; 00155 } 00156 else if ( BTN_Monte ) 00157 {// Si on est en haut 00158 if ( ( Hauteur_P >= Consigne_Haute_P ) 00159 ||( Hauteur_RD >= Consigne_Haute_P ) 00160 ||( Hauteur_RG >= Consigne_Haute_P ) ) 00161 { 00162 BTN_Monte = 0 ; 00163 pRive_Droite->Arret() ; 00164 pRive_Gauche->Arret() ; 00165 PC3.printf("\r\n En Haut") ; 00166 } 00167 else 00168 {// Montée 00169 PC3.printf("\t Monte Auto mode: %i", Mode_Synchro) ; 00170 vSynchronise ( Mode_Synchro , MONTE ) ; 00171 pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RD ) ; 00172 pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RG ) ; 00173 Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ; 00174 } 00175 00176 BTN_Arret = 0 ; 00177 BTN_Descend = 0 ; 00178 } 00179 else if ( BTN_Descend ) 00180 { 00181 if ( ( Hauteur_P <= Consigne_Basse_P ) 00182 ||( Hauteur_RD <= Consigne_Basse_P ) 00183 ||( Hauteur_RG <= Consigne_Basse_P ) ) 00184 {// Si on est en bas 00185 BTN_Descend = 0 ; 00186 pRive_Droite->Arret() ; 00187 pRive_Gauche->Arret() ; 00188 PC3.printf("\r\n En Bas") ; 00189 } 00190 else 00191 {// Descente du pont 00192 PC3.printf("\t Descend Auto") ; 00193 PC3.printf("\t Descend Auto mode: %i", Mode_Synchro) ; 00194 vSynchronise ( Mode_Synchro , DESCEND ) ; 00195 Consigne_Vitesse_RD = Consigne_Vitesse_Auto ; 00196 Consigne_Vitesse_RG = Consigne_Vitesse_Auto ; 00197 00198 pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RD ) ; 00199 pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RG ) ; 00200 Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ; 00201 } 00202 00203 BTN_Arret = 0 ; 00204 BTN_Monte = 0 ; 00205 } 00206 00207 else 00208 { 00209 pRive_Droite->Arret() ; 00210 pRive_Gauche->Arret() ; 00211 Consigne_aout.write ( 0.0 ) ; 00212 } 00213 } 00214 } 00215 else if ( Mode_Fonctionnement == MODE_PARAMETRAGE ) 00216 {// En mode parametrage, 00217 //PC3.printf("\r\n No Param = %i " , No_Parametre) ; 00218 00219 if ( Chrono_Param.read_ms() > PERIODE_MAJ_PARAM ) 00220 { 00221 Chrono_Param.reset () ; 00222 // Vérifie si le dernier parametre lu est a jour 00223 if ( *(pParametres_Console_RG + No_Parametre ) != (S16) ( pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) ) 00224 {// Sinon, on l'écrit dans le variateur 00225 PC3.printf("\r\n No Param RG= %i - %i " , *(pParametres_Console_RG + No_Parametre ) , pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) ; 00226 pRive_Gauche->Configure ( No_Parametre , (S32) *(pParametres_Console_RG + No_Parametre ) ) ; 00227 } 00228 // Vérifie si le dernier parametre lu est a jour 00229 if ( *(pParametres_Console_RD + No_Parametre ) != (S16) ( pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) ) 00230 {// Sinon, on l'écrit dans le variateur 00231 PC3.printf("\r\n No Param RD= %i - %i " , *(pParametres_Console_RD + No_Parametre ) , pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) ; 00232 pRive_Droite->Configure ( No_Parametre , (S32) *(pParametres_Console_RD + No_Parametre ) ) ; 00233 } 00234 No_Parametre++ ; 00235 if ( No_Parametre >= NB_PARAM_VARIATEUR ) 00236 { 00237 No_Parametre = 0 ; 00238 pRive_Droite->Arret() ; 00239 pRive_Gauche->Arret() ; 00240 Consigne_aout.write ( 0.0 ) ; 00241 } 00242 // Lecture du parametre suivant 00243 pRive_Gauche->Lecture ( No_Parametre ) ; 00244 pRive_Droite->Lecture ( No_Parametre ) ; 00245 00246 } 00247 00248 } 00249 else if ( Mode_Fonctionnement == MODE_ETALONNAGE ) 00250 {// Etalonnage 00251 if ( BTN_Etalonnage ) 00252 { 00253 pRive_Droite->Etalonnage (Hauteur_RD1_pts , Hauteur_RD1_mm , Hauteur_RD2_pts , Hauteur_RD2_mm ) ; 00254 pRive_Gauche->Etalonnage (Hauteur_RG1_pts , Hauteur_RG1_mm , Hauteur_RG2_pts , Hauteur_RG2_mm ) ; 00255 BTN_Etalonnage = 0 ; 00256 wait(0.1) ; 00257 } 00258 pRive_Droite->Arret() ; 00259 pRive_Gauche->Arret() ; 00260 } 00261 00262 else if ( Mode_Fonctionnement == MODE_ALIGNEMENT ) 00263 { 00264 if ( BTN_Aligne) 00265 {// Alignement des hauteurs de pile pour le mode automatique 00266 if ( abs( Hauteur_RD - Hauteur_RG ) < Defaut_Mineur_Synchro / 2 ) 00267 {// Si l'ecart de hauteur est faible, on arrête tout 00268 BTN_Aligne = 0 ; 00269 pRive_Droite->Arret() ; 00270 pRive_Gauche->Arret() ; 00271 PC3.printf("\r\n Aligne") ; 00272 } 00273 else if ( Hauteur_RD > Hauteur_RG ) 00274 {// La pile droite est plus haute 00275 if ( Hauteur_RD > Consigne_Basse_P ) 00276 {// On descend la pile droite 00277 Consigne_Vitesse_RD = Consigne_Vitesse_Manu ; 00278 pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RG , Consigne_Vitesse_RD ) ; 00279 } 00280 else 00281 { 00282 pRive_Droite->Arret() ; 00283 } 00284 if ( Hauteur_RG < Consigne_Haute_P ) 00285 {// On monte la pile gauche 00286 Consigne_Vitesse_RG = Consigne_Vitesse_Manu ; 00287 pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RD , Consigne_Vitesse_RG ) ; 00288 } 00289 else 00290 { 00291 pRive_Gauche->Arret() ; 00292 } 00293 } 00294 else if ( Hauteur_RD < Hauteur_RG ) 00295 {// La pile gauche est plus haute 00296 if ( Hauteur_RD < Consigne_Haute_P ) 00297 {// On monte la pile droite 00298 Consigne_Vitesse_RD = Consigne_Vitesse_Manu ; 00299 pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RG , Consigne_Vitesse_RD ) ; 00300 } 00301 else 00302 { 00303 pRive_Droite->Arret() ; 00304 } 00305 if ( Hauteur_RG > Consigne_Basse_P ) 00306 {// On descend la pile gauche 00307 Consigne_Vitesse_RG = Consigne_Vitesse_Manu ; 00308 pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RD , Consigne_Vitesse_RG ) ; 00309 00310 } 00311 else 00312 { 00313 pRive_Gauche->Arret() ; 00314 } 00315 } 00316 } 00317 else 00318 { 00319 pRive_Droite->Arret() ; 00320 pRive_Gauche->Arret() ; 00321 } 00322 } 00323 else 00324 // Si le mode est inconnu, mode arret! 00325 { 00326 Mode_Fonctionnement = MODE_ARRET ; 00327 pRive_Droite->Arret() ; 00328 pRive_Gauche->Arret() ; 00329 BTN_Arret = 1 ; 00330 BTN_Monte = 0 ; 00331 BTN_Descend = 0 ; 00332 Consigne_aout.write ( 0.0 ) ; 00333 PC3.printf("\r\n Mode Inconnu") ; 00334 } 00335 }
Generated on Tue Jul 12 2022 23:24:06 by
1.7.2