Odometrie

/*****************************************************************
*   Author: KHADRAOUI Ibrahim
*   Company: USTHB FEI
*   Date:   July 2019
*   Project: Odometry on my proto robot
***********
*   Description: Print on the serial monitor the x,y coordinates
*               of the current position of the robot and the
*               angle of the robot by using odometry and
*               wheele encodeurs.
*
******************************************************************/

#include <math.h>
#define pi 3.141592

////////////////////////////////////////////////////
// Variables Robot
////////////////////////////////////////////////////
const float N=231;  //nombre de tick par tour (encodeur)
const float D=6.7;  //diametre roues
const float L=17;   //distance entre les deux roues
////////////////////////////////////////////////////

////////////////////////////////////////////////////
// Variables Odometerie
////////////////////////////////////////////////////
volatile float countD,countG;
float distanceD,distanceG,distanceC;
float x,y,phi;
float countDLast,countGLast,deltaCountD,deltaCountG;
////////////////////////////////////////////////////



void setup() {

  pinMode(2,INPUT);pinMode(11,INPUT);
  pinMode(3,INPUT);pinMode(12,INPUT);
  attachInterrupt(digitalPinToInterrupt(2), tikG, FALLING);
  attachInterrupt(digitalPinToInterrupt(3), tikD, FALLING);
  Serial.begin(9600);
  delay(1000);

  distanceD = distanceG = distanceC = x = y = phi = 0;
  countDLast = countGLast = 0;

  currentTime=millis();
}

void loop() {

while(millis()-currentTime<50){ //50ms calcule de la distance parcourue par les deux roues
    interrupts();
    }

 noInterrupts();
 odom();
 currentTime=millis();
}

void tikG() {
  noInterrupts();
  if(digitalRead(11)==HIGH) countG++;
  else countG--;
}

void tikD() {
  noInterrupts();
  if(digitalRead(12)==HIGH) countD--;
  else countD++;
}

void odom(){

//Delta count:
//number of count since the last sample
deltaCountD = countD-countDLast;
deltaCountG = countG-countGLast;

// distance traveled by the two wheels
distanceD = pi*D*(deltaCountD/N);
distanceG = pi*D*(deltaCountG/N);

// distance traveled by the robot
distanceC = (distanceD+distanceG)/2;

// x&y coordinates of the robot
x = x+(distanceC*cos(phi));
y = y+(distanceC*sin(phi));

// angle of the robot
phi = phi+((distanceD-distanceG)/L);
phi = atan2(sin(phi),cos(phi));


Serial.print("x:   ");Serial.print(x);
Serial.print("                ");
Serial.print("y:   ");Serial.print(y);
Serial.print("                ");
Serial.print("phi:   ");Serial.println(phi);

/*
Serial.print("D:   ");Serial.print(distanceD);
Serial.print("                ");
Serial.print("G:   ");Serial.print(distanceG);
Serial.print("                ");
Serial.print("C:   ");Serial.println(distanceC);
*/
countDLast=countD;
countGLast=countG;
}