summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2004-06-27 11:53:30 +0000
committerschodet2004-06-27 11:53:30 +0000
commita9f3689ee85da91a51d01a76a469cbb5298806d2 (patch)
treeed10ed02c6ffe5bb53548e1da0d15d6be13f4d92
parent9c5225470159aad23dac1c3903a518c1b8be6236 (diff)
Initial revision
-rw-r--r--2003/n/pic/asserv/src/asserv.c912
-rw-r--r--2003/n/pic/asserv/src/asserv.h187
-rw-r--r--2003/n/pic/asserv/src/dial.c185
-rw-r--r--2003/n/pic/asserv/src/routine.c193
4 files changed, 1477 insertions, 0 deletions
diff --git a/2003/n/pic/asserv/src/asserv.c b/2003/n/pic/asserv/src/asserv.c
new file mode 100644
index 0000000..2d8ee6a
--- /dev/null
+++ b/2003/n/pic/asserv/src/asserv.c
@@ -0,0 +1,912 @@
+// Ce programme permet l'asservissement en vitesse et en position de moteur
+// à courant continu. Le retour se fait par l'intermédiare d'une roue codeuse.
+
+ #include "asserv.h"
+ #include "dial.c"
+ #include "routine.c"
+
+
+
+ //---------------------------------------------------------------------
+ // finEnvoi()
+ // Cette fonction permet de vérifier la trame reçue et de faire le traitement adéquat
+ //---------------------------------------------------------------------
+ void finEnvoi(void)
+ {
+ unsigned char chaine[15];
+ unsigned char cmdsuiv;
+ unsigned infini=0, rotation=0, ar_g=0, ar_d=0;
+ unsigned int j=0, erreur=0, t=1, nb_para=1;
+ unsigned int32 para1, para2, para3;
+ unsigned int32 mul_para1, mul_para2, mul_para3;
+ if(inpbuf[0]==BOT)
+ {
+ cmdsuiv=inpbuf[1];
+
+ if(cmdsuiv==AV_FIX || cmdsuiv==AR_FIX || cmdsuiv==GA_FIX || cmdsuiv==DR_FIX || //
+ cmdsuiv==AV_LIB || cmdsuiv==AR_LIB || cmdsuiv==GA_LIB || cmdsuiv==DR_LIB || //
+ cmdsuiv==STOP || cmdsuiv==CALIB || cmdsuiv==RAZ || cmdsuiv==RCG || //
+ cmdsuiv==RCD || cmdsuiv==RM || cmdsuiv==ERR || cmdsuiv==PID || //
+ cmdsuiv==ETA || cmdsuiv==ETASUIV || cmdsuiv==STR || cmdsuiv==CAPTEUR || //*
+ cmdsuiv==RSCD || cmdsuiv==RSCG || cmdsuiv==REGP || cmdsuiv==REGD ||
+ cmdsuiv==PLUS || cmdsuiv==MOINS || cmdsuiv==READC || cmdsuiv==PSTOP ||
+ cmdsuiv==SERVO)
+ {
+ do
+ {
+ t++;
+ j=0;
+
+ // Récupération des paramètres
+ while((inpbuf[t]!=SEP && inpbuf[t]!=EOT) || inpbuf[t-1]==ECH)
+ {
+ chaine[j]=inpbuf[t];
+ t++;
+ j++;
+ }
+
+ if(t>2)
+ {
+ // Indiquer la fin de trame
+ chaine[j]='\0';
+
+ // Vérification des paramètres
+ if(validation(chaine))
+ {
+ if (nb_para==1)
+ {
+ para1=decodage(chaine);
+
+ }
+ else
+ {
+ if (nb_para==2)
+ {
+ para2=decodage(chaine);
+ }
+ else
+ {
+ if (nb_para==3)
+ {
+ para3=decodage(chaine);
+ }
+ else
+ {
+ if(nb_para>=4)
+ {
+ erreur=ERREUR_TRAME;
+ }
+ }
+ }
+ }
+
+ nb_para++;
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ }
+ }while(erreur==0 && inpbuf[t]!=EOT && inpbuf[t-1]!=ECH);
+ }
+ else
+ {
+ erreur = ERREUR_CMD;
+ }
+
+ // Si les paramètres sont corrects, continuer
+ if(erreur==0)
+ {
+ switch(cmdsuiv)
+ {
+ case REGP :
+ if(nb_para==2)
+ {
+ printf("%c%c%c", BOT, REGP, EOT);
+ kp = para1;
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case CAPTEUR :
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%lu;%lu;%lu%c", BOT, CAPTEUR, gauche.cptavt, gauche.cptarr, droit.cptavt, droit.cptarr, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case STR :
+ if(nb_para==1)
+ {
+ printf("%c%c%u;%u;%u;%u;%u;%u;%u;%u%c", BOT, STR, gauche.phase, gauche.neg_move, gauche.encours, gauche.g, droit.phase, droit.neg_move, droit.encours, droit.g, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case ETA :
+ if(nb_para==1)
+ {
+ printf("%c%c%u;%u;%u;%u;%u;%u%c", BOT, ETA, etat.infini, etat.attente, etat.stoppe, etat.interr, etat.calibr, etat.reset, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case ETASUIV :
+ if(nb_para==1)
+ {
+ printf("%c%c%u;%u;%u;%u;%u;%u%c", BOT, ETASUIV, etatsuiv.infini, etatsuiv.attente, etatsuiv.stoppe, etatsuiv.interr, etatsuiv.calibr, etatsuiv.reset, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RM :
+ if(nb_para==1)
+ {
+ int32 dmes;
+ int32 gmes;
+ dmes = dmes / kdist;
+ gmes = gmes / kdist;
+ printf("%c%c%c%c%c%c", BOT, RM, (gmes >> 24) & 0xff,(gmes >> 16) & 0xff,
+ (gmes >> 8) & 0xff,gmes & 0xff);
+ printf(";%c%c%c%c%c", (dmes >> 24) & 0xff, (dmes >> 16) & 0xff,
+ (dmes >> 8) & 0xff, dmes & 0xff, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RCG :
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%ld;%ld;%lu;%lu;%ld%c", BOT, RCG, gauche.acc, gauche.vact, gauche.pcom, gauche.vlim, gauche.pfin, gauche.phase1dist, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RSCG :
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%lu;%lu%c", BOT, RSCG, gauche.accsuiv, gauche.vlimsuiv, gauche.pfinsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case ERR :
+ if(nb_para==1)
+ {
+ printf("%c%c%ld;%ld%c", BOT, ERR, gauche.u, droit.u, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case PID :
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%lu%c", BOT, PID, gauche.pwm, droit.pwm, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RCD:
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%ld;%ld;%lu;%lu;%ld%c", BOT, RCD, droit.acc, droit.vact, droit.pcom, droit.vlim, droit.pfin, droit.phase1dist, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RSCD :
+ if(nb_para==1)
+ {
+ printf("%c%c%lu;%lu;%lu%c", BOT, RSCD, droit.accsuiv, droit.vlimsuiv, droit.pfinsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case RAZ :
+ if(nb_para==1)
+ {
+ etat.reset = 1;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case STOP :
+ if(nb_para==1)
+ {
+ etat.attente = 1;
+ etat.stoppe=1;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case PSTOP:
+ if(nb_para==1)
+ {
+ gauche.phase = 1;
+ droit.phase = 1;
+ gauche.cptsat = 0;
+ droit.cptsat = 0;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case CALIB :
+ if(nb_para==1)
+ {
+ etatsuiv.calibr=1;
+ nb_int =0;
+ amcg =0;
+ vmcg=0;
+ amcd =0;
+ vmcd=0;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case PLUS :
+ if(nb_para==1)
+ {
+ kp++;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case MOINS :
+ if(nb_para==1)
+ {
+ kp--;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case READC:
+ if(nb_para==1)
+ {
+ printf("%c%c%c%c", BOT, cmdsuiv, input_d(), EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ case SERVO:
+ if(nb_para==2)
+ {
+ int s;
+ s = para1;
+ output_bit (PIN_B3, s & 1);
+ output_bit (PIN_B4, (s >> 1) & 1);
+ output_bit (PIN_B5, (s >> 2) & 1);
+ output_bit (PIN_B6, (s >> 3) & 1);
+ // Clk.
+ output_high (PIN_B2);
+ delay_cycles (200);
+ output_low (PIN_B2);
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ else
+ {
+ erreur=ERREUR_PARA;
+ }
+ break;
+ default :
+ if(!etatsuiv.calibr)
+ {
+ etatsuiv.attente=0;
+
+ // Indique que le mouvement désiré est un mouvement dont la distance est commandée
+ if(cmdsuiv==AV_FIX || cmdsuiv==AR_FIX || cmdsuiv==GA_FIX || cmdsuiv==DR_FIX)
+ {
+ infini=0;
+ }
+ else
+ {
+ infini=1;
+ }
+
+ // Indique que le mouvement desiré est une translation
+ if(cmdsuiv==AV_FIX || cmdsuiv==AR_FIX || cmdsuiv==AV_LIB || cmdsuiv==AR_LIB)
+ {
+ rotation=0;
+ }
+ else
+ {
+ rotation=1;
+ }
+
+ // Indique les sens des moteurs
+ if(cmdsuiv==AV_FIX || cmdsuiv==AV_LIB)
+ {
+ ar_g = 1;
+ ar_d = 1;
+ }
+ // Indique les sens des moteurs
+ else
+ {
+ if(cmdsuiv==AR_FIX || cmdsuiv==AR_LIB)
+ {
+ ar_g = 0;
+ ar_d = 0;
+ }
+ else
+ {
+ // Indique les sens des moteurs
+ if(cmdsuiv==GA_FIX || cmdsuiv==GA_LIB)
+ {
+ ar_g = 0;
+ ar_d = 1;
+ }
+ else
+ {
+ ar_g = 1;
+ ar_d = 0;
+ }
+ }
+ }
+
+ // Si pas de paramètres spécifiés
+ if(nb_para==1)
+ {
+ // Pour un mouvement fini, il faut obligatoirement 1 paramètre
+ if(!infini)
+ {
+ erreur = ERREUR_PARA;
+ }
+ else
+ {
+ gauche.accsuiv = accel_def_fix;
+ droit.accsuiv = accel_def_fix;
+ gauche.vlimsuiv = vlim_def_lib;
+ droit.vlimsuiv = vlim_def_lib;
+ }
+ }
+ else
+ {
+ if(nb_para==4 && infini)
+ {
+ erreur = ERREUR_PARA;
+ }
+ else
+ {
+ // Le premier paramètre existe sinon, le calculer
+ if(!infini)
+ {
+ if(!rotation)
+ {
+ mul_para1 = kdist;
+ }
+ else
+ {
+ mul_para1 = kangle;
+ }
+
+ droit.pfinsuiv = mul_para1 * para1;
+ gauche.pfinsuiv = droit.pfinsuiv;
+ }
+ else
+ {
+ if(!rotation)
+ {
+ mul_para1 = kacc;
+ }
+ else
+ {
+ mul_para1 = krot;
+ }
+
+ droit.accsuiv = mul_para1 * para1;
+ gauche.accsuiv = droit.accsuiv;
+ }
+
+ // Si plus d'1 paramètre a été spécifié
+ if(nb_para>2)
+ {
+ if(!infini)
+ {
+ if(!rotation)
+ {
+ mul_para2 = kacc;
+ }
+ else
+ {
+ mul_para2 = krot;
+ }
+
+ droit.accsuiv = mul_para2 * para2;
+ gauche.accsuiv = droit.accsuiv;
+ }
+ else
+ {
+ if(!rotation)
+ {
+ mul_para2 = kvit;
+ }
+ else
+ {
+ mul_para2 = kfre;
+ }
+
+ droit.vlimsuiv = mul_para2 * para2;
+ gauche.vlimsuiv = droit.vlimsuiv;
+ }
+
+ // Si 3 paramètres ont été spécifiés
+ if(nb_para==4)
+ {
+ if(!rotation)
+ {
+ mul_para3 = kvit;
+ }
+ else
+ {
+ mul_para3 = kfre;
+ }
+
+ droit.vlimsuiv = mul_para3 * para3;
+ gauche.vlimsuiv = droit.vlimsuiv;
+ }
+ // Sinon, 2 paramètres ont été spécifiés
+ else
+ {
+ if(!infini)
+ {
+ gauche.vlimsuiv = vlim_def_fix;
+ droit.vlimsuiv = vlim_def_fix;
+ }
+ }
+ }
+ // Si 1 paramètre a été spécifié
+ else
+ {
+ if(!infini)
+ {
+ gauche.accsuiv = accel_def_fix;
+ droit.accsuiv = accel_def_fix;
+ gauche.vlimsuiv = vlim_def_fix;
+ droit.vlimsuiv = vlim_def_fix;
+ }
+ else
+ {
+ gauche.vlimsuiv = vlim_def_lib;
+ droit.vlimsuiv = vlim_def_lib;
+ }
+ }
+ }
+ }
+
+ if(erreur==0)
+ {
+ etatsuiv.infini = infini;
+ gauche.neg_movesuiv=ar_g;
+ droit.neg_movesuiv=ar_d;
+ etatsuiv.attente=1;
+ printf("%c%c%c", BOT, cmdsuiv, EOT);
+ }
+ }
+ break;
+ }
+ }
+ }
+ else
+ {
+ erreur=ERREUR_TRAME;
+ }
+
+ if(erreur!=0)
+ {
+ printf("%c%c%c%c",BOT, ERREUR_ID, erreur, EOT);
+ }
+
+ cmdsuiv = '0';
+ }
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Déclaration des traitements à faire si interruptions
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ //---------------------------------------------------------------------
+ // Définition de l'interruption pour TIMER2
+ // Refaire une remise à jour quand cette interruption s'est produite
+ //---------------------------------------------------------------------
+// #int_TIMER2
+// TIMER2_isr()
+ void routine (void)
+ {
+// #byte wreg=0xFE8
+// sauvegarde[ptr++] = wreg;
+ // Calcul des nouvelles position à commander
+ UpdTraj(&gauche);
+ UpdTraj(&droit);
+
+ // Récupération de la valeur mesurée de la position
+ UpdPos(&gauche);
+ UpdPos(&droit);
+
+ // Calcul de l'erreur
+ CalcError(&gauche);
+ CalcError(&droit);
+
+ // Calcul de la commande après passage par le PID
+ CalcPID(&gauche);
+ CalcPID(&droit);
+
+ // Ecriture de la nouvelle valeur du rapport cyclique
+ if(gauche.sens==1)
+ {
+ output_high(PIN_D0);
+ }
+ else
+ {
+ output_low(PIN_D0);
+ }
+
+ if(droit.sens==1)
+ {
+ output_high(PIN_D1);
+ }
+ else
+ {
+ output_low(PIN_D1);
+ }
+
+ set_pwm1_duty( gauche.pwm );
+ set_pwm2_duty( droit.pwm );
+
+
+ if( etat.attente )
+ {
+ if( ( !(gauche.encours) && !(droit.encours) ) || etat.stoppe)
+ {
+ etat.attente = 0;
+ if(etat.stoppe)
+ {
+ droit.encours = 0;
+ gauche.encours = 0;
+ etat.stoppe = 0;
+ gauche.pcom = gauche.pmes*256;
+ droit.pcom = droit.pmes*256;
+ gauche.vact = 0;
+ droit.vact = 0;
+ gauche.vmes = 0;
+ droit.vmes = 0;
+ gauche.pwm = 0;
+ droit.pwm = 0;
+ }
+ printf("%c%c%c", BOT, FIN, EOT);
+ }
+ }
+
+ // Si un déplacement est en attente
+ if(etatsuiv.attente )
+ {
+ // S'il s'agit d'un mouvement infini, remettre à jour la vitesse uniquement
+ if(etat.infini)
+ {
+ gauche.vlim = gauche.vlimsuiv;
+ droit.vlim = droit.vlimsuiv;
+ etatsuiv.attente = 0;
+ }
+ else
+ {
+ if( !(etat.attente) )
+ {
+ // Recopier les paramètres du déplacement
+ etat.stoppe = 0;
+ etat.infini = etatsuiv.infini;
+ etat.attente = 1;
+ etatsuiv.attente = 0;
+ nv_dep(&gauche);
+ nv_dep(&droit);
+ droit.encours = 1;
+ gauche.encours = 1;
+ etatsuiv.attente = 0;
+ }
+ }
+ }
+// wreg = sauvegarde[--ptr];
+ }
+
+ //---------------------------------------------------------------------
+ // Définition pour une interruption externe
+ //---------------------------------------------------------------------
+ #int_EXT
+ EXT_isr()
+ {
+ timer0Prime++;
+ output_high(PIN_B7);
+ }
+
+ #int_EXT1
+ EXT1_isr()
+ {
+ timer1Prime++;
+ output_low(PIN_B7);
+ }
+
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Initialisation des variables et des registres de périphériques
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ void initialisation(void)
+ {
+ //----------
+ // Configuration des registres pour faire fonctionner le PIC
+ //----------
+ // Configuration de la liaison série (voir le .h)
+ setup_psp(PSP_DISABLED);
+
+ // Configuration de l'interface SPI : non utilisée
+ setup_spi(FALSE);
+
+ // Configuration du chien de garde
+ setup_wdt(WDT_OFF);
+
+ // Configuration du TIMER 0 pour lire le nombre de pas faits en reculant
+ setup_timer_0(RTCC_EXT_L_TO_H);
+
+ // Confguration du TIMER 1 pour lire le nombre de pas faits en avançant
+ setup_timer_1(T1_DISABLED);
+
+ // Configuration du TIMER 2 pour une fréquence de PWM de 19.53kHz
+ setup_timer_2(T2_DIV_BY_16,255,1);
+
+ // Configuration du TIMER 3 : non utilisé
+ setup_timer_3(T3_EXTERNAL|T3_DIV_BY_1);
+
+ //Configuration de l'ADC
+ setup_adc_ports(NO_ANALOGS);
+ setup_adc(ADC_CLOCK_DIV_2);
+
+ // Configuration des registres pour un fonctionnement en PWM
+ setup_ccp1(CCP_PWM);
+ setup_ccp2(CCP_PWM);
+
+ // Configuration pour l'autorisation des interruptions
+
+// enable_interrupts(INT_TIMER2); //TIMER2 c'est pas mieux ?
+ enable_interrupts(INT_EXT);
+ enable_interrupts(INT_EXT1);
+ enable_interrupts(global);
+
+ output_low (PIN_B2);
+
+}
+
+void redem(void)
+{
+
+ // Ton de 50%
+ timer0Prime=0;
+ timer1Prime=0;
+
+ set_timer0(0);
+ set_timer3(0);
+
+ gauche.encours = 0;
+ droit.encours = 0;
+ gauche.neg_move = 0;
+ droit.neg_move = 0;
+ etatsuiv.attente = 0; // Aucun mouvement en attente
+ etat.attente = 0; // Aucun deplacement en cours
+ etatsuiv.calibr=0;
+ //----------
+ // Initialisation des variables et des registres de périphériques
+ //----------
+ // Variables diverses
+
+ etat.reset=0;
+
+ kp=KP_ASSERV;
+
+ kdist = KDISTANCE;
+ kvit = KVITESSE;
+ kacc = KACCEL;
+ kangle = KANG;
+ kfre = KFREQ;
+ krot = KROTATION;
+
+ accel_def_fix = ACCEL_DEF_FIX_C;
+ vlim_def_fix = VLIM_DEF_FIX_C;
+ pos_def_fix = POS_DEF_FIX_C;
+ vlim_def_lib = VLIM_DEF_LIB_C;
+
+
+ //----------
+ // Différents registres de contrôle
+ //----------
+
+
+ // Positionnement des différents flags à 0
+ gauche.sens = 1;
+ droit.sens = 1;
+
+ gauche.g=1;
+ droit.g=0;
+
+ gauche.pcom = 0;
+ droit.pcom = 0;
+
+ gauche.pfin = 0;
+ droit.pfin = 0;
+
+ gauche.pfinsuiv = 0;
+ droit.pfinsuiv = 0;
+
+ gauche.phase1dist = 0;
+ droit.phase1dist = 0;
+
+ gauche.pmes = 0;
+ droit.pmes = 0;
+
+ gauche.u = 0;
+ droit.u = 0;
+
+ gauche.ypid = 0;
+ droit.ypid = 0;
+
+ gauche.pwm = 0;
+ droit.pwm = 0;
+
+ gauche.vact = 0;
+ droit.vact = 0;
+
+ gauche.vlim = 0;
+ droit.vlim = 0;
+
+ gauche.vlimsuiv = 0;
+ droit.vlimsuiv = 0;
+
+ gauche.vmes = 0;
+ droit.vmes = 0;
+
+ gauche.acc = 0;
+ droit.acc = 0;
+
+ gauche.accsuiv = 0;
+ droit.accsuiv = 0;
+
+ gauche.cptavt = 0;
+ droit.cptavt = 0;
+
+ gauche.cptarr = 0;
+ droit.cptarr = 0;
+
+ gauche.cptavt_p = 0;
+ droit.cptavt_p = 0;
+
+ gauche.cptarr_p = 0;
+ droit.cptarr_p = 0;
+
+ gauche.cptsat = 0;
+ droit.cptsat = 0;
+
+ idxbuf = 0;
+ ptr =0;
+
+ printf("%c%c%c",BOT, REDEMARRE, EOT);
+
+ }
+
+
+
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// main()
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ void main(void)
+ {
+ etat.interr=1;
+ // Initialisation des registres de configuration et des variables
+ initialisation();
+ ext_int_edge(0, L_TO_H);
+ etat.interr=0;
+
+ while(1)
+ {
+ etat.interr=1;
+ // Initialisation des registres de configuration et des variables
+ redem();
+ etat.interr=0;
+
+ // Boucle infinie
+ while(etat.reset==0)
+ {
+ if(!kbhit())
+ {
+ routine();
+ }
+ else
+ {
+ // Si un caractère a été reçu
+ // Le caractère est mis dans le buffer de réception
+ inpbuf[idxbuf] = getc();
+
+ // Si un dépassement de buffer a lieu
+ if(idxbuf>LG_INPBUF-1)
+ {
+ // Remettre le buffer à 0
+ idxbuf=0;
+ }
+
+ // Si la trame est terminée par une EOT, analyser la trame reçue
+ if(inpbuf[idxbuf] == EOT )
+ {
+ if(idxbuf>0)
+ {
+ if( inpbuf[idxbuf-1]!=ECH)
+ {
+ // Remettre le compteur de lettres reçues à 0
+ idxbuf=0;
+
+ // Appeler la fonction traitant la trame reçue
+ etat.interr=1;
+ finEnvoi();
+ etat.interr=0;
+ }
+ else
+ {
+ idxbuf++;
+ }
+ }
+ else
+ {
+ printf("%c%c%c%c", BOT, ERREUR_ID, ERREUR_TRAME, EOT);
+ }
+ }
+ // Sinon, incrémenter le compteur du buffer
+ else
+ {
+ idxbuf++;
+ }
+ }
+ }
+ }
+ }
diff --git a/2003/n/pic/asserv/src/asserv.h b/2003/n/pic/asserv/src/asserv.h
new file mode 100644
index 0000000..8fca605
--- /dev/null
+++ b/2003/n/pic/asserv/src/asserv.h
@@ -0,0 +1,187 @@
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Définition des fichiers à inclure
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ #include <18f452.h> // Définition des registres du PIC18
+ #include <stdlib.h> // Définition de la librairie standard
+
+// Pattes libres.
+/*
+40 rb7 (coupee)
+39 rb6 : D3
+38 rb5 : D2
+37 rb4 : D1
+36 rb3 : D0
+35 rb2 : Clk busp
+30 rd7
+29 rd6 : Capteur sharp
+28 rd5 : Capteur doigts
+27 rd4 : Capteur couleur
+24 rc5
+23 rc4
+22 rd3 : Jack
+21 rd2
+18 rc3
+10 re2
+9 re1
+8 re0
+7 ra5
+5 ra3
+4 ra2
+3 ra1
+
+A1-5 B2-7 C4-5 D2-6 E0-2
+
+
+*/
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// General configuration
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ #fuses HS,NOWDT,WDT1
+ #use delay(clock=20000000)
+ #use rs232(baud=9600,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8)
+// #use standard_io(B)
+// #priority ext
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Définition des constantes
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ #define LG_INPBUF 50 // Longueur du buffeur de réception
+
+ #define BOT '<' // Caractère de début de trame
+ #define EOT '>' // Caractère de fin de trame
+ #define SEP ';' // Caractère de séparation de paramètres
+ #define ECH '\\' // Caractère d'échappement
+ #define BCD '#' // Caractère indiquant un nombre codé en BCD
+ #define AV_FIX 'F' // Caractère traduisant une avancée avec une distance fixée
+ #define AR_FIX 'R' // Caractère traduisant un recul avec une distance fixée
+ #define DR_FIX 'D' // Caractère traduisant une rotation à droite avec un angle fixé
+ #define GA_FIX 'G' // Caractère traduisant une rotation à gauche avec un angle fixé
+ #define AV_LIB 'f' // Caractère traduisant une avancée avec une distance libre
+ #define AR_LIB 'r' // Caractère traduisant un recul avec une distance libre
+ #define DR_LIB 'd' // Caractère traduisant une rotation à droite avec un angle libre
+ #define GA_LIB 'g' // Caractère traduisant une rotation à gauche avec un angle fixé
+ #define STOP 's' // Caractère demandant un arret du robot
+ #define PSTOP 'S' // Caractère demandant un arret du robot
+ #define FIN 'Y' // Caractère traduisant la fin du deplacement
+ #define CALIB 'C' // Caractère traduisant un calibrage
+ #define RAZ 'Z' // Caractère traduisant un reglage de l'asservissement
+ #define RCG 'a'
+ #define RCD 'p'
+ #define RSCG 'A'
+ #define RSCD 'P'
+ #define RM 'q'
+ #define ERR 'u'
+ #define PID 'y'
+ #define ETA 'e'
+ #define ETASUIV 'E'
+ #define STR 'b'
+ #define CAPTEUR 'c'
+ #define REDEMARRE 'Y'
+ #define REGD 'w'
+ #define REGP 'x'
+ #define PLUS '+'
+ #define MOINS '-'
+ #define READC 'h'
+ #define SERVO 'H'
+
+ #define ERREUR_ID '?' // Caractère indiquant une erreur dans la transmission
+ #define ERREUR_TRAME '1' // Erreur dans la trame
+ #define ERREUR_PARA '2' // Erreur dans les paramètres
+ #define ERREUR_CMD '3' // Erreur dans les commandes
+
+ #define KDISTANCE 11000 // Valeur du coefficient pour transformer la distance
+ #define KANG 23300 // Valeur du coefficient pour transformer l'angle
+ #define POS_DEF_FIX_C 10995750 // Position maximale pour faire le test de KP
+
+ #define KVITESSE 1441896 // Valeur du coefficient pour transformer la vitesse
+ #define KACCEL 738250 // Valeur du coefficient pour transformer l'accélération
+ #define KFREQ 1441896 // Valeur du coefficient pour transformer la vitesse de rotation
+ #define KROTATION 738250 // Valeur du coefficient pour transformer l'accélération de rotation
+ #define ACCEL_DEF_FIX_C 738250497 // Accélération par défaut : 1m/s² soit 576 °/s²
+ #define VLIM_DEF_FIX_C 1441895503 // Vitesse limite par défaut si distance connue : 1m/s soit 576 °/s
+ #define VLIM_DEF_LIB_C 144189550 // Vitesse limite par défaut si distance non spécifiée : 78mm/s et 45°/s
+
+ #define KP_ASSERV 1 // Valeur du Kp pour un asservissement
+
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Définitions de structure
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+
+ // Etat général du déplacement
+ struct {
+ unsigned infini :1; // Indique si le déplacement doit être fait en boucle
+ unsigned attente :1; // Indique si un déplacement est en attente d'exécution
+ unsigned stoppe :1; // Indique si un déplacement doit être arrêté
+ unsigned interr :1; // Masquage pour les interruptions
+ unsigned calibr :1; // Calibration de la vitesse et de l'acceleration
+ unsigned reset :1; // Calibration de la vitesse et de l'acceleration
+ } etat, etatsuiv;
+
+ struct moteur
+ {
+ unsigned phase :1; // Indique si dans la 1° ou 2° partie du déplacement
+ unsigned neg_move :1; // Indique un mouvement dans le sens négatif
+ unsigned neg_movesuiv :1; // Indique un mouvement dans le sens négatif
+ unsigned encours :1; // Indique qu'un déplacement est en cours
+ unsigned g :1; // Indique qu'un déplacement est en cours
+ unsigned sens :1; // Indique qu'un déplacement est en cours
+ unsigned long cptavt;
+ unsigned long cptarr; // Compteurs indiquant un pas fait en avant ou en arrière
+ unsigned long cptavt_p;
+ unsigned long cptarr_p; // Compteurs indiquant un pas fait en avant ou en arrière
+ unsigned int32 cptsat; // Compte le nombre de périodes d'échantillonnage pour lesquels Vlim est atteinte
+ unsigned int32 vlim;
+ unsigned int32 vlimsuiv; // Vitesse limite pour le déplacement
+ unsigned int32 acc;
+ unsigned int32 accsuiv; // Valeur de l'accélération pour le segment
+ unsigned int32 pfin;
+ unsigned int32 pfinsuiv; // Position finale souhaitée
+ signed int32 phase1dist; // Distance que doit durer la phase1 (D/2)
+ signed int32 vmes; // Vitesse mesurée
+ signed int32 u; // Erreur entre la consigne et la valeur mesurée
+ signed int32 ypid; // Sauvegarde de la valeur en sortie du PID
+ unsigned int32 pwm; // Sauvegarde de la valeur en sortie du PID
+ signed int32 vact; // Vitesse commandée
+ signed int32 pcom; // Position commandée
+ signed int32 pmes; // Position mesurée
+ signed int32 tmp;
+ };
+
+
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Déclaration des variables
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ char inpbuf[LG_INPBUF]; // Buffer de commande pour la liaison avec un PC
+ unsigned int idxbuf; // Index du buffer de commande
+ unsigned long nb_int; // Indique le nombre d'interruption
+ unsigned long vmcg, amcg;
+ unsigned long vmcd, amcd;
+
+ unsigned long kp; // Coefficients du PID pour l'asservissement
+ unsigned int32 kdist; // Coefficient pour transformer la distance en nb d'impulsions
+ unsigned int32 kvit; // Coefficient pour transformer la vitesse en nb d'impulsions
+ unsigned int32 kacc; // Coefficient pour transformer l'accélération en nb d'impulsions
+ unsigned int32 kangle; // Coefficient pour transformer l'angle en nb d'impulsions
+ unsigned int32 kfre; // Coefficient pour transformer la vitesse de rotation en nb d'impulsions
+ unsigned int32 krot; // Coefficient pour transformer l'accélération de rotation en nb d'impulsions
+ unsigned long timer0Prime, timer1Prime; // Compteurde pas fait en avant et en arrière
+
+ int32 accel_def_fix, vlim_def_fix, vlim_def_lib, pos_def_fix;
+ int wreg_sauv;
+
+ struct moteur gauche, droit;
+
+ unsigned char sauvegarde[256];
+ unsigned char ptr;
diff --git a/2003/n/pic/asserv/src/dial.c b/2003/n/pic/asserv/src/dial.c
new file mode 100644
index 0000000..6076ee5
--- /dev/null
+++ b/2003/n/pic/asserv/src/dial.c
@@ -0,0 +1,185 @@
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+// Fonctions concernant le dialogue avec le PC
+//---------------------------------------------------------------------
+//---------------------------------------------------------------------
+ //---------------------------------------------------------------------
+ // valide_num()
+ // Cette fonction permet de vérifier la validité des paramètres envoyés sous forme de 3octets précédés d'un #
+ //---------------------------------------------------------------------
+ boolean valide_num(char *caract)
+ {
+ // Si le caractère reçu n'est pas un chiffre
+ if( caract[0]<'0' || caract[0]>'2' || caract[1]<'0' || caract[1]>'9' || caract[2]<'0' || caract[2]>'9')
+ {
+ return false;
+ }
+
+ return true;
+ }
+
+ //---------------------------------------------------------------------
+ // validation()
+ // Cette fonction permet de vérifier la validité des paramètres envoyés
+ //---------------------------------------------------------------------
+ boolean validation(char *caract)
+ {
+ // Si le premier chiffre est codé en BCD
+ if(caract[0]==BCD)
+ {
+ // Si le premier chiffre est codé en BCD
+ if(!valide_num(caract+1))
+ {
+ return false;
+ }
+
+ // Si le second mot est en BCD
+ if(caract[4]==BCD)
+ {
+ if(caract[8]!='\0')
+ {
+ return false;
+ }
+
+ if(!valide_num(caract+5))
+ {
+ return false;
+ }
+ }
+ // Si le second mot est un simple octet ou un octet échappé
+ else
+ {
+ if((caract[4]==ECH && caract[6]!='\0') || (caract[4]!=ECH && caract[5]!='\0'))
+ {
+ return false;
+ }
+ }
+ }
+ else
+ {
+ // Si le premier mot est échappé
+ if(caract[0]==ECH)
+ {
+ // Si le second est codé en BCD
+ if(caract[2]==BCD)
+ {
+ if(caract[6]!='\0')
+ {
+ return false;
+ }
+
+ if(!valide_num(caract+3))
+ {
+ return false;
+ }
+ }
+ else
+ {
+ // Si le second mot est un simple octet ou un octet échappé
+ if((caract[2]==ECH && caract[4]!='\0') || (caract[2]!=ECH && caract[3]!='\0'))
+ {
+ return false;
+ }
+ }
+ }
+ // Si le premier mot est un octet simple
+ else
+ {
+ // Si le second est codé en BCD
+ if(caract[1]==BCD)
+ {
+ if(caract[5]!='\0')
+ {
+ return false;
+ }
+ if(!valide_num(caract+2))
+ {
+ return false;
+ }
+ }
+ else
+ {
+ // Si le second mot est un simple octet ou un octet échappé
+ if((caract[1]==ECH && caract[3]!='\0') || (caract[1]!=ECH && caract[2]!='\0'))
+ {
+ return false;
+ }
+ }
+ }
+ }
+
+ return true;
+ }
+
+ //---------------------------------------------------------------------
+ // decodage()
+ // Cette fonction récupère la valeur passée en paramètre au module une fois qu'ils ont été vérifiés
+ //---------------------------------------------------------------------
+ long decodage(char *caract)
+ {
+ long tmp;
+
+ if(caract[0]==BCD)
+ {
+ tmp= (100*(caract[1]-'0') + 10*(caract[2]-'0') + (caract[3]-'0'))*256;
+
+ if(caract[4]==BCD)
+ {
+ tmp= 100*(caract[5]-'0') +10*(caract[6]-'0') + caract[7] -'0'+tmp;
+ }
+ else{
+ if(caract[4]==ECH)
+ {
+ tmp=(caract[5])+tmp;
+ }
+ else
+ {
+ tmp=(caract[4])+tmp;
+ }
+ }
+ }
+ else
+ {
+ if(caract[0]==ECH)
+ {
+ tmp=256*(caract[1]);
+
+ if(caract[2]==BCD)
+ {
+ tmp= 100*(caract[3]-'0') +10*(caract[4]-'0') + (caract[5]-'0')+tmp;
+ }
+ else
+ {
+ if(caract[2]==ECH)
+ {
+ tmp=(caract[3])+tmp;
+ }
+ else
+ {
+ tmp=(caract[2])+tmp;
+ }
+ }
+ }
+ else
+ {
+ tmp=256*(caract[0]);
+
+ if(caract[1]==BCD)
+ {
+ tmp= 100*(caract[2]-'0') +10*(caract[3]-'0') + (caract[4]-'0')+tmp;
+ }
+
+ if(caract[1]==ECH)
+ {
+ tmp=(caract[2])+tmp;
+ }
+ else
+ {
+ tmp=(caract[1])+tmp;
+ }
+ }
+ }
+
+ return tmp;
+ }
+
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;
+
+ }
+