Concours voiture autonome / Mbed 2 deprecated MOTEUR_LPC_TEST_V2

Dependencies:   mbed PROGRAMME_MOTEUR_V1

main.cpp

Committer:
QuentinD
Date:
2021-04-15
Revision:
2:ce23bd78a43a
Parent:
1:b71ac293907d

File content as of revision 2:ce23bd78a43a:

// Display changes in encoder position and direction 
#include "mbed.h"
#include "qeihw.h"
#define PLAGE_ANGULAIRE 800 // 800 // 420
#define GAUCHE_MAX 500 // 500 /// 720
#define TOUT_DROIT 900 
#define ARRET_MOTEUR_NON_VOULU 3
#define BLINKING_RATE_MS 
#define CAPTEUR_TEMPO 200
#define CAN_TEMPO 50
#define MOTEUR_TEMPO 50
#include "mbed.h"
#define POS_PER_METER 43636
//#define GAUCHE_MAX 710
//#define PLAGE_ANGULAIRE 420

CAN can2(p30, p29,1000000);
  
PwmOut Servo(p23);
PwmOut pwm_mot(p26);
Serial usb(USBTX,USBRX,115200) ;
DigitalOut inA(p24);
DigitalOut inB(p25);
DigitalOut led1(LED1);
DigitalOut led3(LED3);

void print (void);

void serial_ISR(void);

int Bytes2Int(char data[], int position ) ;

void set_position(unsigned short us) ;

void send_can (float vitesse, int nbr) ;

void send_can2(bool Moteur_stopper);

float Bytes2Float( char data[], int position) ;

float Float2Bytes( char data[], float nbr) ;

QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
Timer tempo ;
Timer can_timer ;
Timer mot_timer ;
Timer capteur_timer ;
Timer Probleme_arret;

unsigned short pos = 1000;
float vit = 0 ;

int main() {  

    int32_t Position2, position = 0 , temp  = 0, premier_probleme = 0;
    float vitesse = 1.05 , direction , ValMot, ValServo ;
    unsigned char *chptr;  
    char Sens = 1, Moteur_stopper = 0; 
    int ajustement_direction = 0;
    
    CANMessage msg_rx;
    CANMessage msg_tx;
    
    usb.attach(&serial_ISR,Serial::RxIrq);
       
                   
    qei.SetDigiFilter(10);
    qei.SetMaxPosition(0xFFFFFFFF);
    
    mot_timer.start() ;
    can_timer.start() ;
    Probleme_arret.start();
    capteur_timer.start() ;
    tempo.start() ;
   
    
    Servo.period_ms(20);
    pwm_mot.period_us(50);
    inA = 1;   
    inB = 0; 
    while(1) 
    {  
       if (can2.read(msg_rx)) 
        {
            ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;
            if(ValServo>=TOUT_DROIT)
            {
                ajustement_direction = (ValServo - TOUT_DROIT)*0.33;
                ValServo = ValServo - ajustement_direction;
            }
   
            ValMot =  float(msg_rx.data[1]/255.0f);
            Sens = msg_rx.data[2] ; 
            inA = Sens;   
            inB = !Sens;  
                        
        }
       
        if (capteur_timer.read_ms() > CAPTEUR_TEMPO){   
                Position2 = qei.GetPosition();  
                led1 = qei.Direction() == SET ? 1 : 0 ;
                led3 = !led1;
                vitesse = float(Position2 - position)/(POS_PER_METER*0.020)  ;                      
                //usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r",ValMot, Position2,vitesse);
                position = Position2 ;
                capteur_timer.reset() ;
              }  
        
       
        set_position(ValServo);
        pwm_mot.write(ValMot);
        //set_position(pos);
        //pwm_mot.write(vit);           
       
            
            
          
        if(can_timer.read_ms() > CAN_TEMPO )
        { 
            if (vitesse <= ARRET_MOTEUR_NON_VOULU)
            {
                if(premier_probleme == 0)
                    premier_probleme = Probleme_arret.read_ms();
                if(Probleme_arret.read_ms() >= premier_probleme + 5000); 
                    Moteur_stopper = 1;
                if(vitesse >= ARRET_MOTEUR_NON_VOULU) 
                    {
                        Moteur_stopper = 0;
                        premier_probleme = 0;
                    }   
            }
            send_can(vitesse, position) ;
            send_can2(Moteur_stopper);
            can_timer.reset() ;
        }
       
        
          
    }
}
     
     /******* Régulation PID ********/
  // Ecart entre la consigne et la mesure
 // ecart = vref - vitesse;
 
  // Terme proportionnel
 // P_x = Kp * ecart;
 
  // Calcul de la commande
//  commande = P_x + I_x;
 
  // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
  // I_x = I_x + Ki * dt * ecart;
  /******* Fin régulation PID ********/
  



void send_can (float vitesse, int nbr){
    float res ;
    int res2 ;
    unsigned char *chptr ;
    CANMessage message;
    message.len=8;
    chptr = (unsigned char *) & vitesse;  
    message.data[0] = (*chptr++);
    message.data[1] = (*chptr++);
    message.data[2] = (*chptr++);
    message.data[3] = (*chptr);
    message.data[4] = char(nbr) ;
    message.data[5] = char(nbr>>8) ;
    message.data[6] = char(nbr>>16) ;
    message.data[7] = char(nbr>>24) ;

    message.id=0x400;
    
    res = Bytes2Float((char*)message.data, 0) ;
    res2 = Bytes2Int((char*)message.data, 4) ;
    usb.printf("\rvitesse = %f,   pos = %d", vitesse, nbr) ;
    message.type=CANData;
    message.format=CANStandard;
    can2.write(message);

}            
void send_can2(bool Moteur_stopper)
{   
    CANMessage message2;
    message2.len=1;
    message2.data[0] = bool(Moteur_stopper);
    
    message2.id=0x500;
    
    usb.printf("\rARRET MOTEUR %d",Moteur_stopper);
    
    message2.type=CANData;
    message2.format=CANStandard;
    can2.write(message2);
    
}


void set_position(unsigned short us)
{
    unsigned short period= us+20000;
    float pulse_width;
    if (tempo.read_ms() > 50)
    {
        pulse_width=((float)us)/((float)period);
        Servo.period_us(period);
        Servo.write(pulse_width);
        tempo.reset();
    }
}

int Bytes2Int(char data[], int position ){ 
    int nbr = 0 ;
    return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
}  
       

float Float2Bytes( char data[], float nbr){ 
    unsigned char* chptr ;
    chptr = (unsigned char *) & nbr;  
    data[0]= (*chptr++);
    data[1]= (*chptr++);
    data[2]= (*chptr++);
    data[3]= (*chptr);
    
}

float Bytes2Float( char data[], int position){ 
  union {
    float float_sent;
    unsigned char bytes[4];
  } u;
memcpy(u.bytes, data + position,4) ;
return u.float_sent ;
}

void serial_ISR(void){
    char msg=usb.getc();
    switch (msg){
        case 'a':
            pos=pos+100;
            break;
        case 'z':
            if (pos>100)
                pos=pos-100;
            break;
        case 'e':
            pos=pos+10;
            break;
        case 'r':
            if (pos>10)
                pos=pos-10;
            break;
        case 'q':
            if (vit<1.0)
                vit+=0.1 ;
            break;
        case 's':
            if (vit>0.0)
                vit-= 0.1 ;
            break;
        case 'd':
            if (vit<1.0)
                vit+=0.01 ;
            break;
        case 'f':
            if (vit>0.0)
                vit-=0.01 ;
            break;
        
            
    }
    usb.printf("pos = %d us vit = %f \n",pos,vit);
}