File content as of revision 5:22ad3005bc11:

// Coded by Luís Afonso 11-04-2019
#include "BufferedSerial.h"
#include "Robot.h"
#include "mbed.h"
#include "rplidar.h"
#include <cstdio>

BufferedSerial pc(USBTX, USBRX, 115200);
RPLidar lidar;
BufferedSerial se_lidar(PA_9, PA_10);
PwmOut rplidar_motor(D3);

#define rodaDist 7.0
#define eixoDist 14.05
#define PI 3.14159265358979323846f

float distLeft = 0.0f;
float distRight = 0.0f;
float distCenter = 0.0f;
float deltaTheta = 0.0f;
double x = 0.0f;
double y = 0.0f;
double theta = 0.0f;
float speedL = 0.0f;
float speedR = 0.0f;

float currentTime = 0.0f;
float lastTime = 0.0f;
float timeStep = 0.0f;

void updateOdometer(int countsLeft, int countsRight);
Timer t;
using namespace std::chrono;

int main() {
  float odomX, odomY, odomTheta;
  // Lidar initialization
  setSpeeds(100, 100);

  while (1) {
    getCountsAndReset();
    updateOdometer(countsLeft, countsRight);

    printf("X: %f\tY: %f\t T: %f\n", x, y, theta);
    printf("SpeedL: %f\tSpeedR: %f\n", speedL, speedR);
  }
}

void updateOdometer(int countsLeft, int countsRight) {
  currentTime = t.read_us();
  timeStep = currentTime - lastTime;

  lastTime = currentTime;

  printf("Left: %d\tRight: %d\n", countsLeft, countsRight);

  distLeft = (double)countsLeft * ((rodaDist * PI) / 1440.0);
  distRight = (double)countsRight * ((rodaDist * PI) / 1440.0);
  distCenter = (distLeft + distRight) / 2;
  deltaTheta = (distRight - distLeft) / eixoDist;
  deltaTheta = atan2(sin(deltaTheta), cos(deltaTheta));

  if (deltaTheta == 0) {
    x = x + distCenter * cos(theta);
    y = y + distCenter * sin(theta);
  } else {
    x = x + distCenter * sin(theta / 2.0) * cos(theta + deltaTheta / 2.0) /
                (deltaTheta / 2.0);
    y = y + distCenter * sin(theta / 2.0) * sin(theta + deltaTheta / 2.0) /
                (deltaTheta / 2.0);
    theta += deltaTheta;
    theta = atan2(sin(theta), cos(theta));
  }

  speedL = distLeft / float(timeStep / 1E6);
  speedR = distRight / float(timeStep / 1E6);
}