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_Synchro )
00044                 {
00045                 PC3.printf("\t Monte RD") ;
00046                 pRive_Droite->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu  ) ;             
00047                 }
00048             BTN_Descend_RD = 0 ;
00049             }
00050         else if ( BTN_Descend_RD )             
00051             {// Descente rive droite
00052             if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Synchro )
00053                 {
00054                 PC3.printf("\t Descend RD") ;
00055                 pRive_Droite->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu  ) ;         
00056                 }
00057             BTN_Monte_RD = 0 ;
00058             }
00059         else            
00060             {// Arrêt rive droite
00061             pRive_Droite->Arret() ;
00062             BTN_Descend_RD = 0 ;
00063             BTN_Monte_RD = 0 ;
00064             //PC3.printf("\r\n Arret RD") ;
00065             }
00066         if ( BTN_Monte_RG )
00067             {// Montée rive gauche
00068             if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Synchro )
00069                 {
00070                 PC3.printf("\t Monte RG") ;
00071                 pRive_Gauche->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu  ) ;
00072                 Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ;
00073                 }
00074             BTN_Descend_RG = 0 ;
00075             }
00076         else if ( BTN_Descend_RG )
00077             {// Descente rive gauche
00078             if ( ( Hauteur_RD - Hauteur_RG ) < Defaut_Critique_Synchro )
00079                 {
00080                 PC3.printf("\t Descend RG") ;
00081                 pRive_Gauche->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu  ) ;
00082                 Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ;
00083                 }
00084             BTN_Monte_RG = 0 ;
00085             }
00086         else
00087             {// Arret rive gauche
00088             pRive_Gauche->Arret() ;
00089             BTN_Monte_RG = 0 ;
00090             BTN_Descend_RG = 0 ;
00091             Consigne_aout.write ( 0.0 ) ;
00092             //PC3.printf("\r\n Arret RG") ;
00093             }
00094         if ( BTN_Frein )  
00095             {// Liberation des freins
00096             if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD )
00097                 {
00098                 if (pRive_Gauche->Etat_Frein == SERRE ) pRive_Gauche->Frein( DESSERRE ) ;
00099                 if (pRive_Droite->Etat_Frein == SERRE ) pRive_Droite->Frein( DESSERRE ) ;
00100                 }                            
00101             }
00102         else              
00103             {// Serrage des freins
00104             if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD )
00105                 {
00106                 if (pRive_Gauche->Etat_Frein == DESSERRE ) pRive_Gauche->Frein( SERRE ) ;
00107                 if (pRive_Droite->Etat_Frein == DESSERRE ) pRive_Droite->Frein( SERRE ) ;
00108                 }                  
00109             }
00110         }
00111         
00112     else if ( Mode_Fonctionnement == MODE_AUTOMATIQUE )
00113         {// En mode automatique, les rives sont synchronisées, avec consigne de vitesse et hauteur cible                     
00114         if ( BTN_Arret )      
00115             {// Arret du pont
00116             BTN_Monte = 0 ;
00117             BTN_Descend = 0 ;
00118             BTN_Arret = 0 ;
00119             pRive_Droite->Arret() ;
00120             pRive_Gauche->Arret() ;
00121             Consigne_aout.write ( 0.0 ) ;   
00122             
00123             vSynchro_Initialise ( ) ;
00124                
00125             //PC3.printf("\r\n Btn Arret") ;    
00126             }
00127          else
00128             {// Vérification de l'ecart de synchronisation: différence de hauteur entre droite et gauche
00129             if ( abs( Hauteur_RD - Hauteur_RG ) > Defaut_Critique_Synchro )
00130                 {// Si l'ecart depasse le seuil critique: on arrete tout!
00131                 BTN_Arret = 0 ;
00132                 BTN_Descend = 0 ;
00133                 BTN_Monte = 0 ;
00134                 pRive_Droite->Arret() ;
00135                 pRive_Gauche->Arret() ;
00136                 Consigne_aout.write ( 0.0 ) ;
00137                 PC3.printf("\r\n Defaut Critique") ; 
00138                 }
00139             else if ( BTN_Monte )
00140                 {// Si on est en haut
00141                 if (  ( Hauteur_P >= Consigne_Haute_P )
00142                     ||( Hauteur_RD >= Consigne_Haute_P )
00143                     ||( Hauteur_RG >= Consigne_Haute_P ) )
00144                     {
00145                     BTN_Monte = 0 ;
00146                     pRive_Droite->Arret() ;
00147                     pRive_Gauche->Arret() ;
00148                     PC3.printf("\r\n En Haut") ; 
00149                     }
00150                 else
00151                     {// Montée
00152                     PC3.printf("\t Monte Auto mode: %i", Mode_Synchro) ;
00153                     vSynchronise ( Mode_Synchro , MONTE ) ;                                             
00154                     pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RD ) ;
00155                     pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RG ) ;
00156                     Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ;
00157                     }
00158                 
00159                 BTN_Arret = 0 ;
00160                 BTN_Descend = 0 ;
00161                 }
00162             else if ( BTN_Descend )
00163                 {
00164                 if (  ( Hauteur_P <= Consigne_Basse_P )
00165                     ||( Hauteur_RD <= Consigne_Basse_P )
00166                     ||( Hauteur_RG <= Consigne_Basse_P ) )
00167                     {// Si on est en bas
00168                     BTN_Descend = 0 ;
00169                     pRive_Droite->Arret() ;
00170                     pRive_Gauche->Arret() ;
00171                     PC3.printf("\r\n En Bas") ; 
00172                     }
00173                 else
00174                     {// Descente du pont
00175                     PC3.printf("\t Descend Auto") ;
00176                     PC3.printf("\t Descend Auto mode: %i", Mode_Synchro) ;
00177                     vSynchronise ( Mode_Synchro , DESCEND ) ;
00178                     Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
00179                     Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
00180                     
00181                     pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RD ) ;
00182                     pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RG ) ;
00183                     Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ;
00184                     }
00185 
00186                 BTN_Arret = 0 ;
00187                 BTN_Monte = 0 ;
00188                 }
00189             
00190             else
00191                 {
00192                 pRive_Droite->Arret() ;
00193                 pRive_Gauche->Arret() ;
00194                 Consigne_aout.write ( 0.0 ) ;
00195                 }   
00196             }
00197         }
00198     else if ( Mode_Fonctionnement == MODE_PARAMETRAGE )
00199         {// En mode parametrage, 
00200         //PC3.printf("\r\n No Param = %i " , No_Parametre) ; 
00201         
00202         if ( Chrono_Param.read_ms() > PERIODE_MAJ_PARAM )
00203             {
00204             Chrono_Param.reset () ;
00205             // Vérifie si le dernier parametre lu est a jour
00206             if ( *(pParametres_Console_RG + No_Parametre ) != (S16) ( pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) )
00207                 {// Sinon, on l'écrit dans le variateur
00208                 PC3.printf("\r\n No Param RG= %i - %i " , *(pParametres_Console_RG + No_Parametre ) , pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) ; 
00209                 pRive_Gauche->Configure ( No_Parametre , (S32) *(pParametres_Console_RG + No_Parametre ) ) ;
00210                 }
00211             // Vérifie si le dernier parametre lu est a jour
00212             if ( *(pParametres_Console_RD + No_Parametre ) != (S16) ( pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) )
00213                 {// Sinon, on l'écrit dans le variateur
00214                 PC3.printf("\r\n No Param RD= %i - %i " , *(pParametres_Console_RD + No_Parametre ) , pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) ;
00215                 pRive_Droite->Configure ( No_Parametre , (S32) *(pParametres_Console_RD + No_Parametre ) ) ;
00216                 }
00217             No_Parametre++ ;
00218             if ( No_Parametre >= NB_PARAM_VARIATEUR )
00219                 {
00220                 No_Parametre = 0 ;
00221                 }
00222             // Lecture du parametre suivant
00223             pRive_Gauche->Lecture ( No_Parametre ) ;
00224             pRive_Droite->Lecture ( No_Parametre ) ;
00225             
00226             }                   
00227 
00228         }
00229     else if ( Mode_Fonctionnement == MODE_ETALONNAGE )
00230         {// Etalonnage
00231         if ( BTN_Etalonnage )
00232             {
00233             pRive_Droite->Etalonnage (Hauteur_RD1_pts , Hauteur_RD1_mm , Hauteur_RD2_pts , Hauteur_RD2_mm ) ;
00234             pRive_Gauche->Etalonnage (Hauteur_RG1_pts , Hauteur_RG1_mm , Hauteur_RG2_pts , Hauteur_RG2_mm ) ;
00235             BTN_Etalonnage = 0 ;
00236             wait(0.1) ;
00237             } 
00238         pRive_Droite->Arret() ;
00239         pRive_Gauche->Arret() ;                   
00240         }
00241     
00242     else if ( Mode_Fonctionnement == MODE_ALIGNEMENT )
00243         {
00244         if ( BTN_Aligne)
00245             {// Alignement des hauteurs de pile pour le mode automatique
00246             if ( abs( Hauteur_RD - Hauteur_RG ) < Defaut_Mineur_Synchro / 2 )
00247                 {// Si l'ecart de hauteur est faible, on arrête tout
00248                 BTN_Aligne = 0 ;
00249                 pRive_Droite->Arret() ;
00250                 pRive_Gauche->Arret() ;
00251                 PC3.printf("\r\n Aligne") ; 
00252                 }
00253             else if ( Hauteur_RD > Hauteur_RG )
00254                 {// La pile droite est plus haute
00255                 if ( Hauteur_RD > Consigne_Basse_P )               
00256                     {// On descend la pile droite                      
00257                     Consigne_Vitesse_RD = Consigne_Vitesse_Manu ;                      
00258                     pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RG , Consigne_Vitesse_RD ) ;
00259                     }
00260                 else
00261                     {
00262                     pRive_Droite->Arret() ;
00263                     }
00264                 if ( Hauteur_RG < Consigne_Haute_P )
00265                     {// On monte la pile gauche                      
00266                     Consigne_Vitesse_RG = Consigne_Vitesse_Manu ;                      
00267                     pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RD , Consigne_Vitesse_RG ) ;
00268                     }
00269                 else
00270                     {
00271                     pRive_Gauche->Arret() ;
00272                     }
00273                 }      
00274             else if ( Hauteur_RD < Hauteur_RG )
00275                 {// La pile gauche est plus haute
00276                 if ( Hauteur_RD < Consigne_Haute_P )                
00277                     {// On monte la pile droite                      
00278                     Consigne_Vitesse_RD = Consigne_Vitesse_Manu ;                      
00279                     pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RG , Consigne_Vitesse_RD ) ;                    
00280                     }
00281                 else
00282                     {
00283                     pRive_Droite->Arret() ;
00284                     }
00285                 if ( Hauteur_RG > Consigne_Basse_P )
00286                     {// On descend la pile gauche                      
00287                     Consigne_Vitesse_RG = Consigne_Vitesse_Manu ;                      
00288                     pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RD , Consigne_Vitesse_RG ) ;
00289                     
00290                     }
00291                 else
00292                     {
00293                     pRive_Gauche->Arret() ;
00294                     }
00295                 }
00296             }
00297         else
00298             {
00299             pRive_Droite->Arret() ;
00300             pRive_Gauche->Arret() ;
00301             }
00302         }
00303     else
00304         // Si le mode est inconnu, mode arret!
00305         {
00306         Mode_Fonctionnement = MODE_ARRET ;
00307         pRive_Droite->Arret() ;
00308         pRive_Gauche->Arret() ;
00309         BTN_Arret = 1 ;
00310         BTN_Monte = 0 ;
00311         BTN_Descend = 0 ;
00312         Consigne_aout.write ( 0.0 ) ;
00313         PC3.printf("\r\n Mode Inconnu") ; 
00314         }
00315     }