File content as of revision 0:8b3c6f593515:

/*******************************************************************/
/*                                                                 */
/*                 Pilote                                          */
/*                                                                 */
/*  Objet de pilotage de variateur                                 */
/*                                                                 */
/*                                                                 */
/*******************************************************************/


#include "Pilote.h"

Serial PC2(USBTX, USBRX) ;
/** Class Pilote
*
*/

    Pilote::Pilote ( S16 Id )
        {
        Hauteur_Cible = 0 ;
        Etat_Deplacement = ARRET ;
        Chaine_A_Emettre[0] = 0 ;
        pReception = &Chaine_Recue[0] ;
        Nb_Caracteres_Recus = 0 ;
        Statut_Ordre_En_Cours = AUCUN ;
        
        Parametres_Vario_S32 [ VERSION ] = 100 ;
        Parametres_Vario_S32 [ STARTUP ] = 500 ;
        Parametres_Vario_S32 [ INCREMENT ] = 20 ;
        Parametres_Vario_S32 [ SEUIL_DEMARRAGE ] = 10 ;
        Parametres_Vario_S32 [ ACCELERATION ] = 20 ;
        Parametres_Vario_S32 [ DECELERATION ] = 20 ;
        Parametres_Vario_S32 [ KPV ] = 1000 ;
        Parametres_Vario_S32 [ KIV ] = 100 ;
        Parametres_Vario_S32 [ KDV ] = 0 ;
        Parametres_Vario_S32 [ KA ] = 1000 ;
        Parametres_Vario_S32 [ CONSIGNE ] = 1500 ;
        
        };


/*******************************************************************/ 
/*                                                                 */
/*                 Init                                            */
/*                                                                 */
/*  Initialisation de l'objet de pilotage de variateur             */
/*                                                                 */
/*                                                                 */
/*******************************************************************/  
    void    Pilote::Init ( int Baudrates , int bits , int Stop )
        {
        U8  Index ;
        // Initialisation du port série
        pPort->baud ( Baudrates );
        pPort->format ( bits , Serial::None , Stop ) ;
        
        while ( pPort->readable() )
        {
        // Réception des caractères pour vider le buffer
        Index = pPort->getc() ;
        }
        // RAZ des chaines recues et à emettre
        for ( Index=0 ; Index < LONGUEUR_CHAINE_EMISSION ; Index++ )
            {
            Chaine_A_Emettre[Index] = 0 ;
            }
        for ( Index=0 ; Index < LONGUEUR_CHAINE_RECEPTION ; Index++ )
            {
            Chaine_Recue[Index] = 0 ;
            }
         
        Chrono_Pilote.start() ;   
        Chrono_Arret.start() ;
        Debut_Emission_ms = Chrono_Pilote.read_ms () ;
        COM_OK = TRUE ;
        Compteur_Timeout = 0 ;
        Dernier_Ordre_Confirme = ORDRE_ARRET ;
        Etat_Deplacement = ARRET ;
        
        }
        
/*******************************************************************/ 
/*                                                                 */
/*                 Marche                                          */
/*                                                                 */
/*        Ordre de marche automatique                              */
/*                                                                 */
/*                                                                 */
/*******************************************************************/ 
    void    Pilote::Marche ( U8 Mode, U8 Sens, S16 Hauteur , S16 Consigne_Vitesse )
        {
        F32 Valeur_F32 ;
        
        // Construction de la trame de marche    
        Chaine_A_Emettre[0] = 0x02 ;
        
        if ( Sens == MONTE )
            {
            Chaine_A_Emettre [1] = ORDRE_MONTE ; // M
            Etat_Deplacement = MONTE ;
            }
        else
            {
            Chaine_A_Emettre [1] = ORDRE_DESCEND ; // D
            Etat_Deplacement = DESCEND ;
            }
        
        if ( Consigne_Vitesse > Vitesse_maxi )
            {
            Consigne_Vitesse = Vitesse_maxi ;
            }
        else if ( Consigne_Vitesse < 0 )
            {
            Consigne_Vitesse = 0 ;
            }
        Chaine_A_Emettre [2] = Consigne_Vitesse / 256 ; // Poids fort
        Chaine_A_Emettre [3] = Consigne_Vitesse % 256 ; // Poids faible
        
        if ( Hauteur > Hauteur_maxi )
            {
            Hauteur = Hauteur_maxi ;
            }
        else if ( Hauteur < Hauteur_mini )
            {
            Hauteur = Hauteur_mini ;
            }
            
        Hauteur_Cible = Hauteur ;
        
        //Calcul de la mesure à atteindre
        Valeur_F32 = ( (F32) Hauteur - MM_Offset ) / MM_par_Points ;
        
        Chaine_A_Emettre [4] = (S16)Valeur_F32 / 256 ; // Poids fort
        Chaine_A_Emettre [5] = (S16)Valeur_F32 % 256 ; // Poids faible
        
        Chaine_A_Emettre [6] = 0x03 ;
        Chaine_A_Emettre [7] = 0x00 ;
        
        if( Statut_Ordre_En_Cours != ATTENTE )
            {
            Ordre_En_Cours = Chaine_A_Emettre [1] ;
            Emission ( &Chaine_A_Emettre [0] , 7 ) ;
            Statut_Ordre_En_Cours = ATTENTE ;
            
            }
        }

