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