SAGNES Christophe
/
Le_Pont_V10116
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Le_Pont_V10116
by 
3R
Diff: Pilote.cpp
- Revision:
- 0:8b3c6f593515
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Pilote.cpp Thu Jun 22 09:33:04 2017 +0000
@@ -0,0 +1,568 @@
+/*******************************************************************/
+/* */
+/* 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);
+ }
+ }
+ }
+