/*******************************************************************/ 
/*                                                                 */
/*                 Arret                                           */
/*                                                                 */
/*        Ordre d'arret                                            */
/*                                                                 */
/*                                                                 */
/*******************************************************************/ 
    void    Pilote::Arret (void)
        {
        Chaine_A_Emettre[0] = 0x02 ;
        Chaine_A_Emettre[1] = ORDRE_ARRET ;
        Chaine_A_Emettre[2] = 0x03 ;
        Chaine_A_Emettre[3] = 0x00 ;
       
        //PC2.printf("\r\n Chrono %i: %i ms COM: %i \r\n" ,Id, Chrono_Arret.read_ms() , COM_OK ) ;
        
        if ( ( Dernier_Ordre_Confirme == ORDRE_ARRET )
          &&  ( Chrono_Arret.read_ms() > DELAI_COMMANDE_ARRET_ms ) 
          ||  ( Ordre_En_Cours != ORDRE_ARRET ) 
          ||  ( Statut_Ordre_En_Cours == DEFAUT ))
            {
            Chrono_Arret.reset() ;
            Emission ( &Chaine_A_Emettre [0] , 3 ) ;
            Etat_Deplacement = ARRET ;
            Statut_Ordre_En_Cours = ATTENTE ;
            Ordre_En_Cours = Chaine_A_Emettre [1] ;
            }
        else 
            {
            //PC2.printf("\r\n Chrono %i: %i ms Ordre: %i \r\n" ,Id, Chrono_Arret.read_ms() , Ordre_En_Cours ) ;
            }
            
        }
/*******************************************************************/ 
/*                                                                 */
/*                 Etalonnage                                      */
/*                                                                 */
/*        Ordre d'étalonnage de hauteur                            */
/*                                                                 */
/*                                                                 */
/*******************************************************************/ 
   // Etalonnage de hauteur
   void    Pilote::Etalonnage (S16 Points1, S16 Hauteur1 , S16 Points2 , S16 Hauteur2 )
    {
    F32     Valeur_F32 ;
    
    Valeur_F32 = (F32) ( Points2 - Points1 ) ;
    
    if ( abs( Valeur_F32 ) > EPSILON )
        {
        MM_par_Points = (F32) ( Hauteur2 - Hauteur1 ) / Valeur_F32 ;
        }
    else
        {
        MM_par_Points = 0.0 ;
        }
        
    MM_Offset = (F32)Hauteur1 - MM_par_Points * (F32)Points1 ;
    
    //PC2.printf("\r\n MM par pts %i: %f \r\n" ,Id, MM_par_Points) ;
    //PC2.printf("\r\n Offset %i: %f \r\n" ,Id,MM_Offset) ;
    
    }
/*******************************************************************/ 
/*                                                                 */
/*                 Frein                                           */
/*                                                                 */
/*        Ordre d'activation du frein                              */
/*                                                                 */
/*                                                                 */
/*******************************************************************/    
    void    Pilote::Frein ( U8 Etat )
        {
        Chaine_A_Emettre[0] = 0x02 ;
        Chaine_A_Emettre[1] = ORDRE_FREIN ;
        Chaine_A_Emettre[2] = Etat ;
        Chaine_A_Emettre[3] = 0x03 ;
        Chaine_A_Emettre[4] = 0x00 ;
        
        if( Statut_Ordre_En_Cours != ATTENTE )
            {
            Ordre_En_Cours = Chaine_A_Emettre [1] ;
            Emission ( &Chaine_A_Emettre [0] , 4 ) ;
            Statut_Ordre_En_Cours = ATTENTE ;
            Etat_Frein = Etat ;
            }
        } 
