Assevissement pid

dambeaufort Messages postés 60 Date d'inscription mardi 10 février 2004 Statut Membre Dernière intervention 17 septembre 2004 - 7 avril 2004 à 09:57
marabou34 Messages postés 4 Date d'inscription dimanche 16 septembre 2007 Statut Membre Dernière intervention 12 novembre 2008 - 12 nov. 2008 à 23:29
bonjour,
je dois realiser un asservissement de type PId en c, est ce que quelqu'un aurait un code pour cde genre d'asservissement?

Je vous remercie d'avance.

5 réponses

lpikachu58 Messages postés 351 Date d'inscription samedi 2 février 2002 Statut Membre Dernière intervention 6 mai 2004 2
7 avril 2004 à 10:46
putain c'est juste une division à réaliser c'est quand même pas difficile

be linux
0
cs_zigouille Messages postés 4 Date d'inscription jeudi 25 janvier 2007 Statut Membre Dernière intervention 28 janvier 2007
25 janv. 2007 à 19:22
Bonjour je suis en BTS électronique et je dois réaliser un programme en C afin de programme une régulation de dépression pour commander une pompe. Je suis pas tré bon en progra et j'aurai besoin d'aide urgent. Meme le plu petit conseil. Je veu pa kon me le fasse mé juste des conseille car il faut ke ji arrive. Je vous laisse mon mail au cas ou kelkun pouré venir a mon secour. Merci bien

Mail: [mailto:Zigouille16@hotmail.fr Zigouille16@hotmail.fr]
0
nnizarr Messages postés 11 Date d'inscription vendredi 16 mars 2007 Statut Membre Dernière intervention 19 juillet 2007
21 juin 2007 à 14:26
Bonjour
pour faire de la régulation c pas trop compliquer, vous devez seulement calculer l'erreur  entre ta consigne et ta valeur mesurer. Apres générer une commande par des calcules, cette dernière ça être appliquer a ton actionneur pour faire la régulation. En générale c serais ça
 Je crois que ca doit vous aidez:
http://perso.wanadoo.fr/philippe.baucour/pratiquer/equadiff/equadiff3.html
0
marabou34 Messages postés 4 Date d'inscription dimanche 16 septembre 2007 Statut Membre Dernière intervention 12 novembre 2008
12 nov. 2008 à 23:24
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=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;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 ("\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      
//{}

}
}
0

Vous n’avez pas trouvé la réponse que vous recherchez ?

Posez votre question
marabou34 Messages postés 4 Date d'inscription dimanche 16 septembre 2007 Statut Membre Dernière intervention 12 novembre 2008
12 nov. 2008 à 23:29
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=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;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 ("\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
//{}

}
}
0
Rejoignez-nous