summaryrefslogtreecommitdiff
path: root/2003/n/pic/asserv/src/routine.c
diff options
context:
space:
mode:
Diffstat (limited to '2003/n/pic/asserv/src/routine.c')
-rw-r--r--2003/n/pic/asserv/src/routine.c193
1 files changed, 193 insertions, 0 deletions
diff --git a/2003/n/pic/asserv/src/routine.c b/2003/n/pic/asserv/src/routine.c
new file mode 100644
index 0000000..4ab5e54
--- /dev/null
+++ b/2003/n/pic/asserv/src/routine.c
@@ -0,0 +1,193 @@
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// 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;
+
+ }
+