/*******************************************************************/ 
/*                                                                 */
/*                 Lecture                                         */
/*                                                                 */
/*        Ordre de lecture d'un parametre de variateur             */
/*                                                                 */
/*                                                                 */
/*******************************************************************/        
    void    Pilote::Lecture ( U16 Num_Parametre ) 
        {
        Chaine_A_Emettre[0] = 0x02 ;
        Chaine_A_Emettre[1] = ORDRE_LECTURE ;
        Chaine_A_Emettre[2] = Num_Parametre ;
        Chaine_A_Emettre[3] = 0x03 ;
        Chaine_A_Emettre[4] = 0x00 ;
        
        if( Statut_Ordre_En_Cours != ATTENTE )
            {
            Ordre_En_Cours = Chaine_A_Emettre [1] ;
            Emission ( &Chaine_A_Emettre [0] , 4 ) ;
            Statut_Ordre_En_Cours = ATTENTE ;
            
            }
        }
/*******************************************************************/ 
/*                                                                 */
/*                 Configure                                       */
/*                                                                 */
/*        Ordre de configuration d'un parametre variateur          */
/*                                                                 */
/*                                                                 */
/*******************************************************************/ 
    void    Pilote::Configure ( U16 Num_Parametre , S32 Valeur_S32 ) 
        {
        
        Chaine_A_Emettre[0] = 0x02 ;
        Chaine_A_Emettre[1] = ORDRE_CONFIGURE ;
        Chaine_A_Emettre[2] = Num_Parametre ;
        Chaine_A_Emettre[6] = (U8)( Valeur_S32 & 0x000000FF) ;
        Valeur_S32 = Valeur_S32 >> 8 ;
        Chaine_A_Emettre[5] = (U8)( Valeur_S32 & 0x000000FF) ;
        Valeur_S32 = Valeur_S32 >> 8 ;
        Chaine_A_Emettre[4] = (U8)( Valeur_S32 & 0x000000FF) ;
        Valeur_S32 = Valeur_S32 >> 8 ;
        Chaine_A_Emettre[3] = (U8)( Valeur_S32 & 0x000000FF) ; 
        Chaine_A_Emettre[7] = 0x03 ;
        Chaine_A_Emettre[8] = 0x00 ;
        
        if( Statut_Ordre_En_Cours != ATTENTE )
            {
            Ordre_En_Cours = Chaine_A_Emettre [1] ;
            Emission ( &Chaine_A_Emettre [0] , 8 ) ;
            Statut_Ordre_En_Cours = ATTENTE ;
            
            }
        }     
/*******************************************************************/ 
/*                                                                 */
/*                 Emission                                        */
/*                                                                 */
/*        Emission d'une trame vers le variateur                   */
/*                                                                 */
/*                                                                 */
/*******************************************************************/ 
    void Pilote::Emission ( U8 *pChaine , U8 Longueur ) 
        {
        U8  Index = 0 ; 
    
        if ( pPort->writeable() )
            {
            // Mesure du timeout
            Debut_Emission_ms = Chrono_Pilote.read_ms() ;
        
            while ( Index < Longueur )
                {
                pPort->putc( *pChaine ) ;
                //PC2.printf("\r\n  %i: %i \r\n" ,Id, *pChaine) ;
                pChaine++ ;
                Index++ ;              
                } 
            }
        else
            {
            PC2.printf("\r\n  %i: Bloque \r\n" ,Id) ;
            }
            
        }
