Diff: Gemma.cpp

Revision:

0:8b3c6f593515

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gemma.cpp	Thu Jun 22 09:33:04 2017 +0000
@@ -0,0 +1,315 @@
+//******************************************************************
+//
+//      Gemma
+//
+//  Logiciel de gestion des modes de marche et d'arret du Pont Bacalan
+//
+// 
+//
+//********************************************************************
+
+#include "Gemma.h"
+
+Serial PC3(USBTX, USBRX) ;
+// Chronomètre
+Timer   Chrono_Param ;
+AnalogOut   Consigne_aout (p18) ;
+
+U8  No_Parametre ;
+S16  *pParametres_Console_RG ;
+S16  *pParametres_Console_RD ;
+
+void    vGemma_Init () 
+    {
+    Chrono_Param.start () ;
+    pParametres_Console_RG = &Param_Version_RG ;
+    pParametres_Console_RD = &Param_Version_RD ;
+    No_Parametre = 0 ;
+    Consigne_aout.write ( 0.0 ) ;
+    }
+
+void    vGemma ( Pilote* pRive_Droite , Pilote* pRive_Gauche )
+    {
+    // Fonctionnement des differents modes
+    PC3.printf("\r\n Mode: %i", Mode_Fonctionnement) ;
+    if ( Mode_Fonctionnement == MODE_MANUEL )
+        // En mode manuel, pas d'action maintenue, les 2 rives sont indépendantes
+        {
+        //Raz des parametres de synchronisation
+        vSynchro_Initialise ( ) ;
+        
+        if ( BTN_Monte_RD )  
+            {// Montée rive droite
+            if ( ( Hauteur_RD - Hauteur_RG ) < Defaut_Critique_Synchro )
+                {
+                PC3.printf("\t Monte RD") ;
+                pRive_Droite->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu  ) ;             
+                }
+            BTN_Descend_RD = 0 ;
+            }
+        else if ( BTN_Descend_RD )             
+            {// Descente rive droite
+            if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Synchro )
+                {
+                PC3.printf("\t Descend RD") ;
+                pRive_Droite->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu  ) ;         
+                }
+            BTN_Monte_RD = 0 ;
+            }
+        else            
+            {// Arrêt rive droite
+            pRive_Droite->Arret() ;
+            BTN_Descend_RD = 0 ;
+            BTN_Monte_RD = 0 ;
+            //PC3.printf("\r\n Arret RD") ;
+            }
+        if ( BTN_Monte_RG )
+            {// Montée rive gauche
+            if ( ( Hauteur_RG - Hauteur_RD ) < Defaut_Critique_Synchro )
+                {
+                PC3.printf("\t Monte RG") ;
+                pRive_Gauche->Marche ( MODE_MANUEL, MONTE, Hauteur_maxi , Consigne_Vitesse_Manu  ) ;
+                Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ;
+                }
+            BTN_Descend_RG = 0 ;
+            }
+        else if ( BTN_Descend_RG )
+            {// Descente rive gauche
+            if ( ( Hauteur_RD - Hauteur_RG ) < Defaut_Critique_Synchro )
+                {
+                PC3.printf("\t Descend RG") ;
+                pRive_Gauche->Marche ( MODE_MANUEL, DESCEND, Hauteur_mini , Consigne_Vitesse_Manu  ) ;
+                Consigne_aout.write ( (F32) Consigne_Vitesse_Manu / V_MAX ) ;
+                }
+            BTN_Monte_RG = 0 ;
+            }
+        else
+            {// Arret rive gauche
+            pRive_Gauche->Arret() ;
+            BTN_Monte_RG = 0 ;
+            BTN_Descend_RG = 0 ;
+            Consigne_aout.write ( 0.0 ) ;
+            //PC3.printf("\r\n Arret RG") ;
+            }
+        if ( BTN_Frein )  
+            {// Liberation des freins
+            if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD )
+                {
+                if (pRive_Gauche->Etat_Frein == SERRE ) pRive_Gauche->Frein( DESSERRE ) ;
+                if (pRive_Droite->Etat_Frein == SERRE ) pRive_Droite->Frein( DESSERRE ) ;
+                }                            
+            }
+        else              
+            {// Serrage des freins
+            if ( !BTN_Monte_RG && !BTN_Descend_RG && !!BTN_Monte_RD && !BTN_Descend_RD )
+                {
+                if (pRive_Gauche->Etat_Frein == DESSERRE ) pRive_Gauche->Frein( SERRE ) ;
+                if (pRive_Droite->Etat_Frein == DESSERRE ) pRive_Droite->Frein( SERRE ) ;
+                }                  
+            }
+        }
+        
+    else if ( Mode_Fonctionnement == MODE_AUTOMATIQUE )
+        {// En mode automatique, les rives sont synchronisées, avec consigne de vitesse et hauteur cible                     
+        if ( BTN_Arret )      
+            {// Arret du pont
+            BTN_Monte = 0 ;
+            BTN_Descend = 0 ;
+            BTN_Arret = 0 ;
+            pRive_Droite->Arret() ;
+            pRive_Gauche->Arret() ;
+            Consigne_aout.write ( 0.0 ) ;   
+            
+            vSynchro_Initialise ( ) ;
+               
+            //PC3.printf("\r\n Btn Arret") ;    
+            }
+         else
+            {// Vérification de l'ecart de synchronisation: différence de hauteur entre droite et gauche
+            if ( abs( Hauteur_RD - Hauteur_RG ) > Defaut_Critique_Synchro )
+                {// Si l'ecart depasse le seuil critique: on arrete tout!
+                BTN_Arret = 0 ;
+                BTN_Descend = 0 ;
+                BTN_Monte = 0 ;
+                pRive_Droite->Arret() ;
+                pRive_Gauche->Arret() ;
+                Consigne_aout.write ( 0.0 ) ;
+                PC3.printf("\r\n Defaut Critique") ; 
+                }
+            else if ( BTN_Monte )
+                {// Si on est en haut
+                if (  ( Hauteur_P >= Consigne_Haute_P )
+                    ||( Hauteur_RD >= Consigne_Haute_P )
+                    ||( Hauteur_RG >= Consigne_Haute_P ) )
+                    {
+                    BTN_Monte = 0 ;
+                    pRive_Droite->Arret() ;
+                    pRive_Gauche->Arret() ;
+                    PC3.printf("\r\n En Haut") ; 
+                    }
+                else
+                    {// Montée
+                    PC3.printf("\t Monte Auto mode: %i", Mode_Synchro) ;
+                    vSynchronise ( Mode_Synchro , MONTE ) ;                                             
+                    pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RD ) ;
+                    pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Consigne_Haute_P , Consigne_Vitesse_RG ) ;
+                    Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ;
+                    }
+                
+                BTN_Arret = 0 ;
+                BTN_Descend = 0 ;
+                }
+            else if ( BTN_Descend )
+                {
+                if (  ( Hauteur_P <= Consigne_Basse_P )
+                    ||( Hauteur_RD <= Consigne_Basse_P )
+                    ||( Hauteur_RG <= Consigne_Basse_P ) )
+                    {// Si on est en bas
+                    BTN_Descend = 0 ;
+                    pRive_Droite->Arret() ;
+                    pRive_Gauche->Arret() ;
+                    PC3.printf("\r\n En Bas") ; 
+                    }
+                else
+                    {// Descente du pont
+                    PC3.printf("\t Descend Auto") ;
+                    PC3.printf("\t Descend Auto mode: %i", Mode_Synchro) ;
+                    vSynchronise ( Mode_Synchro , DESCEND ) ;
+                    Consigne_Vitesse_RD = Consigne_Vitesse_Auto ;
+                    Consigne_Vitesse_RG = Consigne_Vitesse_Auto ;
+                    
+                    pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RD ) ;
+                    pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Consigne_Basse_P , Consigne_Vitesse_RG ) ;
+                    Consigne_aout.write ( (F32) Consigne_Vitesse_RG / V_MAX ) ;
+                    }
+
+                BTN_Arret = 0 ;
+                BTN_Monte = 0 ;
+                }
+            
+            else
+                {
+                pRive_Droite->Arret() ;
+                pRive_Gauche->Arret() ;
+                Consigne_aout.write ( 0.0 ) ;
+                }   
+            }
+        }
+    else if ( Mode_Fonctionnement == MODE_PARAMETRAGE )
+        {// En mode parametrage, 
+        //PC3.printf("\r\n No Param = %i " , No_Parametre) ; 
+        
+        if ( Chrono_Param.read_ms() > PERIODE_MAJ_PARAM )
+            {
+            Chrono_Param.reset () ;
+            // Vérifie si le dernier parametre lu est a jour
+            if ( *(pParametres_Console_RG + No_Parametre ) != (S16) ( pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) )
+                {// Sinon, on l'écrit dans le variateur
+                PC3.printf("\r\n No Param RG= %i - %i " , *(pParametres_Console_RG + No_Parametre ) , pRive_Gauche->Parametres_Vario_S32 [ No_Parametre ] ) ; 
+                pRive_Gauche->Configure ( No_Parametre , (S32) *(pParametres_Console_RG + No_Parametre ) ) ;
+                }
+            // Vérifie si le dernier parametre lu est a jour
+            if ( *(pParametres_Console_RD + No_Parametre ) != (S16) ( pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) )
+                {// Sinon, on l'écrit dans le variateur
+                PC3.printf("\r\n No Param RD= %i - %i " , *(pParametres_Console_RD + No_Parametre ) , pRive_Droite->Parametres_Vario_S32 [ No_Parametre ] ) ;
+                pRive_Droite->Configure ( No_Parametre , (S32) *(pParametres_Console_RD + No_Parametre ) ) ;
+                }
+            No_Parametre++ ;
+            if ( No_Parametre >= NB_PARAM_VARIATEUR )
+                {
+                No_Parametre = 0 ;
+                }
+            // Lecture du parametre suivant
+            pRive_Gauche->Lecture ( No_Parametre ) ;
+            pRive_Droite->Lecture ( No_Parametre ) ;
+            
+            }                   
+
+        }
+    else if ( Mode_Fonctionnement == MODE_ETALONNAGE )
+        {// Etalonnage
+        if ( BTN_Etalonnage )
+            {
+            pRive_Droite->Etalonnage (Hauteur_RD1_pts , Hauteur_RD1_mm , Hauteur_RD2_pts , Hauteur_RD2_mm ) ;
+            pRive_Gauche->Etalonnage (Hauteur_RG1_pts , Hauteur_RG1_mm , Hauteur_RG2_pts , Hauteur_RG2_mm ) ;
+            BTN_Etalonnage = 0 ;
+            wait(0.1) ;
+            } 
+        pRive_Droite->Arret() ;
+        pRive_Gauche->Arret() ;                   
+        }
+    
+    else if ( Mode_Fonctionnement == MODE_ALIGNEMENT )
+        {
+        if ( BTN_Aligne)
+            {// Alignement des hauteurs de pile pour le mode automatique
+            if ( abs( Hauteur_RD - Hauteur_RG ) < Defaut_Mineur_Synchro / 2 )
+                {// Si l'ecart de hauteur est faible, on arrête tout
+                BTN_Aligne = 0 ;
+                pRive_Droite->Arret() ;
+                pRive_Gauche->Arret() ;
+                PC3.printf("\r\n Aligne") ; 
+                }
+            else if ( Hauteur_RD > Hauteur_RG )
+                {// La pile droite est plus haute
+                if ( Hauteur_RD > Consigne_Basse_P )               
+                    {// On descend la pile droite                      
+                    Consigne_Vitesse_RD = Consigne_Vitesse_Manu ;                      
+                    pRive_Droite->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RG , Consigne_Vitesse_RD ) ;
+                    }
+                else
+                    {
+                    pRive_Droite->Arret() ;
+                    }
+                if ( Hauteur_RG < Consigne_Haute_P )
+                    {// On monte la pile gauche                      
+                    Consigne_Vitesse_RG = Consigne_Vitesse_Manu ;                      
+                    pRive_Gauche->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RD , Consigne_Vitesse_RG ) ;
+                    }
+                else
+                    {
+                    pRive_Gauche->Arret() ;
+                    }
+                }      
+            else if ( Hauteur_RD < Hauteur_RG )
+                {// La pile gauche est plus haute
+                if ( Hauteur_RD < Consigne_Haute_P )                
+                    {// On monte la pile droite                      
+                    Consigne_Vitesse_RD = Consigne_Vitesse_Manu ;                      
+                    pRive_Droite->Marche ( MODE_AUTOMATIQUE, MONTE, Hauteur_RG , Consigne_Vitesse_RD ) ;                    
+                    }
+                else
+                    {
+                    pRive_Droite->Arret() ;
+                    }
+                if ( Hauteur_RG > Consigne_Basse_P )
+                    {// On descend la pile gauche                      
+                    Consigne_Vitesse_RG = Consigne_Vitesse_Manu ;                      
+                    pRive_Gauche->Marche ( MODE_AUTOMATIQUE, DESCEND, Hauteur_RD , Consigne_Vitesse_RG ) ;
+                    
+                    }
+                else
+                    {
+                    pRive_Gauche->Arret() ;
+                    }
+                }
+            }
+        else
+            {
+            pRive_Droite->Arret() ;
+            pRive_Gauche->Arret() ;
+            }
+        }
+    else
+        // Si le mode est inconnu, mode arret!
+        {
+        Mode_Fonctionnement = MODE_ARRET ;
+        pRive_Droite->Arret() ;
+        pRive_Gauche->Arret() ;
+        BTN_Arret = 1 ;
+        BTN_Monte = 0 ;
+        BTN_Descend = 0 ;
+        Consigne_aout.write ( 0.0 ) ;
+        PC3.printf("\r\n Mode Inconnu") ; 
+        }
+    }
\ No newline at end of file