asservissement position et de vitesse PID LPC2138 et LMD18200T

Question

marabou34 · Answer

salut a tous voici le code source d un asservissement de position pour moteur dc a base lpc2138 et d un lmd18200t actuellement l asservissement et asservie a la position 0 il manque la génération de trajectoire ci vous avez des idée comment l implanté dans ce source je serai prenant merci d avance *******************************************************************************/ /* */ /* BLINKY.C: asservissement2 position et vitesse moteur dc */ /* */ /* */ /*******************************************************************************/ #include /* LPC21xx definitions */ #include signed int positionx=0; signed int old_positionx=0; signed int compte_pulse=0; static int capture0; unsigned int LED_out; unsigned int PWM_correct; /*************************************************************************************************************************/ /**************************************** PROGRAMME GPIO ***********************************************************/ /*************************************************************************************************************************/ /*ecriture sur le port p1 bit 16 a 23*/ void write_p1_16_23(int data) { IODIR1 = 0xFF0000; /* P1.16..23 défini comme sortie */ data <<= 16; /*décalage a gauche de 16*/ IOSET1 |= data; /* Turn on LED */ } /*ecriture sur le port p0 bit 16 a 23*/ void write_p0_16_23(int data) { IODIR0 = 0xFF0000; /* P0.16..23 defini comme sortie */ data <<= 16; /*décalage a gauche de 16*/ IOSET0 |= data; /* Turn on LED */ } /*lecture du port p1 bit 16 a 23*/ char read_p1_16_23(void) { int data; IODIR1 = 0x000000; /* P1.16..23 defini comme entrée */ data=(unsigned char)(IOPIN1>>16); /* lecture du P1_16_23 et decalé data de 16 */ return(data); } /*lecture du port p0 bit 16 a 23*/ char read_p0_16_23(void) { int data; IODIR0 = 0x000000; /* P1.16..23 defini comme entrée */ data=(unsigned char)(IOPIN0>>16); /* lecture du P1_16_23 et decalé data de 16 */ return(data); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // edge capture with Timer 0 //void T0isr(void) __irq; void T0isr (void) __irq { static int value; unsigned int sens_codeur; value = T0CR0; // read the capture value capture0=value; sens_codeur = read_p0_16_23(); sens_codeur=sens_codeur & 0x00000001; //lecture du sens codeur p0.16 //sens=1>>ccw,sens=0>>cw // IODIR1 = 0xFF0000; // LED_out = positionx; // set LED out // IOCLR1 = 0xFC0000; // clear LED's // IOSET1 = (LED_out << 18);//&0xFFFC0000; // output intrp_count on LED //if (old_positionx!=positionx) // { switch (sens_codeur) { case (0x1): positionx++; break; case (0x0): positionx--; break; }; // } old_positionx=positionx; T0IR |= 0x00000001; //Clear match 0 interrupt VICVectAddr = 0x00000000; //Dummy write to signal end of interrupt } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //void init_timer (void) { //VPBDIV = 0x00000002; //Set pclk to 30 Mhz //PINSEL0 |= 0x00000020; //Enable pin 0.2 as capture channel0 //T0PR = 0x00000001; //Load prescaler for /////0.5Msec tick/////F ///,,,,,,,///1 Msec tick ////////1E //T0TCR = 0x00000002; //Reset counter and prescaler //T0CCR = 0x00000007; //Capture on rising edge and falling edge of channel0 //T0TCR = 0x00000001; //enable timer //VICVectAddr4 = (unsigned)T0isr; //Set the timer ISR vector address //VICVectCntl4 = 0x00000024; //Set channel //VICIntEnable = 0x00000010; //Enable the interrupt //} void init_timer(void) { VICVectAddr1 = (unsigned int) &T0isr; VICVectCntl1 = 0x24; // Channel1 on Source#4 ... enabled VICIntEnable |= 0x10; // Channel#4 is the Timer 0 T0PR 60; // prescaler 60, timer runs at 60 MHz / 60 1 MHz T0MR0 1000000; // 1 sec / 1 us T0MCR = 3; T0CCR = 0x0FC7; // Capture on both edges and enable the interrupt T0TC = 0; // Reset timer T0TCR = 1; // start timer } //////////////////////////////////////////////////////////////////////////////////////////////// // déffinition du timer1 // //////////////////////////////////////////////////////////////////////////////////////////////// char f_10ms = 0; void T1_Isr(void) __irq // Timer 1 ISR every 10 msec { f_10ms = 1; // toggles every 10 mseconds T1IR = 0x01; // reset interrupt flag VICVectAddr = 0; // reset VIC } void T1_Init(void) { VICVectAddr2 = (unsigned int) &T1_Isr; VICVectCntl2 = 0x25; // Channel2 on Source#5 ... enabled VICIntEnable |= 0x20; // Channel#5 is the Timer 1 T1MR0 600000; // 10 msec / 16,67 nsec T1MCR = 3; // Interrupt on Match0, reset timer on match // Pclk = 60 MHz // timer count = 16,67 nsec T1TC = 0; // reset Timer counter T1TCR = 1; // enable Timer } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void init_serial (void) { /* Initialize Serial Interface */ U1LCR = 0x83; /* 8 bits, no Parity, 1 Stop bit */ U1DLL = 0x08; /* 115200 Baud Rate @ 15MHz VPB Clock */ U1LCR 0x03; /* DLAB 0 */ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////// // initialisation du pwm2 // ///////////////////////////////////// void init_pwm2(void) { //initialisation PWM2 PWMPR = 0x00000002; //Load prescaler pour une frequence de 23.47 khz PWMPCR = 0x0000404; //PWM channel 2 double edge control, output enabled PWMMCR = 0x00000003; //On match with timer reset the counter PWMMR0 = 0x00000100; //set cycle rate to 256=hex100 "la période PWM fractionné en 256 " ces la resolution du PWM " PWMMR1 = 0x00000000; //set rising edge of PWM2 to 0 ticks PWMMR2 = 0x00000000; //set falling edge of PWM2 to 0 ticks PWMLER = 0x00000007; //enable shadow latch for match 0 - 2 PWMEMR = 0x00000280; //Match 1 and Match 2 outputs set high PWMTCR = 0x00000002; //Reset counter and prescaler PWMTCR = 0x00000009; //enable counter and PWM, release counter from reset } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////// // remise a zero du controle moteur p1.16"brake=0" p1.17"sign=0" // //////////////////////////////////////////////////////////////////// void init_sens_brake_moteur(void) { IOCLR1= 0x00030000; //effacer p1_16 a p1_17 c utilisé uniquement pour la remise a zero du controle moteur //brake=0 et signe=0 apret un mouvement moteur } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////// // fonction3 affiche valeur du courant et renvoi la valeur // //////////////////////////////////////////////////////////////////////////////////////////////// unsigned long affiche_courant3 (void) //fonction pour veriffier ci adc fonctionne toujours ;-) // { unsigned int volume=0; unsigned int val=0; unsigned long imageI1=0; // init_timer(); /***********************************************/ /* test sortie DAC avec generateur de fonction */ /***********************************************/ PINSEL1 = 0x01080000; /* enable DAC */ DACR = 0x00008000; /* DAC Output set to Middle Point */ // IODIR0 = 0xFF0000; /* P1.16..23 defined as Outputs */ /****************************************************************************************/ /* test entrée ADC avec affichage de la valeur convertie sur le port 0 de fonction */ /****************************************************************************************/ AD0CR = 0x00200402; /* Setup A/D: 10-bit AIN0 @ 3MHz */ AD0CR |= 0x01000000; /* Start A/D Conversion */ do { val = AD0DR; /* Read A/D Data Register */ } while ((val & 0x80000000) == 0); /* Wait for end of A/D Conversion */ AD0CR &= ~0x01000000; /* Stop A/D Conversion */ val=val>>=6; /* decalage Data register de 6 */ val=val&0x3ff; /* recuperation uniquement des 10bit*/ imageI1=val; /* affectation de la valeur val a volume */ return(imageI1); /* renvoi de la valeur mesuré */ } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //// fonction regulation de position PID //// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// char FCM_pid(unsigned int FCV_KP,unsigned int FCV_KI,unsigned int FCV_KD,signed int position_actuelle,signed int position_de_consigne) { //regulation pid //Définition des variables locales signed int FCL_POSITION; signed int FCL_CONSIGNE; signed int FCL_ERREUR; signed long FCL_VAL_PROPORTIONELLE; signed long FCL_VAL_INTEGRALE; signed long FCL_VAL_DERRIVE; signed int FCL_OLD_ERREUR; unsigned char FCL_PID; unsigned long FCV_INTEGRAT; position_actuelle=FCL_POSITION; position_de_consigne=FCL_CONSIGNE; //1 Calcul: // pid.erreur = pid.consigne - pid.position FCL_ERREUR = FCL_CONSIGNE - FCL_POSITION ; //calcul de l'erreur position //2 Calcul: // pid.val_proportionelle = pid.erreur * kp FCL_VAL_PROPORTIONELLE = FCL_ERREUR * FCV_KP ; //calcul de la valeur proportionelle ****************(ERR*KP) //3 Calcul: // pid.val_derrive = ( pid.erreur - pid.olderr ) * kd FCL_VAL_DERRIVE = ( FCL_ERREUR - FCL_OLD_ERREUR ) * FCV_KD ; //calcul de la valeur de la dérivé ****************{ERR(t)-ERR(t-1)}*KD //4 Calcul: // integrat = integrat + pid.erreur FCV_INTEGRAT = FCV_INTEGRAT + FCL_ERREUR ; //calcul de la valeur de l'integral ****************somme de toutes les ERR //5 Calcul: // pid.val_integrale = integrat * ki FCL_VAL_INTEGRALE = FCV_INTEGRAT * FCV_KI ; //calcul de la valeur de l'integral ***************somme de toutes les ERR*Ki //6 Calcul: // pid.olderr = pid.erreur FCL_OLD_ERREUR = FCL_ERREUR ; //affectation de nouvelle valeur d'erreur en remplacement a l encienne //7 Calcul: // pid.pid = pid.val_proportionelle + pid.val_integrale + pid.val_derrive FCL_PID = FCL_VAL_PROPORTIONELLE + FCL_VAL_INTEGRALE + FCL_VAL_DERRIVE ;//calcul de nouvel valeur de consigne moteur return FCL_PID; } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////// //mouvement standard non codé ACC VMAX DEC// //////////////////////////////////////////// //p1.16=brake p1.17=sign //faut rajouté ,l'asservissement vitesse et de position , //lecture du capteur de home ,sc1,sc2 //lecture de la protection temperature LMD18200T,time out ,Vmin void fnl_lmd18200(unsigned long pwm,int brake,int sign,unsigned long acc,unsigned long tmpvmax,unsigned long dec,unsigned long imax,signed int position_final) { unsigned long ic0; //premiere valeur du courant apret la mesure unsigned long i; //période de la pulse positive du signal PWM "vitesse max " unsigned long t; //variable temporel de l'accélération int control_M; //valeur de controle moteur "sens moteur et brake moteur" control_M=0; //remise a zero du controle moteur"sens et brake " control_M=(brake|(sign<<1));//faire un ou logique entre brake et (sign avec decalage a gauche de 1) write_p1_16_23(control_M);//ecriture sur le port1 "P1.16 a P1.17"_____"brake et sign" //I=((pwm/100)*255); //calcul pwm% vers valeur comprise entre 0 et 255 ???????????????a revoir???? ////////////////////////////// // phase d'accélération // ////////////////////////////// for (i=0;i=imax) //ci ya overload avec ic0 et overload avec ic1 faire un arret moteur { PWMMR2 = 0; //modulation du signal pwm PWMLER = 0x00000007; //enable shadow latch for match 0 - 2 // init_serial(); /* Initialize Serial Interface to 115200 baud */ printf (" * overload i * "); printf ("courant mesuré en div : %04d ",ic0); printf ("courant mesuré en mA : %04d ",ic0*4);/* renvoi de la valeur mesuré par RS232 *///3,3816576 return; } } } /////////////////////////// // phase vitesse max // /////////////////////////// for (t=0;t=imax) //ci ya overload avec ic0 et overload avec ic1 faire un arret moteur { PWMMR2 = 0; //modulation du signal pwm PWMLER = 0x00000007; //enable shadow latch for match 0 - 2 // init_serial(); /* Initialize Serial Interface to 115200 baud */ printf (" * overload i * "); printf ("courant mesuré en div : %04d ",ic0); printf ("courant mesuré en mA : %04d ",ic0*4);/* renvoi de la valeur mesuré par RS232 *///33816576 return; } } //////////////////////////////// // phase de déccelération // //////////////////////////////// for (i=pwm;i>0;i--) { for (t=0;t=imax) //ci ya overload avec ic0 et overload avec ic1 faire un arret moteur { PWMMR2 = 0; //modulation du signal pwm PWMLER = 0x00000007; //enable shadow latch for match 0 - 2 // init_serial(); /* Initialize Serial Interface to 115200 baud */ printf (" * overload i * "); printf ("courant mesuré en div : %04d ",ic0); printf ("courant mesuré en mA : %04d ",ic0*4);/* renvoi de la valeur mesuré par RS232 *///33816576 return; } } } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void fnlx_lmd18200(unsigned long pwm,int brake,int sign,unsigned long imax,signed int position_final,unsigned long kp,unsigned long ki,unsigned long kd) { int sign2; unsigned int ic0; //premiere valeur du courant apret la mesure unsigned int p; int control_M; //valeur de controle moteur "sens moteur et brake moteur" signed int position_a_asservir; unsigned int tempo; // for (p=0;p0) { IOCLR1= 0x00020000;//sens=0 PWM_correct=FCM_pid(kp,ki,kd,positionx,0);//calcule de la corection PWMMR2 =(PWM_correct); //correction du pwm PWMLER = 0x00000007; //enable shadow latch for match 0 - 2 } if (positionx=0) { break; } // for (tempo=0;tempo<10000;tempo++) //tempo // {}; // init_serial(); /* Initialize Serial Interface to 115200 baud */ // printf (" pppwwwmmm:%09d ",PWM_correct); // printf (" positionx:%09d ",positionx); } // } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // // PROGRAMME PRINCIPALE SY // // // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int main(void) { //int code_mouvement;//commande du mouvement voulu unsigned int position=0; unsigned tempo0; PINSEL0 |= 0x00058020; // configuration pin processeur init_serial(); /* Initialize Serial Interface to 115200 baud */ printf (" ******************************************************* "); printf ("****************************************************** "); printf ("** MCB2130 DC motor driver ** "); printf ("** by sidhoum yacine ** "); printf ("****************************************************** "); printf ("****************************************************** "); init_timer () ;//initialisation timer count pulse init_pwm2(); //initialisation pw2 while(1) { //mouvement codée //positionx=0; fnlx_lmd18200(100,0,1,500,0,500,1,8000);// vitesse demandé pwm fixe , brake, sign, imax, position_final ,kp,ki,kd //init_sens_brake_moteur(); //mouvement non codée //positionx=0; //fnl_lmd18200(255,0,1,100,200000,100,500,200000000);//pwm,brake,sens,tacc,tvmax,tdec,imax,position final //init_sens_brake_moteur(); //positionx=0; //fnl_lmd18200(255,0,0,100,200000,100,500,200000000);//pwm,brake,sens,tacc,tvmax,tdec,imax,position final //init_sens_brake_moteur(); //printf (" positionx :%09d ",positionx); //printf ("old_positionx:%09d ",old_positionx); //printf ("capture0 :%09d ",capture0); //for (tempo0=0;tempo0<20000000;tempo0++) //tempo a la vitesse max //{} } }

Asservissement position et de vitesse PID LPC2138 et LMD18200T

1 réponse

Votre réponse

Discussions similaires