/*******************************************************************/ 
/*                                                                 */
/*                 Reception                                       */
/*                                                                 */
/*        Reception de la reponse du variateur                     */
/*                                                                 */
/*                                                                 */
/*******************************************************************/    
    void Pilote::Reception ( void ) 
        {
        U8  Index1 = 0 ;
        U8  Index2 = 0 ; 
        S16 Valeur_S16 ;
        S32 Valeur_S32 ;
        U8  *pChaine ;
        
        while ( pPort->readable() )
            {
            // Réception des caractères si il y en a
            *pReception = pPort->getc() ;
            //PC2.printf("\r\n %i",*pReception ) ;
            pReception++ ;
            Nb_Caracteres_Recus++ ;               
            }
        
        pChaine = &Chaine_Recue[0] ;
        
        // La plus petite trame comporte 3 caractères
        if ( Nb_Caracteres_Recus > 2 )
            {
            while ( *pChaine != 0x02 )
                {
                // Recherche d'un début de trame: STX = 0x02            
                if ( Index2 == Nb_Caracteres_Recus )
                    {
                    // Pas de début de trame, on ecrase les caractères reçus et on s'en va!
                    pReception = &Chaine_Recue[0] ;
                    Nb_Caracteres_Recus = 0 ;
                    //PC2.printf("\r\n Poubelle %i \r\n" ,Id) ;
                    return ;
                    }
                pChaine++ ;
                Index2++ ;
                }
            /**********************************************************************************/  
            // Ordre d'arret
            if ( *(pChaine+1) == ORDRE_ARRET )
                {
                //PC2.printf("\r\n Ordre d'Arret %i\r\n" ,Id) ;
                if ( Nb_Caracteres_Recus < ( Index2 + 4 ) )
                    {
                    // Trame incomplete, on reviendra
                    //PC2.printf("\r\n Arret incomplet %i \r\n" ,Id) ;
                    return ;
                    }
                // Vérifie le caractere de fin de trame
                if ( *(pChaine+4) == 0x03 )
                    {
                    // Trame OK, on efface
                    //PC2.printf("\r\n Arret OK %i\r\n" ,Id) ;
                    // On lit la mesure
                    Valeur_S16 = (S16)(*(pChaine+2) ) * 256 + (S16)(*(pChaine+3)) ;
                    Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ;
                    
                    // Calcule la hauteur en mm
                    Valeur_S16= (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ;
                    
                    if (   ( Valeur_S16 > Hauteur_mini )
                        && ( Valeur_S16 < Hauteur_maxi ) )
                        {
                        Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ;
                        }
                    Vitesse_Courante = 0 ;
                    // On recale la trame réponse
                    Index2 = Index2 + 5 ;
                    
                    if ( Ordre_En_Cours == ORDRE_ARRET )
                        {
                        Statut_Ordre_En_Cours = VALIDE ;
                        Fin_Reception_ms = Chrono_Pilote.read_ms() ;
                        Dernier_Ordre_Confirme = ORDRE_ARRET ;
                        COM_OK = TRUE ;
                        }
                    }
                }
            /***************************************************************************************/
            // Ordre de mouvement automatique
            else if ( ( *(pChaine+1) == ORDRE_DESCEND )
                    ||( *(pChaine+1) == ORDRE_MONTE ) )
                {
                //PC2.printf("\r\n Ordre de mouvement %i\r\n",Id ) ;
                if ( Nb_Caracteres_Recus < ( Index2 + 6 ) )
                    {
                    // Trame incomplete, on reviendra
                    //PC2.printf("\r\n Mouvement Incomplet %i \r\n",Id ) ;
                    return ;
                    }
                // Vérifie le caractere de fin de trame
                if ( *(pChaine+6) == 0x03 )
                    {
                    // Trame OK, on lit la vitesse
                    Valeur_S16= (S16) (*(pChaine+2)) * 256 + (S16) (*(pChaine+3)) ;
                    Vitesse_Courante = ( Valeur_S16 + Vitesse_Courante ) / 2 ;
                    // et la mesure
                    Valeur_S16 = (S16) (*(pChaine+4)) * 256 + (S16) (*(pChaine+5)) ;
                    Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ;
                    
                    // Calcule la hauteur en mm
                    Valeur_S16 = (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ;
                    if (   ( Valeur_S16 > Hauteur_mini )
                        && ( Valeur_S16 < Hauteur_maxi ) )
                        {
                        Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ;
                        }
                    
                    
                    // On recale la trame réponse
                    Index2 = Index2 + 7 ;
                    //PC2.printf("\r\n Ordre de mouvement %i OK\r\n",Id ) ;
                    
                    if ( Ordre_En_Cours == *(pChaine+1) )
                        {
                        Statut_Ordre_En_Cours = VALIDE ;
                        Fin_Reception_ms = Chrono_Pilote.read_ms() ;
                        COM_OK = TRUE ;
                        Dernier_Ordre_Confirme = Ordre_En_Cours ;
                        }
                    }
                }
            /**********************************************************************************/  
            // Ordre de frein
            if ( *(pChaine+1) == ORDRE_FREIN )
                {
                //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
                if ( Nb_Caracteres_Recus < ( Index2 + 2 ) )
                    {
                    // Trame incomplete, on reviendra
                    //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
                    return ;
                    }
                // Vérifie le caractere de fin de trame
                if ( *(pChaine+2) == 0x03 )
                    {
                    // Trame OK
                    //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ;
                    // On recale la trame réponse
                    Index2 = Index2 + 3 ;
  
                    if ( Ordre_En_Cours == ORDRE_FREIN )
                        {
                        Statut_Ordre_En_Cours = VALIDE ;
                        Fin_Reception_ms = Chrono_Pilote.read_ms() ;
                        Dernier_Ordre_Confirme = ORDRE_FREIN ;
                        COM_OK = TRUE ;
                        }
                    }
                }
            /**********************************************************************************/  
            // Ordre de lecture parametre
            if ( *(pChaine+1) == ORDRE_LECTURE )
                {
                //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
                if ( Nb_Caracteres_Recus < ( Index2 + 7 ) )
                    {
                    // Trame incomplete, on reviendra
                    //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
                    return ;
                    }
                // Vérifie le caractere de fin de trame
                if ( *(pChaine+7) == 0x03 )
                    {
                    // Trame OK
                    //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ;
                    // Lecture du parametre
                    Valeur_S32 = (S32) (*(pChaine+3)) * 256 + (S32) (*(pChaine+4)) ;
                    Valeur_S32 = Valeur_S32 * 256 + (S32) (*(pChaine+5)) ;
                    Parametres_Vario_S32[*(pChaine+2)] = Valeur_S32 * 256 + (S32) (*(pChaine+6)) ; 
                    // On recale la trame réponse
                    Index2 = Index2 + 8 ;
  
                    if ( Ordre_En_Cours == ORDRE_LECTURE )
                        {
                        Statut_Ordre_En_Cours = VALIDE ;
                        Fin_Reception_ms = Chrono_Pilote.read_ms() ;
                        Dernier_Ordre_Confirme = ORDRE_LECTURE ;
                        COM_OK = TRUE ;
                        }
                    }
                }
            /**********************************************************************************/  
            // Ordre de configuration parametre
            if ( *(pChaine+1) == ORDRE_CONFIGURE )
                {
                //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
                if ( Nb_Caracteres_Recus < ( Index2 + 3 ) )
                    {
                    // Trame incomplete, on reviendra
                    //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
                    return ;
                    }
                // Vérifie le caractere de fin de trame
                if ( *(pChaine+3) == 0x03 )
                    {
                    // Trame OK
                    //PC2.printf("\r\n Ecriture OK %i\r\n" ,Id) ;
                    // Ecriture du parametre
                    // On recale la trame réponse
                    Index2 = Index2 + 4 ;                   
  
                    if ( Ordre_En_Cours == ORDRE_CONFIGURE )
                        {
                        Statut_Ordre_En_Cours = VALIDE ;
                        Fin_Reception_ms = Chrono_Pilote.read_ms() ;
                        Dernier_Ordre_Confirme = ORDRE_CONFIGURE ;
                        COM_OK = TRUE ;
                        }
                    }
                }
            /**********************************************************************************/  
            // La trame est traitée, on réaligne le reste des caracteres recus
            Index1 = 0 ;
            while ( Index2 < Nb_Caracteres_Recus )
                {
                Chaine_Recue [ Index1 ] = Chaine_Recue [ Index2 ] ;
                Chaine_Recue [ Index2 ] = 0 ;
                //PC2.printf("\t %i:%i",Index1,Chaine_Recue [ Index1 ]);
                Index1++ ;
                Index2++ ;
                
                }
            Nb_Caracteres_Recus = Index1 ;
            pReception = &Chaine_Recue[Nb_Caracteres_Recus] ;
            }
            // Gestion du timeout
            if ( Statut_Ordre_En_Cours == ATTENTE )
                {
                if ( ( Chrono_Pilote.read_ms() - Debut_Emission_ms ) > TIMEOUT_RECEPTION_ms )
                    {
                    Statut_Ordre_En_Cours = DEFAUT ;
                    Compteur_Timeout++ ;
                    COM_OK = FALSE ;
                    //PC2.printf("\n\r *********** Timeout : %i - %i \n\r",Id,Compteur_Timeout);
                    }
                }
    }