mise a jours
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.H> /* LPC21xx definitions */
#include <stdio.h>
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<pwm;i++)
{
for (t=0;t<acc;t++) //valeur de l'accélération tempo
{
//lecture de la vitesse & position réel ???????????????????
//correction de la vitesse & position ???????????????????
PWMMR2 = i; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
//lecture du courant par interuption timer sera plus judicieux!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ic0=0;
ic0=affiche_courant3 ();//lecture du courant et affichage sur p1_16 a p1_23
//test ci le mouvement es arrivé a la position final
if (positionx==position_final)//ci la position moteur = position final faire un arret moteur
{
PWMMR2 = 0; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
return;
}
//test du overload courant ci c oui stop moteur sa fonctionne pas ?????????????????????????????????????????????????????????????????
if (ic0>=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 ("\n* overload i *\n\n");
printf ("courant mesuré en div : %04d\n",ic0);
printf ("courant mesuré en mA : %04d\n",ic0*4);/* renvoi de la valeur mesuré par RS232 *///3,3816576
return;
}
}
}
///////////////////////////
// phase vitesse max //
///////////////////////////
for (t=0;t<tmpvmax;t++) //tempo a la vitesse max
{
//lecture de la vitesse réel & position réel ??????????????
//correction de la vitesse & position ???????????????????
PWMMR2 = i; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
//lecture du courant par interuption timer sera plus judicieux!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ic0=0;
ic0=affiche_courant3 ();//lecture du courant et affichage sur p1_16 a p1_23
//test ci le mouvement es arrivé a la position final
if (positionx==position_final)//ci la position moteur = position final faire un arret moteur
{
PWMMR2 = 0; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
return;
}
// IODIR1 = 0xFF0000;
// IOCLR1 = 0xFF0000; // clear LED's
// IOSET1 = (positionx << 16); // output intrp_count on LED
// IOCLR1 = 0xFF0000; // clear LED's
//test du overload courant ci c oui stop moteur sa fonctionne pas ?????????????????????????????????????????????????????????????????
if (ic0>=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 ("\n* overload i *\n\n");
printf ("courant mesuré en div : %04d\n",ic0);
printf ("courant mesuré en mA : %04d\n",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<dec;t++) //valeur de l'accélération tempo
{
//lecture de la vitesse réel & position réel ??????????????
//correction de la vitesse & position ???????????????????
PWMMR2 = i; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
//lecture du courant par interuption timer sera plus judicieux!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ic0=0;
ic0=affiche_courant3 ();//lecture du courant et affichage sur p1_16 a p1_23
//test ci le mouvement es arrivé a la position final
if (positionx==position_final)//ci la position moteur = position final faire un arret moteur
{
PWMMR2 = 0; //modulation du signal pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
return;
}
//test du overload courant ci c oui stop moteur sa fonctionne pas ?????????????????????????????????????????????????????????????????
if (ic0>=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 ("\n* overload i *\n\n");
printf ("courant mesuré en div : %04d\n",ic0);
printf ("courant mesuré en mA : %04d\n",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;p<position_final;p++) //valeur de la consigne de position
// {
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"
//consigne de vitesse de départ
//PWMMR2 =pwm;
//PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
//IOCLR1= 0x00020000;//effacer p1_16 a p1_17 c utilisé uniquement pour la remise a zero du controle moteur
//positionx=position_a_asservir;
while(positionx!=0)//position_final)//ci la position moteur es differante de la position demandé
{
if (positionx<0)
{
IOSET1= 0x00020000;//sens =1
PWM_correct=FCM_pid(kp,ki,kd,positionx,0);//calcule de la corection
PWMMR2 =(255-PWM_correct); //correction du pwm
PWMLER = 0x00000007; //enable shadow latch for match 0 - 2
}
if (positionx>0)
{
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 ("\n\npppwwwmmm:%09d\n",PWM_correct);
// printf ("\n\npositionx:%09d\n",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 ("\n*******************************************************\n");
printf ("******************************************************\n");
printf ("** MCB2130 DC motor driver **\n");
printf ("** by sidhoum yacine **\n");
printf ("******************************************************\n");
printf ("******************************************************\n\n");
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 ("\n\npositionx :%09d\n",positionx);
//printf ("old_positionx:%09d\n",old_positionx);
//printf ("capture0 :%09d\n\n",capture0);
//for (tempo0=0;tempo0<20000000;tempo0++) //tempo a la vitesse max
//{}
}
}