SAGNES Christophe
/
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.
Fork of
Le_Pont_V10015
by 
3R
Embed:
(wiki syntax)
Show/hide line numbers
Pilote.cpp
00001 /*******************************************************************/ 00002 /* */ 00003 /* Pilote */ 00004 /* */ 00005 /* Objet de pilotage de variateur */ 00006 /* */ 00007 /* */ 00008 /*******************************************************************/ 00009 00010 00011 #include "Pilote.h" 00012 00013 Serial PC2(USBTX, USBRX) ; 00014 /** Class Pilote 00015 * 00016 */ 00017 00018 Pilote::Pilote ( S16 Id ) 00019 { 00020 Hauteur_Cible = 0 ; 00021 Etat_Deplacement = ARRET ; 00022 Chaine_A_Emettre[0] = 0 ; 00023 pReception = &Chaine_Recue[0] ; 00024 Nb_Caracteres_Recus = 0 ; 00025 Statut_Ordre_En_Cours = AUCUN ; 00026 00027 Parametres_Vario_S32 [ VERSION ] = 100 ; 00028 Parametres_Vario_S32 [ STARTUP ] = 500 ; 00029 Parametres_Vario_S32 [ INCREMENT ] = 20 ; 00030 Parametres_Vario_S32 [ SEUIL_DEMARRAGE ] = 10 ; 00031 Parametres_Vario_S32 [ ACCELERATION ] = 20 ; 00032 Parametres_Vario_S32 [ DECELERATION ] = 20 ; 00033 Parametres_Vario_S32 [ KPV ] = 1000 ; 00034 Parametres_Vario_S32 [ KIV ] = 100 ; 00035 Parametres_Vario_S32 [ KDV ] = 0 ; 00036 Parametres_Vario_S32 [ KA ] = 1000 ; 00037 Parametres_Vario_S32 [ CONSIGNE ] = 1500 ; 00038 00039 }; 00040 00041 00042 /*******************************************************************/ 00043 /* */ 00044 /* Init */ 00045 /* */ 00046 /* Initialisation de l'objet de pilotage de variateur */ 00047 /* */ 00048 /* */ 00049 /*******************************************************************/ 00050 void Pilote::Init ( int Baudrates , int bits , int Stop ) 00051 { 00052 U8 Index ; 00053 // Initialisation du port série 00054 pPort->baud ( Baudrates ); 00055 pPort->format ( bits , Serial::None , Stop ) ; 00056 00057 while ( pPort->readable() ) 00058 { 00059 // Réception des caractères pour vider le buffer 00060 Index = pPort->getc() ; 00061 } 00062 // RAZ des chaines recues et à emettre 00063 for ( Index=0 ; Index < LONGUEUR_CHAINE_EMISSION ; Index++ ) 00064 { 00065 Chaine_A_Emettre[Index] = 0 ; 00066 } 00067 for ( Index=0 ; Index < LONGUEUR_CHAINE_RECEPTION ; Index++ ) 00068 { 00069 Chaine_Recue[Index] = 0 ; 00070 } 00071 00072 Chrono_Pilote.start() ; 00073 Chrono_Arret.start() ; 00074 Debut_Emission_ms = Chrono_Pilote.read_ms () ; 00075 COM_OK = TRUE ; 00076 Compteur_Timeout = 0 ; 00077 Dernier_Ordre_Confirme = ORDRE_ARRET ; 00078 Etat_Deplacement = ARRET ; 00079 00080 } 00081 00082 /*******************************************************************/ 00083 /* */ 00084 /* Marche */ 00085 /* */ 00086 /* Ordre de marche automatique */ 00087 /* */ 00088 /* */ 00089 /*******************************************************************/ 00090 void Pilote::Marche ( U8 Mode, U8 Sens, S16 Hauteur , S16 Consigne_Vitesse ) 00091 { 00092 F32 Valeur_F32 ; 00093 00094 // Construction de la trame de marche 00095 Chaine_A_Emettre[0] = 0x02 ; 00096 00097 if ( Sens == MONTE ) 00098 { 00099 Chaine_A_Emettre [1] = ORDRE_MONTE ; // M 00100 Etat_Deplacement = MONTE ; 00101 } 00102 else 00103 { 00104 Chaine_A_Emettre [1] = ORDRE_DESCEND ; // D 00105 Etat_Deplacement = DESCEND ; 00106 } 00107 00108 if ( Consigne_Vitesse > Vitesse_maxi ) 00109 { 00110 Consigne_Vitesse = Vitesse_maxi ; 00111 } 00112 else if ( Consigne_Vitesse < 0 ) 00113 { 00114 Consigne_Vitesse = 0 ; 00115 } 00116 Chaine_A_Emettre [2] = Consigne_Vitesse / 256 ; // Poids fort 00117 Chaine_A_Emettre [3] = Consigne_Vitesse % 256 ; // Poids faible 00118 00119 if ( Hauteur > Hauteur_maxi ) 00120 { 00121 Hauteur = Hauteur_maxi ; 00122 } 00123 else if ( Hauteur < Hauteur_mini ) 00124 { 00125 Hauteur = Hauteur_mini ; 00126 } 00127 00128 Hauteur_Cible = Hauteur ; 00129 00130 //Calcul de la mesure à atteindre 00131 Valeur_F32 = ( (F32) Hauteur - MM_Offset ) / MM_par_Points ; 00132 00133 Chaine_A_Emettre [4] = (S16)Valeur_F32 / 256 ; // Poids fort 00134 Chaine_A_Emettre [5] = (S16)Valeur_F32 % 256 ; // Poids faible 00135 00136 Chaine_A_Emettre [6] = 0x03 ; 00137 Chaine_A_Emettre [7] = 0x00 ; 00138 00139 if( Statut_Ordre_En_Cours != ATTENTE ) 00140 { 00141 Ordre_En_Cours = Chaine_A_Emettre [1] ; 00142 Emission ( &Chaine_A_Emettre [0] , 7 ) ; 00143 Statut_Ordre_En_Cours = ATTENTE ; 00144 00145 } 00146 } 00147 00148 /*******************************************************************/ 00149 /* */ 00150 /* Arret */ 00151 /* */ 00152 /* Ordre d'arret */ 00153 /* */ 00154 /* */ 00155 /*******************************************************************/ 00156 void Pilote::Arret (void) 00157 { 00158 Chaine_A_Emettre[0] = 0x02 ; 00159 Chaine_A_Emettre[1] = ORDRE_ARRET ; 00160 Chaine_A_Emettre[2] = 0x03 ; 00161 Chaine_A_Emettre[3] = 0x00 ; 00162 00163 //PC2.printf("\r\n Chrono %i: %i ms COM: %i \r\n" ,Id, Chrono_Arret.read_ms() , COM_OK ) ; 00164 00165 if ( ( Dernier_Ordre_Confirme == ORDRE_ARRET ) 00166 && ( Chrono_Arret.read_ms() > DELAI_COMMANDE_ARRET_ms ) 00167 || ( Ordre_En_Cours != ORDRE_ARRET ) 00168 || ( Statut_Ordre_En_Cours == DEFAUT )) 00169 { 00170 Chrono_Arret.reset() ; 00171 Emission ( &Chaine_A_Emettre [0] , 3 ) ; 00172 Etat_Deplacement = ARRET ; 00173 Statut_Ordre_En_Cours = ATTENTE ; 00174 Ordre_En_Cours = Chaine_A_Emettre [1] ; 00175 } 00176 else 00177 { 00178 //PC2.printf("\r\n Chrono %i: %i ms Ordre: %i \r\n" ,Id, Chrono_Arret.read_ms() , Ordre_En_Cours ) ; 00179 } 00180 00181 } 00182 /*******************************************************************/ 00183 /* */ 00184 /* Etalonnage */ 00185 /* */ 00186 /* Ordre d'étalonnage de hauteur */ 00187 /* */ 00188 /* */ 00189 /*******************************************************************/ 00190 // Etalonnage de hauteur 00191 void Pilote::Etalonnage (S16 Points1, S16 Hauteur1 , S16 Points2 , S16 Hauteur2 ) 00192 { 00193 F32 Valeur_F32 ; 00194 00195 Valeur_F32 = (F32) ( Points2 - Points1 ) ; 00196 00197 if ( abs( Valeur_F32 ) > EPSILON ) 00198 { 00199 MM_par_Points = (F32) ( Hauteur2 - Hauteur1 ) / Valeur_F32 ; 00200 } 00201 else 00202 { 00203 MM_par_Points = 0.0 ; 00204 } 00205 00206 MM_Offset = (F32)Hauteur1 - MM_par_Points * (F32)Points1 ; 00207 00208 //PC2.printf("\r\n MM par pts %i: %f \r\n" ,Id, MM_par_Points) ; 00209 //PC2.printf("\r\n Offset %i: %f \r\n" ,Id,MM_Offset) ; 00210 00211 } 00212 /*******************************************************************/ 00213 /* */ 00214 /* Frein */ 00215 /* */ 00216 /* Ordre d'activation du frein */ 00217 /* */ 00218 /* */ 00219 /*******************************************************************/ 00220 void Pilote::Frein ( U8 Etat ) 00221 { 00222 Chaine_A_Emettre[0] = 0x02 ; 00223 Chaine_A_Emettre[1] = ORDRE_FREIN ; 00224 Chaine_A_Emettre[2] = Etat ; 00225 Chaine_A_Emettre[3] = 0x03 ; 00226 Chaine_A_Emettre[4] = 0x00 ; 00227 00228 if( Statut_Ordre_En_Cours != ATTENTE ) 00229 { 00230 Ordre_En_Cours = Chaine_A_Emettre [1] ; 00231 Emission ( &Chaine_A_Emettre [0] , 4 ) ; 00232 Statut_Ordre_En_Cours = ATTENTE ; 00233 Etat_Frein = Etat ; 00234 } 00235 } 00236 /*******************************************************************/ 00237 /* */ 00238 /* Lecture */ 00239 /* */ 00240 /* Ordre de lecture d'un parametre de variateur */ 00241 /* */ 00242 /* */ 00243 /*******************************************************************/ 00244 void Pilote::Lecture ( U16 Num_Parametre ) 00245 { 00246 Chaine_A_Emettre[0] = 0x02 ; 00247 Chaine_A_Emettre[1] = ORDRE_LECTURE ; 00248 Chaine_A_Emettre[2] = Num_Parametre ; 00249 Chaine_A_Emettre[3] = 0x03 ; 00250 Chaine_A_Emettre[4] = 0x00 ; 00251 00252 if( Statut_Ordre_En_Cours != ATTENTE ) 00253 { 00254 Ordre_En_Cours = Chaine_A_Emettre [1] ; 00255 Emission ( &Chaine_A_Emettre [0] , 4 ) ; 00256 Statut_Ordre_En_Cours = ATTENTE ; 00257 00258 } 00259 } 00260 /*******************************************************************/ 00261 /* */ 00262 /* Configure */ 00263 /* */ 00264 /* Ordre de configuration d'un parametre variateur */ 00265 /* */ 00266 /* */ 00267 /*******************************************************************/ 00268 void Pilote::Configure ( U16 Num_Parametre , S32 Valeur_S32 ) 00269 { 00270 00271 Chaine_A_Emettre[0] = 0x02 ; 00272 Chaine_A_Emettre[1] = ORDRE_CONFIGURE ; 00273 Chaine_A_Emettre[2] = Num_Parametre ; 00274 Chaine_A_Emettre[6] = (U8)( Valeur_S32 & 0x000000FF) ; 00275 Valeur_S32 = Valeur_S32 >> 8 ; 00276 Chaine_A_Emettre[5] = (U8)( Valeur_S32 & 0x000000FF) ; 00277 Valeur_S32 = Valeur_S32 >> 8 ; 00278 Chaine_A_Emettre[4] = (U8)( Valeur_S32 & 0x000000FF) ; 00279 Valeur_S32 = Valeur_S32 >> 8 ; 00280 Chaine_A_Emettre[3] = (U8)( Valeur_S32 & 0x000000FF) ; 00281 Chaine_A_Emettre[7] = 0x03 ; 00282 Chaine_A_Emettre[8] = 0x00 ; 00283 00284 if( Statut_Ordre_En_Cours != ATTENTE ) 00285 { 00286 Ordre_En_Cours = Chaine_A_Emettre [1] ; 00287 Emission ( &Chaine_A_Emettre [0] , 8 ) ; 00288 Statut_Ordre_En_Cours = ATTENTE ; 00289 00290 } 00291 } 00292 /*******************************************************************/ 00293 /* */ 00294 /* Emission */ 00295 /* */ 00296 /* Emission d'une trame vers le variateur */ 00297 /* */ 00298 /* */ 00299 /*******************************************************************/ 00300 void Pilote::Emission ( U8 *pChaine , U8 Longueur ) 00301 { 00302 U8 Index = 0 ; 00303 00304 if ( pPort->writeable() ) 00305 { 00306 // Mesure du timeout 00307 Debut_Emission_ms = Chrono_Pilote.read_ms() ; 00308 00309 while ( Index < Longueur ) 00310 { 00311 pPort->putc( *pChaine ) ; 00312 //PC2.printf("\r\n %i: %i \r\n" ,Id, *pChaine) ; 00313 pChaine++ ; 00314 Index++ ; 00315 } 00316 } 00317 else 00318 { 00319 PC2.printf("\r\n %i: Bloque \r\n" ,Id) ; 00320 } 00321 00322 } 00323 /*******************************************************************/ 00324 /* */ 00325 /* Reception */ 00326 /* */ 00327 /* Reception de la reponse du variateur */ 00328 /* */ 00329 /* */ 00330 /*******************************************************************/ 00331 void Pilote::Reception ( void ) 00332 { 00333 U8 Index1 = 0 ; 00334 U8 Index2 = 0 ; 00335 S16 Valeur_S16 ; 00336 S32 Valeur_S32 ; 00337 U8 *pChaine ; 00338 00339 while ( pPort->readable() ) 00340 { 00341 // Réception des caractères si il y en a 00342 *pReception = pPort->getc() ; 00343 //PC2.printf("\r\n %i",*pReception ) ; 00344 pReception++ ; 00345 Nb_Caracteres_Recus++ ; 00346 } 00347 00348 pChaine = &Chaine_Recue[0] ; 00349 00350 // La plus petite trame comporte 3 caractères 00351 if ( Nb_Caracteres_Recus > 2 ) 00352 { 00353 while ( *pChaine != 0x02 ) 00354 { 00355 // Recherche d'un début de trame: STX = 0x02 00356 if ( Index2 == Nb_Caracteres_Recus ) 00357 { 00358 // Pas de début de trame, on ecrase les caractères reçus et on s'en va! 00359 pReception = &Chaine_Recue[0] ; 00360 Nb_Caracteres_Recus = 0 ; 00361 //PC2.printf("\r\n Poubelle %i \r\n" ,Id) ; 00362 return ; 00363 } 00364 pChaine++ ; 00365 Index2++ ; 00366 } 00367 /**********************************************************************************/ 00368 // Ordre d'arret 00369 if ( *(pChaine+1) == ORDRE_ARRET ) 00370 { 00371 //PC2.printf("\r\n Ordre d'Arret %i\r\n" ,Id) ; 00372 if ( Nb_Caracteres_Recus < ( Index2 + 4 ) ) 00373 { 00374 // Trame incomplete, on reviendra 00375 //PC2.printf("\r\n Arret incomplet %i \r\n" ,Id) ; 00376 return ; 00377 } 00378 // Vérifie le caractere de fin de trame 00379 if ( *(pChaine+4) == 0x03 ) 00380 { 00381 // Trame OK, on efface 00382 //PC2.printf("\r\n Arret OK %i\r\n" ,Id) ; 00383 // On lit la mesure 00384 Valeur_S16 = (S16)(*(pChaine+2) ) * 256 + (S16)(*(pChaine+3)) ; 00385 Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ; 00386 00387 // Calcule la hauteur en mm 00388 Valeur_S16= (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ; 00389 00390 if ( ( Valeur_S16 > Hauteur_mini ) 00391 && ( Valeur_S16 < Hauteur_maxi ) ) 00392 { 00393 Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ; 00394 } 00395 Vitesse_Courante = 0 ; 00396 // On recale la trame réponse 00397 Index2 = Index2 + 5 ; 00398 00399 if ( Ordre_En_Cours == ORDRE_ARRET ) 00400 { 00401 Statut_Ordre_En_Cours = VALIDE ; 00402 Fin_Reception_ms = Chrono_Pilote.read_ms() ; 00403 Dernier_Ordre_Confirme = ORDRE_ARRET ; 00404 COM_OK = TRUE ; 00405 } 00406 } 00407 } 00408 /***************************************************************************************/ 00409 // Ordre de mouvement automatique 00410 else if ( ( *(pChaine+1) == ORDRE_DESCEND ) 00411 ||( *(pChaine+1) == ORDRE_MONTE ) ) 00412 { 00413 //PC2.printf("\r\n Ordre de mouvement %i\r\n",Id ) ; 00414 if ( Nb_Caracteres_Recus < ( Index2 + 6 ) ) 00415 { 00416 // Trame incomplete, on reviendra 00417 //PC2.printf("\r\n Mouvement Incomplet %i \r\n",Id ) ; 00418 return ; 00419 } 00420 // Vérifie le caractere de fin de trame 00421 if ( *(pChaine+6) == 0x03 ) 00422 { 00423 // Trame OK, on lit la vitesse 00424 Valeur_S16= (S16) (*(pChaine+2)) * 256 + (S16) (*(pChaine+3)) ; 00425 Vitesse_Courante = ( Valeur_S16 + Vitesse_Courante ) / 2 ; 00426 // et la mesure 00427 Valeur_S16 = (S16) (*(pChaine+4)) * 256 + (S16) (*(pChaine+5)) ; 00428 Mesure_Courante = ( Mesure_Courante + Valeur_S16 ) / 2 ; 00429 00430 // Calcule la hauteur en mm 00431 Valeur_S16 = (S16)((F32) Mesure_Courante * MM_par_Points + MM_Offset ) ; 00432 if ( ( Valeur_S16 > Hauteur_mini ) 00433 && ( Valeur_S16 < Hauteur_maxi ) ) 00434 { 00435 Hauteur_Courante = ( Valeur_S16 + Hauteur_Courante ) / 2 ; 00436 } 00437 00438 00439 // On recale la trame réponse 00440 Index2 = Index2 + 7 ; 00441 //PC2.printf("\r\n Ordre de mouvement %i OK\r\n",Id ) ; 00442 00443 if ( Ordre_En_Cours == *(pChaine+1) ) 00444 { 00445 Statut_Ordre_En_Cours = VALIDE ; 00446 Fin_Reception_ms = Chrono_Pilote.read_ms() ; 00447 COM_OK = TRUE ; 00448 Dernier_Ordre_Confirme = Ordre_En_Cours ; 00449 } 00450 } 00451 } 00452 /**********************************************************************************/ 00453 // Ordre de frein 00454 if ( *(pChaine+1) == ORDRE_FREIN ) 00455 { 00456 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ; 00457 if ( Nb_Caracteres_Recus < ( Index2 + 2 ) ) 00458 { 00459 // Trame incomplete, on reviendra 00460 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ; 00461 return ; 00462 } 00463 // Vérifie le caractere de fin de trame 00464 if ( *(pChaine+2) == 0x03 ) 00465 { 00466 // Trame OK 00467 //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ; 00468 // On recale la trame réponse 00469 Index2 = Index2 + 3 ; 00470 00471 if ( Ordre_En_Cours == ORDRE_FREIN ) 00472 { 00473 Statut_Ordre_En_Cours = VALIDE ; 00474 Fin_Reception_ms = Chrono_Pilote.read_ms() ; 00475 Dernier_Ordre_Confirme = ORDRE_FREIN ; 00476 COM_OK = TRUE ; 00477 } 00478 } 00479 } 00480 /**********************************************************************************/ 00481 // Ordre de lecture parametre 00482 if ( *(pChaine+1) == ORDRE_LECTURE ) 00483 { 00484 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ; 00485 if ( Nb_Caracteres_Recus < ( Index2 + 7 ) ) 00486 { 00487 // Trame incomplete, on reviendra 00488 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ; 00489 return ; 00490 } 00491 // Vérifie le caractere de fin de trame 00492 if ( *(pChaine+7) == 0x03 ) 00493 { 00494 // Trame OK 00495 //PC2.printf("\r\n Frein OK %i\r\n" ,Id) ; 00496 // Lecture du parametre 00497 Valeur_S32 = (S32) (*(pChaine+3)) * 256 + (S32) (*(pChaine+4)) ; 00498 Valeur_S32 = Valeur_S32 * 256 + (S32) (*(pChaine+5)) ; 00499 Parametres_Vario_S32[*(pChaine+2)] = Valeur_S32 * 256 + (S32) (*(pChaine+6)) ; 00500 // On recale la trame réponse 00501 Index2 = Index2 + 8 ; 00502 00503 if ( Ordre_En_Cours == ORDRE_LECTURE ) 00504 { 00505 Statut_Ordre_En_Cours = VALIDE ; 00506 Fin_Reception_ms = Chrono_Pilote.read_ms() ; 00507 Dernier_Ordre_Confirme = ORDRE_LECTURE ; 00508 COM_OK = TRUE ; 00509 } 00510 } 00511 } 00512 /**********************************************************************************/ 00513 // Ordre de configuration parametre 00514 if ( *(pChaine+1) == ORDRE_CONFIGURE ) 00515 { 00516 //PC2.printf("\r\n Ordre Frein %i\r\n" ,Id) ; 00517 if ( Nb_Caracteres_Recus < ( Index2 + 3 ) ) 00518 { 00519 // Trame incomplete, on reviendra 00520 //PC2.printf("\r\n Frein incomplet %i \r\n" ,Id) ; 00521 return ; 00522 } 00523 // Vérifie le caractere de fin de trame 00524 if ( *(pChaine+3) == 0x03 ) 00525 { 00526 // Trame OK 00527 //PC2.printf("\r\n Ecriture OK %i\r\n" ,Id) ; 00528 // Ecriture du parametre 00529 // On recale la trame réponse 00530 Index2 = Index2 + 4 ; 00531 00532 if ( Ordre_En_Cours == ORDRE_CONFIGURE ) 00533 { 00534 Statut_Ordre_En_Cours = VALIDE ; 00535 Fin_Reception_ms = Chrono_Pilote.read_ms() ; 00536 Dernier_Ordre_Confirme = ORDRE_CONFIGURE ; 00537 COM_OK = TRUE ; 00538 } 00539 } 00540 } 00541 /**********************************************************************************/ 00542 // La trame est traitée, on réaligne le reste des caracteres recus 00543 Index1 = 0 ; 00544 while ( Index2 < Nb_Caracteres_Recus ) 00545 { 00546 Chaine_Recue [ Index1 ] = Chaine_Recue [ Index2 ] ; 00547 Chaine_Recue [ Index2 ] = 0 ; 00548 //PC2.printf("\t %i:%i",Index1,Chaine_Recue [ Index1 ]); 00549 Index1++ ; 00550 Index2++ ; 00551 00552 } 00553 Nb_Caracteres_Recus = Index1 ; 00554 pReception = &Chaine_Recue[Nb_Caracteres_Recus] ; 00555 } 00556 // Gestion du timeout 00557 if ( Statut_Ordre_En_Cours == ATTENTE ) 00558 { 00559 if ( ( Chrono_Pilote.read_ms() - Debut_Emission_ms ) > TIMEOUT_RECEPTION_ms ) 00560 { 00561 Statut_Ordre_En_Cours = DEFAUT ; 00562 Compteur_Timeout++ ; 00563 COM_OK = FALSE ; 00564 //PC2.printf("\n\r *********** Timeout : %i - %i \n\r",Id,Compteur_Timeout); 00565 } 00566 } 00567 } 00568
Generated on Tue Jul 12 2022 13:32:39 by
1.7.2