File content as of revision 0:a0ab83db7fa0:

// 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 
#include "mbed.h"

CAN can2(p30, p29,1000000);
  
PwmOut Servo(p23);
PwmOut pwm_mot(p26);

DigitalOut inA(p24);
DigitalOut inB(p25);
DigitalOut led1(LED1);
DigitalOut led3(LED3);

void set_position(unsigned short us);



QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
Timer tempo ;
Timer t;
Timer can_timer ;

int main() {  

    can_timer.start() ;
    tempo.start() ;
    float vitesse , direction;
    unsigned char *chptr;  
    float ValMot,ValServo;
    char Sens; 
    DigitalOut led(LED1);
    CANMessage msg_rx;
    CANMessage msg_tx;
    
    int32_t Position2, position , temp  = 0;
       
    //serial_port.attach(&serial_ISR,Serial::RxIrq);                 
    qei.SetDigiFilter(10);
    qei.SetMaxPosition(0xFFFFFFFF);
    t.start();  
    msg_tx.id = 400 ;
    msg_tx.len = 4 ;
    
    Servo.period_ms(20);
    pwm_mot.period_ms(20);
    while(1) 
    {  wait_ms(50) ;
       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 ( temp%200 == 0 ){   
                Position2 = qei.GetPosition();  
                led1 = qei.Direction() == SET ? 1 : 0;
                led3 = !led1;
                vitesse = (Position2 - position);
                vitesse = (vitesse/42000.0f);
                vitesse = (vitesse /(200/1000.0f));                            
                //printf("la pwm est de %f la position est de %lu la vitesse est de %f KM/H\n\r",pwm, Position2,(vitesse*3.6));
                position = Position2 ;
              }  
              
        set_position(ValServo);
        pwm_mot.write(ValMot);
        
       
        
          
}
}
     
     /******* 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 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();
    }
}