Le Pont 3R
Version initiale
Dependencies: mbed
Fork of
Le_Pont_V10116
by 
SAGNES Christophe
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 23:24:06 by
1.7.2