From a9f3689ee85da91a51d01a76a469cbb5298806d2 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Jun 2004 11:53:30 +0000 Subject: Initial revision --- 2003/n/pic/asserv/src/asserv.c | 912 ++++++++++++++++++++++++++++++++++++++++ 2003/n/pic/asserv/src/asserv.h | 187 ++++++++ 2003/n/pic/asserv/src/dial.c | 185 ++++++++ 2003/n/pic/asserv/src/routine.c | 193 +++++++++ 4 files changed, 1477 insertions(+) create mode 100644 2003/n/pic/asserv/src/asserv.c create mode 100644 2003/n/pic/asserv/src/asserv.h create mode 100644 2003/n/pic/asserv/src/dial.c create mode 100644 2003/n/pic/asserv/src/routine.c (limited to '2003/n/pic/asserv/src') 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 // 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; + + } + -- cgit v1.2.3