3R / Mbed 2 deprecated Le_Pont_V10015

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.

Dependencies:   mbed

Pilote.cpp@0:a53b6ec3fbb3, 2016-05-30 (annotated)

Committer:
CS
Date:
Mon May 30 07:04:49 2016 +0000
Revision:
0:a53b6ec3fbb3
RAZ integrale de la synchronisation

Who changed what in which revision?

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