// 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++; } } } } }