File content as of revision 1:b71ac293907d:

// Display changes in encoder position and direction 
#include "mbed.h"
#include "qeihw.h"
#define PLAGE_ANGULAIRE 420
#define GAUCHE_MAX 710
#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);

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

void set_position(unsigned short us) ;

void send_can (float vitesse, int nbr) ;

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 ;
int main() {  

    int32_t Position2, position = 0 , temp  = 0;
    float vitesse = 1.05 , direction , ValMot, ValServo ;
    unsigned char *chptr;  
    char Sens = 1; 
    
    CANMessage msg_rx;
    CANMessage msg_tx;
    
    
       
                   
    qei.SetDigiFilter(10);
    qei.SetMaxPosition(0xFFFFFFFF);
    
    mot_timer.start() ;
    can_timer.start() ;
    capteur_timer.start() ;
    tempo.start() ;
   
    
    Servo.period_ms(20);
    pwm_mot.period_ms(20);
    while(1) 
    {  
       if (can2.read(msg_rx)) 
        {
            ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;;
            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);
           
       
            
            
          
        if(can_timer.read_ms() > CAN_TEMPO ){ 
           
            send_can(vitesse, position) ;
            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 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 ;
}