main.cpp

00001 // Display changes in encoder position and direction 
00002 #include "mbed.h"
00003 #include "qeihw.h"
00004 #define PLAGE_ANGULAIRE 800 // 800 // 420
00005 #define GAUCHE_MAX 500 // 500 /// 720
00006 #define TOUT_DROIT 900 
00007 #define ARRET_MOTEUR_NON_VOULU 3
00008 #define BLINKING_RATE_MS 
00009 #define CAPTEUR_TEMPO 20
00010 #define CAN_TEMPO 50
00011 #define MOTEUR_TEMPO 50
00012 #include "mbed.h"
00013 #define POS_PER_METER 43636
00014 #define TEMPS_ARRET_MOTEUR 5000
00015 ///
00016 #define TARGET_VITESSE 1000 // mm/s
00017  
00018  
00019  
00020 CAN can2(p30, p29,1000000);
00021   
00022 PwmOut Servo(p23);
00023 PwmOut pwm_mot(p26);
00024 Serial usb(USBTX,USBRX,115200) ;
00025 DigitalOut inA(p24);
00026 DigitalOut inB(p25);
00027 DigitalOut led1(LED1);
00028 DigitalOut led3(LED3);
00029  
00030 void print (void);
00031  
00032 void serial_ISR(void);
00033  
00034 int Bytes2Int(char data[], int position ) ;
00035  
00036 void set_position(unsigned short us) ;
00037  
00038 void send_can (float vitesse, int nbr) ;
00039  
00040 void send_can2(bool Moteur_stopper);
00041  
00042 float Bytes2Float( char data[], int position) ;
00043  
00044 float Float2Bytes( char data[], float nbr) ;
00045  
00046 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
00047 Timer tempo ;
00048 Timer can_timer ;
00049 Timer mot_timer ;
00050 Timer capteur_timer ;
00051 Timer Probleme_arret;
00052 Timer integral_timer;
00053  
00054 unsigned short pos = 1000;
00055 float vit = 0 ;
00056 int spam =0;
00057  
00058 int main() {  
00059  
00060     int32_t NewPosition, position = 0, premier_probleme = 0;
00061     float vitesse = 1.05,direction,ValErreur=0,ValServo,ErreurIntegral=0,CommandeMot=0,CapteurMot=0;
00062     unsigned char *chptr;  
00063     char Sens = 1, Moteur_stopper = 0; 
00064     int ajustement_direction = 0;
00065     
00066     CANMessage msg_rx;
00067     
00068     usb.attach(&serial_ISR,Serial::RxIrq);
00069        
00070                    
00071     qei.SetDigiFilter(10);
00072     qei.SetMaxPosition(0xFFFFFFFF);
00073     
00074     mot_timer.start() ;
00075     can_timer.start() ;
00076     Probleme_arret.start();
00077     capteur_timer.start() ;
00078     tempo.start() ;
00079     integral_timer.start();
00080    
00081     
00082     Servo.period_ms(20);
00083     pwm_mot.period_us(50);
00084     inA = 1;   
00085     inB = 0; 
00086     while(1) 
00087     {  
00088        if (can2.read(msg_rx)) 
00089         {
00090             ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;
00091             if(ValServo>=TOUT_DROIT)
00092             {
00093                 ajustement_direction = (ValServo - TOUT_DROIT)*0.33;
00094                 ValServo = ValServo - ajustement_direction;
00095             }
00096             
00097 //          CommandeMot =  float(msg_rx.data[1]/255.0f);
00098 //          ValErreur =  float(msg_rx.data[1]/255.0f);
00099             CommandeMot = TARGET_VITESSE;
00100             CapteurMot = vitesse*1000;
00101             //) + (ErreurIntegral += ValErreur*(float(integral_timer.read_ms()*1000)))
00102             ValErreur = ((CommandeMot - CapteurMot)/20000)*2;
00103             if(ValErreur>1)
00104             {
00105                 ValErreur = 0.99f;
00106             }   
00107 
00108             Sens = msg_rx.data[2] ; 
00109             inA = Sens;   
00110             inB = !Sens;  
00111                         
00112         }
00113        
00114         if (capteur_timer.read_ms() > CAPTEUR_TEMPO){   
00115                 NewPosition = qei.GetPosition();  
00116                 led1 = qei.Direction() == SET ? 1 : 0 ;
00117                 led3 = !led1;
00118                 vitesse = float(NewPosition - position)/(POS_PER_METER*0.020)  ;                  
00119                 if(spam>50)
00120                 {
00121                 usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r\n",ValErreur, NewPosition,vitesse);
00122                 spam=0;
00123                 }
00124                 else spam++;
00125                 position = NewPosition ;
00126                 capteur_timer.reset() ;
00127               }  
00128         
00129        
00130         set_position(ValServo);
00131         pwm_mot.write(ValErreur);
00132         //set_position(pos);
00133         //pwm_mot.write(vit);           
00134        
00135             
00136             
00137           
00138         if(can_timer.read_ms() > CAN_TEMPO )
00139         { 
00140             if (vitesse <= ARRET_MOTEUR_NON_VOULU)
00141             {
00142                 if(premier_probleme == 0)
00143                     premier_probleme = Probleme_arret.read_ms();
00144                 if(Probleme_arret.read_ms() >= premier_probleme + TEMPS_ARRET_MOTEUR); 
00145                     Moteur_stopper = 1;
00146             }
00147             if(vitesse >= ARRET_MOTEUR_NON_VOULU) 
00148             {
00149                 Moteur_stopper = 0;
00150                 premier_probleme = 0;
00151             }   
00152             send_can(vitesse, position) ;
00153             send_can2(Moteur_stopper);
00154             can_timer.reset() ;
00155         }
00156        
00157         
00158           
00159     }
00160 }
00161      
00162      /******* Régulation PID ********/
00163   // Ecart entre la consigne et la mesure
00164  // ecart = vref - vitesse;
00165  
00166   // Terme proportionnel
00167  // P_x = Kp * ecart;
00168  
00169   // Calcul de la commande
00170 //  commande = P_x + I_x;
00171  
00172   // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
00173   // I_x = I_x + Ki * dt * ecart;
00174   /******* Fin régulation PID ********/
00175   
00176  
00177  
00178  
00179 void send_can (float vitesse, int nbr){
00180     unsigned char *chptr ;
00181     CANMessage message;
00182     message.len=8;
00183     chptr = (unsigned char *) & vitesse;  
00184     message.data[0] = (*chptr++);
00185     message.data[1] = (*chptr++);
00186     message.data[2] = (*chptr++);
00187     message.data[3] = (*chptr);
00188     message.data[4] = char(nbr) ;
00189     message.data[5] = char(nbr>>8) ;
00190     message.data[6] = char(nbr>>16) ;
00191     message.data[7] = char(nbr>>24) ;
00192  
00193     message.id=0x400;
00194     
00195     if(spam>48)
00196     {
00197     usb.printf("\rvitesse = %f,   pos = %d\n\r", vitesse, nbr) ;
00198     }
00199     message.type=CANData;
00200     message.format=CANStandard;
00201     can2.write(message);
00202  
00203 }            
00204 void send_can2(bool Moteur_stopper)
00205 {   
00206     CANMessage message2;
00207     message2.len=1;
00208     message2.data[0] = bool(Moteur_stopper);
00209     
00210     message2.id=0x500;
00211     
00212     if(message2.data[0] == 1);
00213     {
00214     //usb.printf("\rARRET MOTEUR %d",Moteur_stopper);
00215     }
00216     
00217     message2.type=CANData;
00218     message2.format=CANStandard;
00219     can2.write(message2);
00220     
00221 }
00222  
00223  
00224 void set_position(unsigned short us)
00225 {
00226     unsigned short period= us+20000;
00227     float pulse_width;
00228     if (tempo.read_ms() > 50)
00229     {
00230         pulse_width=((float)us)/((float)period);
00231         Servo.period_us(period);
00232         Servo.write(pulse_width);
00233         tempo.reset();
00234     }
00235 }
00236  
00237 //int Bytes2Int(char data[], int position ){ 
00238 //    int nbr = 0 ;
00239 //    return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
00240 //}  
00241 //       
00242 //
00243 //float Float2Bytes( char data[], float nbr){ 
00244 //    unsigned char* chptr ;
00245 //    chptr = (unsigned char *) & nbr;  
00246 //    data[0]= (*chptr++);
00247 //    data[1]= (*chptr++);
00248 //    data[2]= (*chptr++);
00249 //    data[3]= (*chptr);
00250 //    
00251 //}
00252 //
00253 //float Bytes2Float( char data[], int position){ 
00254 //  union {
00255 //    float float_sent;
00256 //    unsigned char bytes[4];
00257 //  } u;
00258 //memcpy(u.bytes, data + position,4) ;
00259 //return u.float_sent ;
00260 //}
00261  
00262 void serial_ISR(void){
00263     char msg=usb.getc();
00264     switch (msg){
00265         case 'a':
00266             pos=pos+100;
00267             break;
00268         case 'z':
00269             if (pos>100)
00270                 pos=pos-100;
00271             break;
00272         case 'e':
00273             pos=pos+10;
00274             break;
00275         case 'r':
00276             if (pos>10)
00277                 pos=pos-10;
00278             break;
00279         case 'q':
00280             if (vit<1.0)
00281                 vit+=0.1 ;
00282             break;
00283         case 's':
00284             if (vit>0.0)
00285                 vit-= 0.1 ;
00286             break;
00287         case 'd':
00288             if (vit<1.0)
00289                 vit+=0.01 ;
00290             break;
00291         case 'f':
00292             if (vit>0.0)
00293                 vit-=0.01 ;
00294             break;
00295         
00296             
00297     }
00298     usb.printf("pos = %d us vit = %f \n",pos,vit);
00299 }