File content as of revision 0:6ca63d45f0ee:

#include "mbed.h"

int AencoderPin1 = 18;
int AencoderPin2 = 19;
int BencoderPin1 = 20;
int BencoderPin2 = 21;
 
volatile int AlastEncoded = 0;
volatile int AencoderValue = 0;
volatile int AencoderValueb = 0;
volatile int BlastEncoded = 0;
volatile int BencoderValue = 0;
volatile int BencoderValueb = 0;
 
long AlastencoderValue = 0;
long BlastencoderValue = 0;

int sumA = 0;
int sumB = 0;

const long e = 11750; // ecart entre les 2 roues
const float D = 8.5;

long pos[2] = {0, 0}; // position en (x, y)
short angle = 0; // angle du robot
long DA = 0;
long DB = 0;

int dr[2]; // vitesse des roues droite et gauche (vg, vd), en millieme de cm / millisec
    
 
void setup() {
  Serial.begin (115200);
 
  pinMode(AencoderPin1, INPUT); 
  pinMode(AencoderPin2, INPUT);
  pinMode(BencoderPin1, INPUT); 
  pinMode(BencoderPin2, INPUT);

 
  digitalWrite(AencoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(AencoderPin2, HIGH); //turn pullup resistor on
  digitalWrite(BencoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(BencoderPin2, HIGH); //turn pullup resistor on
 
  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3) 
  attachInterrupt(digitalPinToInterrupt(AencoderPin1), AupdateEncoder, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(AencoderPin2), AupdateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(BencoderPin1), BupdateEncoder, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(BencoderPin2), BupdateEncoder, CHANGE);
 
}

void getParametres(int dr[])
{
    dr[0] = (int) (AencoderValue * 8.41413233 + 0.5);
    sumA += AencoderValue;
    AencoderValue = 0;
    /*DA += dr[0];
    Serial.print("drA : ");
    Serial.print(DA);*/
    dr[1] = (int) (BencoderValue * 8.4034010 + 0.5);
    sumB+= BencoderValue;
    BencoderValue = 0;
    /*DB += dr[1];
    Serial.print("  drB : ");
    Serial.print(DB);
    Serial.print("  ");*/
}

void updatePos(long pos[], short* pangle, int dr[])
{
    /*
    on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
    */

    long R = 0; // rayon du cercle décrit par la trajectoire
    int d = 0; // vitesse du robot
    long c[2] = {0, 0}; // position du centre du cercle décrit par la trajectoire

    // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
    if (dr[0] != dr[1]){
        R = e / 2 * (dr[1] + dr[0]) / (dr[1] - dr[0]);
        c[0] = pos[0] - (int) (R * sin(*pangle * PI / 32768) + 0.5);
        c[1] = pos[1] + (int) (R * cos(*pangle * PI / 32768) + 0.5);
        d = (dr[0] + dr[1]) / 2;

        // mise à jour des coordonnées du robot
        if (dr[0] + dr[1] != 0){
            *pangle += (int) (d * 32768.0 / R / PI);
        }
        else{
            *pangle += (int) (dr[1] * 2 * 32768.0 / e / PI);
        }

        pos[0] = c[0] + (int) (R * sin(*pangle * PI / 32768) + 0.5);
        pos[1] = c[1] - (int) (R * cos(*pangle * PI / 32768) + 0.5);
    }
    else if (dr[0] == dr[1]){ // cas où la trajectoire est une parfaite ligne droite
        pos[0] += (int) (dr[0] * cos(*pangle * PI / 32768));
        pos[1] += (int) (dr[0] * sin(*pangle * PI / 32768));
    }

  Serial.print("dr : ");
  Serial.print(dr[0]);
  Serial.print(", ");
  Serial.print(dr[1]);
  Serial.print("R : ");
  Serial.println(R);
}


void loop(){ 
  //Do stuff here
  
  Serial.print("tics : ");
  Serial.print(AencoderValue);
  Serial.print(", ");
  Serial.println(BencoderValue);
  
  Serial.print("tics totaux : ");
  Serial.print(AencoderValueb);
  Serial.print(", ");
  Serial.println(BencoderValueb);
  
  Serial.print("tics comptés : ");
  Serial.print(sumA);
  Serial.print(", ");
  Serial.println(sumB);
  
  getParametres(dr);
  updatePos(pos, &angle, dr);

 
  Serial.print("x : ");
  Serial.print(pos[0]);
  Serial.print(", y : ");
  Serial.print(pos[1]);
  Serial.print(" angle : ");
  Serial.println((float) (angle) * 180 / 32768);
  
  Serial.println("");
  
  delay(50); //just here to slow down the output, and show it will work  even during a delay
}
 
 
void AupdateEncoder(){
  int AMSB = digitalRead(AencoderPin1); //MSB = most significant bit
  int ALSB = digitalRead(AencoderPin2); //LSB = least significant bit
 
  int Aencoded = (AMSB << 1) |ALSB; //converting the 2 pin value to single number
  int Asum  = (AlastEncoded << 2) | Aencoded; //adding it to the previous encoded value
 
  if(Asum == 0b1101 || Asum == 0b0100 || Asum == 0b0010 || Asum == 0b1011) {AencoderValue ++; AencoderValueb++;}
  if(Asum == 0b1110 || Asum == 0b0111 || Asum == 0b0001 || Asum == 0b1000) {AencoderValue --; AencoderValueb--;}
 
  AlastEncoded = Aencoded; 
}

void BupdateEncoder(){
  int BMSB = digitalRead(BencoderPin1); //MSB = most significant bit
  int BLSB = digitalRead(BencoderPin2); //LSB = least significant bit
 
  int Bencoded = (BMSB << 1) |BLSB; //converting the 2 pin value to single number
  int Bsum  = (BlastEncoded << 2) | Bencoded; //adding it to the previous encoded value
 
  if(Bsum == 0b1101 || Bsum == 0b0100 || Bsum == 0b0010 || Bsum == 0b1011) {BencoderValue ++; BencoderValueb++;}
  if(Bsum == 0b1110 || Bsum == 0b0111 || Bsum == 0b0001 || Bsum == 0b1000) {BencoderValue --; BencoderValueb--;}
 
  BlastEncoded = Bencoded; 
}