//--------------------------------------------------------------------- //--------------------------------------------------------------------- // Fonction sur les structures moteur //--------------------------------------------------------------------- //--------------------------------------------------------------------- //--------------------------------------------------------------------- // UpdTraj() // Calcul de la prochaine valeur pour la position commandée //--------------------------------------------------------------------- void UpdTraj(struct moteur *mot) { if( (*mot).encours ) { // Si pendant la première partie du déplacement if( !((*mot).phase) ) { // Accélérer (*mot).vact = (*mot).vact + (*mot).acc; if( (*mot).vact >= (*mot).vlim ) { // Incrémenter le compteur de saturation (*mot).vact = (*mot).vlim ; (*mot).cptsat++; } (*mot).tmp = (*mot).vact/65536; // Si la distance jusqu'à la fin de la première phase est négative ou nulle if( !etat.infini ) { // Calculer la nouvelle distance avant la fin de la première phase (*mot).phase1dist = (*mot).phase1dist - (*mot).tmp; if( ((*mot).phase1dist <= 0) ) { // La phase 2 du déplacement est atteinte (*mot).phase = 1; } } } // Si dans la seconde phase du déplacement else { // Si le compteur de saturation n'est pas arrivé à zéro if( (*mot).cptsat > 0) { // Le décrémenter (*mot).vact = (*mot).vlim ; (*mot).cptsat--; } // Si le robot doit déccélérer else { // Décélérer (*mot).vact = (*mot).vact - (*mot).acc ; // Si la vitesse commandée est négative if( (*mot).vact <= 0) { // Arrondir à 0 (*mot).vact = 0; // Le déplacement est fini (*mot).encours = 0; } } (*mot).tmp = (*mot).vact/65536; } // Calculer la nouvelle position commandée if((*mot).neg_move) { (*mot).pcom = (*mot).pcom - (*mot).tmp; } else { (*mot).pcom = (*mot).pcom + (*mot).tmp; } } } //--------------------------------------------------------------------- // UpdPos() // Récupération de la nouvelle position mesurée //--------------------------------------------------------------------- void UpdPos(struct moteur *mot) { // Récupérer les anciennes valeurs des compteurs (*mot).cptarr_p = (*mot).cptarr; (*mot).cptavt_p = (*mot).cptavt; // Ecrire les nouvelles valeurs de cptavt et de cptarr if((*mot).g) { (*mot).cptavt = 2*get_timer0(); (*mot).cptarr = get_timer3(); } else { (*mot).cptavt = timer0Prime; (*mot).cptarr = timer1Prime; } (*mot).pmes = (*mot).pmes + (signed int32)((*mot).cptavt) - (signed int32) ((*mot).cptarr) - (signed int32) ((*mot).cptavt_p) + (signed int32) ((*mot).cptarr_p); if( (*mot).cptavt_p > (*mot).cptavt) { (*mot).pmes = (*mot).pmes +65536; } if( (*mot).cptarr_p > (*mot).cptarr) { (*mot).pmes = (*mot).pmes - 65536; } } //--------------------------------------------------------------------- // CalcError() // Calcul de l'erreur et mise sur 16 bits pour la multiplication à venir //--------------------------------------------------------------------- void CalcError(struct moteur *mot) { (*mot).u = (*mot).pcom/256 - (*mot).pmes; } //--------------------------------------------------------------------- // CalcPID() // Calcul de l'asservissement //--------------------------------------------------------------------- void CalcPID(struct moteur *mot) { // Calcul pour le terme proportionnel (*mot).ypid = (*mot).u * kp; if( (*mot).ypid >0 ) { (*mot).sens = 1; } else { (*mot).sens = 0; } (*mot).pwm = abs( (*mot).ypid ); if( (*mot).pwm < 2 ) { (*mot).pwm = 0; } else { if( (*mot).pwm < 20 ) { (*mot).pwm = 20; } else { if( (*mot).pwm > 1022 ) { (*mot).pwm = 1022; } } } } //--------------------------------------------------------------------- // nv_dep() // Initialise les valeurs quand un nouveau déplacement est demandé //--------------------------------------------------------------------- void nv_dep(struct moteur *mot) { (*mot).acc = ((*mot).accsuiv)/128; (*mot).acc = ((*mot).acc)/2; (*mot).neg_move = (*mot).neg_movesuiv; (*mot).phase = 0; (*mot).cptsat = 0; // Calcul de la distance à parcourir lors de la phase 1 (*mot).phase1dist = (*mot).pfinsuiv/2; // Recopier les paramètres du déplacement en attente (*mot).vlim = (*mot).vlimsuiv; }