Le Pont 3R / Mbed 2 deprecated Le_Pont_V10116

Version initiale

Dependencies:   mbed

Fork of Le_Pont_V10116 by SAGNES Christophe

Pilote.cpp@2:a10c8133d71c, 2018-03-29 (annotated)

Committer:
CS
Date:
Thu Mar 29 15:41:22 2018 +0000
Revision:
2:a10c8133d71c
Parent:
0:8b3c6f593515 
Publish of "Le_Pont_V11003" in the team folder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CS 0:8b3c6f593515 1 /*******************************************************************/
CS 0:8b3c6f593515 2 /* */
CS 0:8b3c6f593515 3 /* Pilote */
CS 0:8b3c6f593515 4 /* */
CS 0:8b3c6f593515 5 /* Objet de pilotage de variateur */
CS 0:8b3c6f593515 6 /* */
CS 0:8b3c6f593515 7 /* */
CS 0:8b3c6f593515 8 /*******************************************************************/
CS 0:8b3c6f593515 9
CS 0:8b3c6f593515 10
CS 0:8b3c6f593515 11 #include "Pilote.h"
CS 0:8b3c6f593515 12
CS 0:8b3c6f593515 13 Serial PC2(USBTX, USBRX) ;
CS 0:8b3c6f593515 14 /** Class Pilote
CS 0:8b3c6f593515 15 *
CS 0:8b3c6f593515 16 */
CS 0:8b3c6f593515 17
CS 0:8b3c6f593515 18 Pilote::Pilote ( S16 Id )
CS 0:8b3c6f593515 19 {
CS 0:8b3c6f593515 20 Hauteur_Cible = 0 ;
CS 0:8b3c6f593515 21 Etat_Deplacement = ARRET ;
CS 0:8b3c6f593515 22 Chaine_A_Emettre[0] = 0 ;
CS 0:8b3c6f593515 23 pReception = &Chaine_Recue[0] ;
CS 0:8b3c6f593515 24 Nb_Caracteres_Recus = 0 ;
CS 0:8b3c6f593515 25 Statut_Ordre_En_Cours = AUCUN ;
CS 0:8b3c6f593515 26
CS 0:8b3c6f593515 27 Parametres_Vario_S32 [ VERSION ] = 100 ;
CS 0:8b3c6f593515 28 Parametres_Vario_S32 [ STARTUP ] = 500 ;
CS 0:8b3c6f593515 29 Parametres_Vario_S32 [ INCREMENT ] = 20 ;
CS 0:8b3c6f593515 30 Parametres_Vario_S32 [ SEUIL_DEMARRAGE ] = 10 ;
CS 0:8b3c6f593515 31 Parametres_Vario_S32 [ ACCELERATION ] = 20 ;
CS 0:8b3c6f593515 32 Parametres_Vario_S32 [ DECELERATION ] = 20 ;
CS 0:8b3c6f593515 33 Parametres_Vario_S32 [ KPV ] = 1000 ;
CS 0:8b3c6f593515 34 Parametres_Vario_S32 [ KIV ] = 100 ;
CS 0:8b3c6f593515 35 Parametres_Vario_S32 [ KDV ] = 0 ;
CS 0:8b3c6f593515 36 Parametres_Vario_S32 [ KA ] = 1000 ;
CS 0:8b3c6f593515 37 Parametres_Vario_S32 [ CONSIGNE ] = 1500 ;
CS 0:8b3c6f593515 38
CS 0:8b3c6f593515 39 };
CS 0:8b3c6f593515 40
CS 0:8b3c6f593515 41
CS 0:8b3c6f593515 42 /*******************************************************************/
CS 0:8b3c6f593515 43 /* */
CS 0:8b3c6f593515 44 /* Init */
CS 0:8b3c6f593515 45 /* */
CS 0:8b3c6f593515 46 /* Initialisation de l'objet de pilotage de variateur */
CS 0:8b3c6f593515 47 /* */
CS 0:8b3c6f593515 48 /* */
CS 0:8b3c6f593515 49 /*******************************************************************/
CS 0:8b3c6f593515 50 void Pilote::Init ( int Baudrates , int bits , int Stop )
CS 0:8b3c6f593515 51 {
CS 0:8b3c6f593515 52 U8 Index ;
CS 0:8b3c6f593515 53 // Initialisation du port série
CS 0:8b3c6f593515 54 pPort->baud ( Baudrates );
CS 0:8b3c6f593515 55 pPort->format ( bits , Serial::None , Stop ) ;
CS 0:8b3c6f593515 56
CS 0:8b3c6f593515 57 while ( pPort->readable() )
CS 0:8b3c6f593515 58 {
CS 0:8b3c6f593515 59 // Réception des caractères pour vider le buffer
CS 0:8b3c6f593515 60 Index = pPort->getc() ;
CS 0:8b3c6f593515 61 }
CS 0:8b3c6f593515 62 // RAZ des chaines recues et à emettre
CS 0:8b3c6f593515 63 for ( Index=0 ; Index < LONGUEUR_CHAINE_EMISSION ; Index++ )
CS 0:8b3c6f593515 64 {
CS 0:8b3c6f593515 65 Chaine_A_Emettre[Index] = 0 ;
CS 0:8b3c6f593515 66 }
CS 0:8b3c6f593515 67 for ( Index=0 ; Index < LONGUEUR_CHAINE_RECEPTION ; Index++ )
CS 0:8b3c6f593515 68 {
CS 0:8b3c6f593515 69 Chaine_Recue[Index] = 0 ;
CS 0:8b3c6f593515 70 }
CS 0:8b3c6f593515 71
CS 0:8b3c6f593515 72 Chrono_Pilote.start() ;
CS 0:8b3c6f593515 73 Chrono_Arret.start() ;
CS 0:8b3c6f593515 74 Debut_Emission_ms = Chrono_Pilote.read_ms () ;
CS 0:8b3c6f593515 75 COM_OK = TRUE ;
CS 0:8b3c6f593515 76 Compteur_Timeout = 0 ;
CS 0:8b3c6f593515 77 Dernier_Ordre_Confirme = ORDRE_ARRET ;
CS 0:8b3c6f593515 78 Etat_Deplacement = ARRET ;
CS 0:8b3c6f593515 79
CS 0:8b3c6f593515 80 }
CS 0:8b3c6f593515 81
CS 0:8b3c6f593515 82 /*******************************************************************/
CS 0:8b3c6f593515 83 /* */
CS 0:8b3c6f593515 84 /* Marche */
CS 0:8b3c6f593515 85 /* */
CS 0:8b3c6f593515 86 /* Ordre de marche automatique */
CS 0:8b3c6f593515 87 /* */
CS 0:8b3c6f593515 88 /* */
CS 0:8b3c6f593515 89 /*******************************************************************/
CS 0:8b3c6f593515 90 void Pilote::Marche ( U8 Mode, U8 Sens, S16 Hauteur , S16 Consigne_Vitesse )
CS 0:8b3c6f593515 91 {
CS 0:8b3c6f593515 92 F32 Valeur_F32 ;
CS 0:8b3c6f593515 93
CS 0:8b3c6f593515 94 // Construction de la trame de marche
CS 0:8b3c6f593515 95 Chaine_A_Emettre[0] = 0x02 ;
CS 0:8b3c6f593515 96
CS 0:8b3c6f593515 97 if ( Sens == MONTE )
CS 0:8b3c6f593515 98 {
CS 0:8b3c6f593515 99 Chaine_A_Emettre [1] = ORDRE_MONTE ; // M
CS 0:8b3c6f593515 100 Etat_Deplacement = MONTE ;
CS 0:8b3c6f593515 101 }
CS 0:8b3c6f593515 102 else
CS 0:8b3c6f593515 103 {
CS 0:8b3c6f593515 104 Chaine_A_Emettre [1] = ORDRE_DESCEND ; // D
CS 0:8b3c6f593515 105 Etat_Deplacement = DESCEND ;
CS 0:8b3c6f593515 106 }
CS 0:8b3c6f593515 107
CS 0:8b3c6f593515 108 if ( Consigne_Vitesse > Vitesse_maxi )
CS 0:8b3c6f593515 109 {
CS 0:8b3c6f593515 110 Consigne_Vitesse = Vitesse_maxi ;
CS 0:8b3c6f593515 111 }
CS 0:8b3c6f593515 112 else if ( Consigne_Vitesse < 0 )
CS 0:8b3c6f593515 113 {
CS 0:8b3c6f593515 114 Consigne_Vitesse = 0 ;
CS 0:8b3c6f593515 115 }
CS 0:8b3c6f593515 116 Chaine_A_Emettre [2] = Consigne_Vitesse / 256 ; // Poids fort
CS 0:8b3c6f593515 117 Chaine_A_Emettre [3] = Consigne_Vitesse % 256 ; // Poids faible
CS 0:8b3c6f593515 118
CS 0:8b3c6f593515 119 if ( Hauteur > Hauteur_maxi )
CS 0:8b3c6f593515 120 {
CS 0:8b3c6f593515 121 Hauteur = Hauteur_maxi ;
CS 0:8b3c6f593515 122 }
CS 0:8b3c6f593515 123 else if ( Hauteur < Hauteur_mini )
CS 0:8b3c6f593515 124 {
CS 0:8b3c6f593515 125 Hauteur = Hauteur_mini ;
CS 0:8b3c6f593515 126 }
CS 0:8b3c6f593515 127
CS 0:8b3c6f593515 128 Hauteur_Cible = Hauteur ;
CS 0:8b3c6f593515 129
CS 0:8b3c6f593515 130 //Calcul de la mesure à atteindre
CS 0:8b3c6f593515 131 Valeur_F32 = ( (F32) Hauteur - MM_Offset ) / MM_par_Points ;
CS 0:8b3c6f593515 132
CS 0:8b3c6f593515 133 Chaine_A_Emettre [4] = (S16)Valeur_F32 / 256 ; // Poids fort
CS 0:8b3c6f593515 134 Chaine_A_Emettre [5] = (S16)Valeur_F32 % 256 ; // Poids faible
CS 0:8b3c6f593515 135
CS 0:8b3c6f593515 136 Chaine_A_Emettre [6] = 0x03 ;
CS 0:8b3c6f593515 137 Chaine_A_Emettre [7] = 0x00 ;
CS 0:8b3c6f593515 138
CS 0:8b3c6f593515 139 if( Statut_Ordre_En_Cours != ATTENTE )
CS 0:8b3c6f593515 140 {
CS 0:8b3c6f593515 141 Ordre_En_Cours = Chaine_A_Emettre [1] ;
CS 0:8b3c6f593515 142 Emission ( &Chaine_A_Emettre [0] , 7 ) ;
CS 0:8b3c6f593515 143 Statut_Ordre_En_Cours = ATTENTE ;
CS 0:8b3c6f593515 144
CS 0:8b3c6f593515 145 }
CS 0:8b3c6f593515 146 }
CS 0:8b3c6f593515 147
CS 0:8b3c6f593515 148 /*******************************************************************/
CS 0:8b3c6f593515 149 /* */
CS 0:8b3c6f593515 150 /* Arret */
CS 0:8b3c6f593515 151 /* */
CS 0:8b3c6f593515 152 /* Ordre d'arret */
CS 0:8b3c6f593515 153 /* */
CS 0:8b3c6f593515 154 /* */
CS 0:8b3c6f593515 155 /*******************************************************************/
CS 0:8b3c6f593515 156 void Pilote::Arret (void)
CS 0:8b3c6f593515 157 {
CS 0:8b3c6f593515 158 Chaine_A_Emettre[0] = 0x02 ;
CS 0:8b3c6f593515 159 Chaine_A_Emettre[1] = ORDRE_ARRET ;
CS 0:8b3c6f593515 160 Chaine_A_Emettre[2] = 0x03 ;
CS 0:8b3c6f593515 161 Chaine_A_Emettre[3] = 0x00 ;
CS 0:8b3c6f593515 162
CS 0:8b3c6f593515 163 //PC2.printf("\r\n Chrono %i: %i ms COM: %i \r\n" ,Id, Chrono_Arret.read_ms() , COM_OK ) ;
CS 0:8b3c6f593515 164
CS 0:8b3c6f593515 165 if ( ( Dernier_Ordre_Confirme == ORDRE_ARRET )
CS 0:8b3c6f593515 166 && ( Chrono_Arret.read_ms() > DELAI_COMMANDE_ARRET_ms )
CS 0:8b3c6f593515 167 || ( Ordre_En_Cours != ORDRE_ARRET )
CS 0:8b3c6f593515 168 || ( Statut_Ordre_En_Cours == DEFAUT ))
CS 0:8b3c6f593515 169 {
CS 0:8b3c6f593515 170 Chrono_Arret.reset() ;
CS 0:8b3c6f593515 171 Emission ( &Chaine_A_Emettre [0] , 3 ) ;
CS 0:8b3c6f593515 172 Etat_Deplacement = ARRET ;
CS 0:8b3c6f593515 173 Statut_Ordre_En_Cours = ATTENTE ;
CS 0:8b3c6f593515 174 Ordre_En_Cours = Chaine_A_Emettre [1] ;
CS 0:8b3c6f593515 175 }
CS 0:8b3c6f593515 176 else
CS 0:8b3c6f593515 177 {
CS 0:8b3c6f593515 178 //PC2.printf("\r\n Chrono %i: %i ms Ordre: %i \r\n" ,Id, Chrono_Arret.read_ms() , Ordre_En_Cours ) ;
CS 0:8b3c6f593515 179 }
CS 0:8b3c6f593515 180
CS 0:8b3c6f593515 181 }
CS 0:8b3c6f593515 182 /*******************************************************************/
CS 0:8b3c6f593515 183 /* */
CS 0:8b3c6f593515 184 /* Etalonnage */
CS 0:8b3c6f593515 185 /* */
CS 0:8b3c6f593515 186 /* Ordre d'étalonnage de hauteur */
CS 0:8b3c6f593515 187 /* */
CS 0:8b3c6f593515 188 /* */
CS 0:8b3c6f593515 189 /*******************************************************************/
CS 0:8b3c6f593515 190 // Etalonnage de hauteur
CS 0:8b3c6f593515 191 void Pilote::Etalonnage (S16 Points1, S16 Hauteur1 , S16 Points2 , S16 Hauteur2 )
CS 0:8b3c6f593515 192 {
CS 0:8b3c6f593515 193 F32 Valeur_F32 ;
CS 0:8b3c6f593515 194
CS 0:8b3c6f593515 195 Valeur_F32 = (F32) ( Points2 - Points1 ) ;
CS 0:8b3c6f593515 196
CS 0:8b3c6f593515 197 if ( abs( Valeur_F32 ) > EPSILON )
CS 0:8b3c6f593515 198 {
CS 0:8b3c6f593515 199 MM_par_Points = (F32) ( Hauteur2 - Hauteur1 ) / Valeur_F32 ;
CS 0:8b3c6f593515 200 }
CS 0:8b3c6f593515 201 else
CS 0:8b3c6f593515 202 {
CS 0:8b3c6f593515 203 MM_par_Points = 0.0 ;
CS 0:8b3c6f593515 204 }
CS 0:8b3c6f593515 205
CS 0:8b3c6f593515 206 MM_Offset = (F32)Hauteur1 - MM_par_Points * (F32)Points1 ;
CS 0:8b3c6f593515 207
CS 0:8b3c6f593515 208 //PC2.printf("\r\n MM par pts %i: %f \r\n" ,Id, MM_par_Points) ;
CS 0:8b3c6f593515 209 //PC2.printf("\r\n Offset %i: %f \r\n" ,Id,MM_Offset) ;
CS 0:8b3c6f593515 210
CS 0:8b3c6f593515 211 }
CS 0:8b3c6f593515 212 /*******************************************************************/
CS 0:8b3c6f593515 213 /* */
CS 0:8b3c6f593515 214 /* Frein */
CS 0:8b3c6f593515 215 /* */
CS 0:8b3c6f593515 216 /* Ordre d'activation du frein */
CS 0:8b3c6f593515 217 /* */
CS 0:8b3c6f593515 218 /* */
CS 0:8b3c6f593515 219 /*******************************************************************/
CS 0:8b3c6f593515 220 void Pilote::Frein ( U8 Etat )
CS 0:8b3c6f593515 221 {
CS 0:8b3c6f593515 222 Chaine_A_Emettre[0] = 0x02 ;
CS 0:8b3c6f593515 223 Chaine_A_Emettre[1] = ORDRE_FREIN ;
CS 0:8b3c6f593515 224 Chaine_A_Emettre[2] = Etat ;
CS 0:8b3c6f593515 225 Chaine_A_Emettre[3] = 0x03 ;
CS 0:8b3c6f593515 226 Chaine_A_Emettre[4] = 0x00 ;
CS 0:8b3c6f593515 227
CS 0:8b3c6f593515 228 if( Statut_Ordre_En_Cours != ATTENTE )
CS 0:8b3c6f593515 229 {
CS 0:8b3c6f593515 230 Ordre_En_Cours = Chaine_A_Emettre [1] ;
CS 0:8b3c6f593515 231 Emission ( &Chaine_A_Emettre [0] , 4 ) ;
CS 0:8b3c6f593515 232 Statut_Ordre_En_Cours = ATTENTE ;
CS 0:8b3c6f593515 233 Etat_Frein = Etat ;
CS 0:8b3c6f593515 234 }
CS 0:8b3c6f593515 235 }
CS 0:8b3c6f593515 236 /*******************************************************************/
CS 0:8b3c6f593515 237 /* */
CS 0:8b3c6f593515 238 /* Lecture */
CS 0:8b3c6f593515 239 /* */
CS 0:8b3c6f593515 240 /* Ordre de lecture d'un parametre de variateur */
CS 0:8b3c6f593515 241 /* */
CS 0:8b3c6f593515 242 /* */
CS 0:8b3c6f593515 243 /*******************************************************************/
CS 0:8b3c6f593515 244 void Pilote::Lecture ( U16 Num_Parametre )
CS 0:8b3c6f593515 245 {
CS 0:8b3c6f593515 246 Chaine_A_Emettre[0] = 0x02 ;
CS 0:8b3c6f593515 247 Chaine_A_Emettre[1] = ORDRE_LECTURE ;
CS 0:8b3c6f593515 248 Chaine_A_Emettre[2] = Num_Parametre ;
CS 0:8b3c6f593515 249 Chaine_A_Emettre[3] = 0x03 ;
CS 0:8b3c6f593515 250 Chaine_A_Emettre[4] = 0x00 ;
CS 0:8b3c6f593515 251
CS 0:8b3c6f593515 252 if( Statut_Ordre_En_Cours != ATTENTE )
CS 0:8b3c6f593515 253 {
CS 0:8b3c6f593515 254 Ordre_En_Cours = Chaine_A_Emettre [1] ;
CS 0:8b3c6f593515 255 Emission ( &Chaine_A_Emettre [0] , 4 ) ;
CS 0:8b3c6f593515 256 Statut_Ordre_En_Cours = ATTENTE ;
CS 0:8b3c6f593515 257
CS 0:8b3c6f593515 258 }
CS 0:8b3c6f593515 259 }
CS 0:8b3c6f593515 260 /*******************************************************************/
CS 0:8b3c6f593515 261 /* */
CS 0:8b3c6f593515 262 /* Configure */
CS 0:8b3c6f593515 263 /* */
CS 0:8b3c6f593515 264 /* Ordre de configuration d'un parametre variateur */
CS 0:8b3c6f593515 265 /* */
CS 0:8b3c6f593515 266 /* */
CS 0:8b3c6f593515 267 /*******************************************************************/
CS 0:8b3c6f593515 268 void Pilote::Configure ( U16 Num_Parametre , S32 Valeur_S32 )
CS 0:8b3c6f593515 269 {
CS 0:8b3c6f593515 270
CS 0:8b3c6f593515 271 Chaine_A_Emettre[0] = 0x02 ;
CS 0:8b3c6f593515 272 Chaine_A_Emettre[1] = ORDRE_CONFIGURE ;
CS 0:8b3c6f593515 273 Chaine_A_Emettre[2] = Num_Parametre ;
CS 0:8b3c6f593515 274 Chaine_A_Emettre[6] = (U8)( Valeur_S32 & 0x000000FF) ;
CS 0:8b3c6f593515 275 Valeur_S32 = Valeur_S32 >> 8 ;
CS 0:8b3c6f593515 276 Chaine_A_Emettre[5] = (U8)( Valeur_S32 & 0x000000FF) ;
CS 0:8b3c6f593515 277 Valeur_S32 = Valeur_S32 >> 8 ;
CS 0:8b3c6f593515 278 Chaine_A_Emettre[4] = (U8)( Valeur_S32 & 0x000000FF) ;
CS 0:8b3c6f593515 279 Valeur_S32 = Valeur_S32 >> 8 ;
CS 0:8b3c6f593515 280 Chaine_A_Emettre[3] = (U8)( Valeur_S32 & 0x000000FF) ;
CS 0:8b3c6f593515 281 Chaine_A_Emettre[7] = 0x03 ;
CS 0:8b3c6f593515 282 Chaine_A_Emettre[8] = 0x00 ;
CS 0:8b3c6f593515 283
CS 0:8b3c6f593515 284 if( Statut_Ordre_En_Cours != ATTENTE )
CS 0:8b3c6f593515 285 {
CS 0:8b3c6f593515 286 Ordre_En_Cours = Chaine_A_Emettre [1] ;
CS 0:8b3c6f593515 287 Emission ( &Chaine_A_Emettre [0] , 8 ) ;
CS 0:8b3c6f593515 288 Statut_Ordre_En_Cours = ATTENTE ;
CS 0:8b3c6f593515 289
CS 0:8b3c6f593515 290 }
CS 0:8b3c6f593515 291 }
CS 0:8b3c6f593515 292 /*******************************************************************/
CS 0:8b3c6f593515 293 /* */
CS 0:8b3c6f593515 294 /* Emission */
CS 0:8b3c6f593515 295 /* */
CS 0:8b3c6f593515 296 /* Emission d'une trame vers le variateur */
CS 0:8b3c6f593515 297 /* */
CS 0:8b3c6f593515 298 /* */
CS 0:8b3c6f593515 299 /*******************************************************************/
CS 0:8b3c6f593515 300 void Pilote::Emission ( U8 *pChaine , U8 Longueur )
CS 0:8b3c6f593515 301 {
CS 0:8b3c6f593515 302 U8 Index = 0 ;
CS 0:8b3c6f593515 303
CS 0:8b3c6f593515 304 if ( pPort->writeable() )
CS 0:8b3c6f593515 305 {
CS 0:8b3c6f593515 306 // Mesure du timeout
CS 0:8b3c6f593515 307 Debut_Emission_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 308
CS 0:8b3c6f593515 309 while ( Index < Longueur )
CS 0:8b3c6f593515 310 {
CS 0:8b3c6f593515 311 pPort->putc( *pChaine ) ;
CS 0:8b3c6f593515 312 //PC2.printf("\r\n %i: %i \r\n" ,Id, *pChaine) ;
CS 0:8b3c6f593515 313 pChaine++ ;
CS 0:8b3c6f593515 314 Index++ ;
CS 0:8b3c6f593515 315 }
CS 0:8b3c6f593515 316 }
CS 0:8b3c6f593515 317 else
CS 0:8b3c6f593515 318 {
CS 0:8b3c6f593515 319 PC2.printf("\r\n %i: Bloque \r\n" ,Id) ;
CS 0:8b3c6f593515 320 }
CS 0:8b3c6f593515 321
CS 0:8b3c6f593515 322 }
CS 0:8b3c6f593515 323 /*******************************************************************/
CS 0:8b3c6f593515 324 /* */
CS 0:8b3c6f593515 325 /* Reception */
CS 0:8b3c6f593515 326 /* */
CS 0:8b3c6f593515 327 /* Reception de la reponse du variateur */
CS 0:8b3c6f593515 328 /* */
CS 0:8b3c6f593515 329 /* */
CS 0:8b3c6f593515 330 /*******************************************************************/
CS 0:8b3c6f593515 331 void Pilote::Reception ( void )
CS 0:8b3c6f593515 332 {
CS 0:8b3c6f593515 333 U8 Index1 = 0 ;
CS 0:8b3c6f593515 334 U8 Index2 = 0 ;
CS 0:8b3c6f593515 335 S16 Valeur_S16 ;
CS 0:8b3c6f593515 336 S32 Valeur_S32 ;
CS 0:8b3c6f593515 337 U8 *pChaine ;
CS 0:8b3c6f593515 338
CS 0:8b3c6f593515 339 while ( pPort->readable() )
CS 0:8b3c6f593515 340 {
CS 0:8b3c6f593515 341 // Réception des caractères si il y en a
CS 0:8b3c6f593515 342 *pReception = pPort->getc() ;
CS 0:8b3c6f593515 343 //PC2.printf("\r\n %i",*pReception ) ;
CS 0:8b3c6f593515 344 pReception++ ;
CS 0:8b3c6f593515 345 Nb_Caracteres_Recus++ ;
CS 0:8b3c6f593515 346 }
CS 0:8b3c6f593515 347
CS 0:8b3c6f593515 348 pChaine = &Chaine_Recue[0] ;
CS 0:8b3c6f593515 349
CS 0:8b3c6f593515 350 // La plus petite trame comporte 3 caractères
CS 0:8b3c6f593515 351 if ( Nb_Caracteres_Recus > 2 )
CS 0:8b3c6f593515 352 {
CS 0:8b3c6f593515 353 while ( *pChaine != 0x02 )
CS 0:8b3c6f593515 354 {
CS 0:8b3c6f593515 355 // Recherche d'un début de trame: STX = 0x02
CS 0:8b3c6f593515 356 if ( Index2 == Nb_Caracteres_Recus )
CS 0:8b3c6f593515 357 {
CS 0:8b3c6f593515 358 // Pas de début de trame, on ecrase les caractères reçus et on s'en va!
CS 0:8b3c6f593515 359 pReception = &Chaine_Recue[0] ;
CS 0:8b3c6f593515 360 Nb_Caracteres_Recus = 0 ;
CS 0:8b3c6f593515 361 //PC2.printf("\r\n Poubelle %i \r\n" ,Id) ;
CS 0:8b3c6f593515 362 return ;
CS 0:8b3c6f593515 363 }
CS 0:8b3c6f593515 364 pChaine++ ;
CS 0:8b3c6f593515 365 Index2++ ;
CS 0:8b3c6f593515 366 }
CS 0:8b3c6f593515 367 /**********************************************************************************/
CS 0:8b3c6f593515 368 // Ordre d'arret
CS 0:8b3c6f593515 369 if ( *(pChaine+1) == ORDRE_ARRET )
CS 0:8b3c6f593515 370 {
CS 0:8b3c6f593515 371 //PC2.printf("\r\n Ordre d'Arret %i\r\n" ,Id) ;
CS 0:8b3c6f593515 372 if ( Nb_Caracteres_Recus < ( Index2 + 4 ) )
CS 0:8b3c6f593515 373 {
CS 0:8b3c6f593515 374 // Trame incomplete, on reviendra
CS 0:8b3c6f593515 375 //PC2.printf("\r\n Arret incomplet %i \r\n" ,Id) ;
CS 0:8b3c6f593515 376 return ;
CS 0:8b3c6f593515 377 }
CS 0:8b3c6f593515 378 // Vérifie le caractere de fin de trame
CS 0:8b3c6f593515 379 if ( *(pChaine+4) == 0x03 )
CS 0:8b3c6f593515 380 {
CS 0:8b3c6f593515 381 // Trame OK, on efface
CS 0:8b3c6f593515 382 //PC2.printf("\r\n Arret OK %i\r\n" ,Id) ;
CS 0:8b3c6f593515 383 // On lit la mesure
CS 0:8b3c6f593515 384 Valeur_S16 = (S16)(*(pChaine+2) ) * 256 + (S16)(*(pChaine+3)) ;
CS 0:8b3c6f593515 385 Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ;
CS 0:8b3c6f593515 386
CS 0:8b3c6f593515 387 // Calcule la hauteur en mm
CS 0:8b3c6f593515 388 Valeur_S16= (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ;
CS 0:8b3c6f593515 389
CS 0:8b3c6f593515 390 if ( ( Valeur_S16 > Hauteur_mini )
CS 0:8b3c6f593515 391 && ( Valeur_S16 < Hauteur_maxi ) )
CS 0:8b3c6f593515 392 {
CS 0:8b3c6f593515 393 Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ;
CS 0:8b3c6f593515 394 }
CS 0:8b3c6f593515 395 Vitesse_Courante = 0 ;
CS 0:8b3c6f593515 396 // On recale la trame réponse
CS 0:8b3c6f593515 397 Index2 = Index2 + 5 ;
CS 0:8b3c6f593515 398
CS 0:8b3c6f593515 399 if ( Ordre_En_Cours == ORDRE_ARRET )
CS 0:8b3c6f593515 400 {
CS 0:8b3c6f593515 401 Statut_Ordre_En_Cours = VALIDE ;
CS 0:8b3c6f593515 402 Fin_Reception_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 403 Dernier_Ordre_Confirme = ORDRE_ARRET ;
CS 0:8b3c6f593515 404 COM_OK = TRUE ;
CS 0:8b3c6f593515 405 }
CS 0:8b3c6f593515 406 }
CS 0:8b3c6f593515 407 }
CS 0:8b3c6f593515 408 /***************************************************************************************/
CS 0:8b3c6f593515 409 // Ordre de mouvement automatique
CS 0:8b3c6f593515 410 else if ( ( *(pChaine+1) == ORDRE_DESCEND )
CS 0:8b3c6f593515 411 ||( *(pChaine+1) == ORDRE_MONTE ) )
CS 0:8b3c6f593515 412 {
CS 0:8b3c6f593515 413 //PC2.printf("\r\n Ordre de mouvement %i\r\n",Id ) ;
CS 0:8b3c6f593515 414 if ( Nb_Caracteres_Recus < ( Index2 + 6 ) )
CS 0:8b3c6f593515 415 {
CS 0:8b3c6f593515 416 // Trame incomplete, on reviendra
CS 0:8b3c6f593515 417 //PC2.printf("\r\n Mouvement Incomplet %i \r\n",Id ) ;
CS 0:8b3c6f593515 418 return ;
CS 0:8b3c6f593515 419 }
CS 0:8b3c6f593515 420 // Vérifie le caractere de fin de trame
CS 0:8b3c6f593515 421 if ( *(pChaine+6) == 0x03 )
CS 0:8b3c6f593515 422 {
CS 0:8b3c6f593515 423 // Trame OK, on lit la vitesse
CS 0:8b3c6f593515 424 Valeur_S16= (S16) (*(pChaine+2)) * 256 + (S16) (*(pChaine+3)) ;
CS 0:8b3c6f593515 425 Vitesse_Courante = ( Valeur_S16 + Vitesse_Courante ) / 2 ;
CS 0:8b3c6f593515 426 // et la mesure
CS 0:8b3c6f593515 427 Valeur_S16 = (S16) (*(pChaine+4)) * 256 + (S16) (*(pChaine+5)) ;
CS 0:8b3c6f593515 428 Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ;
CS 0:8b3c6f593515 429
CS 0:8b3c6f593515 430 // Calcule la hauteur en mm
CS 0:8b3c6f593515 431 Valeur_S16 = (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ;
CS 0:8b3c6f593515 432 if ( ( Valeur_S16 > Hauteur_mini )
CS 0:8b3c6f593515 433 && ( Valeur_S16 < Hauteur_maxi ) )
CS 0:8b3c6f593515 434 {
CS 0:8b3c6f593515 435 Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ;
CS 0:8b3c6f593515 436 }
CS 0:8b3c6f593515 437
CS 0:8b3c6f593515 438
CS 0:8b3c6f593515 439 // On recale la trame réponse
CS 0:8b3c6f593515 440 Index2 = Index2 + 7 ;
CS 0:8b3c6f593515 441 //PC2.printf("\r\n Ordre de mouvement %i OK\r\n",Id ) ;
CS 0:8b3c6f593515 442
CS 0:8b3c6f593515 443 if ( Ordre_En_Cours == *(pChaine+1) )
CS 0:8b3c6f593515 444 {
CS 0:8b3c6f593515 445 Statut_Ordre_En_Cours = VALIDE ;
CS 0:8b3c6f593515 446 Fin_Reception_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 447 COM_OK = TRUE ;
CS 0:8b3c6f593515 448 Dernier_Ordre_Confirme = Ordre_En_Cours ;
CS 0:8b3c6f593515 449 }
CS 0:8b3c6f593515 450 }
CS 0:8b3c6f593515 451 }
CS 0:8b3c6f593515 452 /**********************************************************************************/
CS 0:8b3c6f593515 453 // Ordre de frein
CS 0:8b3c6f593515 454 if ( *(pChaine+1) == ORDRE_FREIN )
CS 0:8b3c6f593515 455 {
CS 0:8b3c6f593515 456 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
CS 0:8b3c6f593515 457 if ( Nb_Caracteres_Recus < ( Index2 + 2 ) )
CS 0:8b3c6f593515 458 {
CS 0:8b3c6f593515 459 // Trame incomplete, on reviendra
CS 0:8b3c6f593515 460 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
CS 0:8b3c6f593515 461 return ;
CS 0:8b3c6f593515 462 }
CS 0:8b3c6f593515 463 // Vérifie le caractere de fin de trame
CS 0:8b3c6f593515 464 if ( *(pChaine+2) == 0x03 )
CS 0:8b3c6f593515 465 {
CS 0:8b3c6f593515 466 // Trame OK
CS 0:8b3c6f593515 467 //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ;
CS 0:8b3c6f593515 468 // On recale la trame réponse
CS 0:8b3c6f593515 469 Index2 = Index2 + 3 ;
CS 0:8b3c6f593515 470
CS 0:8b3c6f593515 471 if ( Ordre_En_Cours == ORDRE_FREIN )
CS 0:8b3c6f593515 472 {
CS 0:8b3c6f593515 473 Statut_Ordre_En_Cours = VALIDE ;
CS 0:8b3c6f593515 474 Fin_Reception_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 475 Dernier_Ordre_Confirme = ORDRE_FREIN ;
CS 0:8b3c6f593515 476 COM_OK = TRUE ;
CS 0:8b3c6f593515 477 }
CS 0:8b3c6f593515 478 }
CS 0:8b3c6f593515 479 }
CS 0:8b3c6f593515 480 /**********************************************************************************/
CS 0:8b3c6f593515 481 // Ordre de lecture parametre
CS 0:8b3c6f593515 482 if ( *(pChaine+1) == ORDRE_LECTURE )
CS 0:8b3c6f593515 483 {
CS 0:8b3c6f593515 484 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
CS 0:8b3c6f593515 485 if ( Nb_Caracteres_Recus < ( Index2 + 7 ) )
CS 0:8b3c6f593515 486 {
CS 0:8b3c6f593515 487 // Trame incomplete, on reviendra
CS 0:8b3c6f593515 488 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
CS 0:8b3c6f593515 489 return ;
CS 0:8b3c6f593515 490 }
CS 0:8b3c6f593515 491 // Vérifie le caractere de fin de trame
CS 0:8b3c6f593515 492 if ( *(pChaine+7) == 0x03 )
CS 0:8b3c6f593515 493 {
CS 0:8b3c6f593515 494 // Trame OK
CS 0:8b3c6f593515 495 //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ;
CS 0:8b3c6f593515 496 // Lecture du parametre
CS 0:8b3c6f593515 497 Valeur_S32 = (S32) (*(pChaine+3)) * 256 + (S32) (*(pChaine+4)) ;
CS 0:8b3c6f593515 498 Valeur_S32 = Valeur_S32 * 256 + (S32) (*(pChaine+5)) ;
CS 0:8b3c6f593515 499 Parametres_Vario_S32[*(pChaine+2)] = Valeur_S32 * 256 + (S32) (*(pChaine+6)) ;
CS 0:8b3c6f593515 500 // On recale la trame réponse
CS 0:8b3c6f593515 501 Index2 = Index2 + 8 ;
CS 0:8b3c6f593515 502
CS 0:8b3c6f593515 503 if ( Ordre_En_Cours == ORDRE_LECTURE )
CS 0:8b3c6f593515 504 {
CS 0:8b3c6f593515 505 Statut_Ordre_En_Cours = VALIDE ;
CS 0:8b3c6f593515 506 Fin_Reception_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 507 Dernier_Ordre_Confirme = ORDRE_LECTURE ;
CS 0:8b3c6f593515 508 COM_OK = TRUE ;
CS 0:8b3c6f593515 509 }
CS 0:8b3c6f593515 510 }
CS 0:8b3c6f593515 511 }
CS 0:8b3c6f593515 512 /**********************************************************************************/
CS 0:8b3c6f593515 513 // Ordre de configuration parametre
CS 0:8b3c6f593515 514 if ( *(pChaine+1) == ORDRE_CONFIGURE )
CS 0:8b3c6f593515 515 {
CS 0:8b3c6f593515 516 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ;
CS 0:8b3c6f593515 517 if ( Nb_Caracteres_Recus < ( Index2 + 3 ) )
CS 0:8b3c6f593515 518 {
CS 0:8b3c6f593515 519 // Trame incomplete, on reviendra
CS 0:8b3c6f593515 520 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ;
CS 0:8b3c6f593515 521 return ;
CS 0:8b3c6f593515 522 }
CS 0:8b3c6f593515 523 // Vérifie le caractere de fin de trame
CS 0:8b3c6f593515 524 if ( *(pChaine+3) == 0x03 )
CS 0:8b3c6f593515 525 {
CS 0:8b3c6f593515 526 // Trame OK
CS 0:8b3c6f593515 527 //PC2.printf("\r\n Ecriture OK %i\r\n" ,Id) ;
CS 0:8b3c6f593515 528 // Ecriture du parametre
CS 0:8b3c6f593515 529 // On recale la trame réponse
CS 0:8b3c6f593515 530 Index2 = Index2 + 4 ;
CS 0:8b3c6f593515 531
CS 0:8b3c6f593515 532 if ( Ordre_En_Cours == ORDRE_CONFIGURE )
CS 0:8b3c6f593515 533 {
CS 0:8b3c6f593515 534 Statut_Ordre_En_Cours = VALIDE ;
CS 0:8b3c6f593515 535 Fin_Reception_ms = Chrono_Pilote.read_ms() ;
CS 0:8b3c6f593515 536 Dernier_Ordre_Confirme = ORDRE_CONFIGURE ;
CS 0:8b3c6f593515 537 COM_OK = TRUE ;
CS 0:8b3c6f593515 538 }
CS 0:8b3c6f593515 539 }
CS 0:8b3c6f593515 540 }
CS 0:8b3c6f593515 541 /**********************************************************************************/
CS 0:8b3c6f593515 542 // La trame est traitée, on réaligne le reste des caracteres recus
CS 0:8b3c6f593515 543 Index1 = 0 ;
CS 0:8b3c6f593515 544 while ( Index2 < Nb_Caracteres_Recus )
CS 0:8b3c6f593515 545 {
CS 0:8b3c6f593515 546 Chaine_Recue [ Index1 ] = Chaine_Recue [ Index2 ] ;
CS 0:8b3c6f593515 547 Chaine_Recue [ Index2 ] = 0 ;
CS 0:8b3c6f593515 548 //PC2.printf("\t %i:%i",Index1,Chaine_Recue [ Index1 ]);
CS 0:8b3c6f593515 549 Index1++ ;
CS 0:8b3c6f593515 550 Index2++ ;
CS 0:8b3c6f593515 551
CS 0:8b3c6f593515 552 }
CS 0:8b3c6f593515 553 Nb_Caracteres_Recus = Index1 ;
CS 0:8b3c6f593515 554 pReception = &Chaine_Recue[Nb_Caracteres_Recus] ;
CS 0:8b3c6f593515 555 }
CS 0:8b3c6f593515 556 // Gestion du timeout
CS 0:8b3c6f593515 557 if ( Statut_Ordre_En_Cours == ATTENTE )
CS 0:8b3c6f593515 558 {
CS 0:8b3c6f593515 559 if ( ( Chrono_Pilote.read_ms() - Debut_Emission_ms ) > TIMEOUT_RECEPTION_ms )
CS 0:8b3c6f593515 560 {
CS 0:8b3c6f593515 561 Statut_Ordre_En_Cours = DEFAUT ;
CS 0:8b3c6f593515 562 Compteur_Timeout++ ;
CS 0:8b3c6f593515 563 COM_OK = FALSE ;
CS 0:8b3c6f593515 564 //PC2.printf("\n\r *********** Timeout : %i - %i \n\r",Id,Compteur_Timeout);
CS 0:8b3c6f593515 565 }
CS 0:8b3c6f593515 566 }
CS 0:8b3c6f593515 567 }
CS 0:8b3c6f593515 568