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 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 912 insertions(+) create mode 100644 2003/n/pic/asserv/src/asserv.c (limited to '2003/n/pic/asserv/src/asserv.c') 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++; + } + } + } + } + } -- cgit v1.2.3