OdomDiffDriveEncoder

#include <mbed.h>
#include <math.h>
#include "QEI.h"

#define SEP_DIST 0.6
#define RATIO 3926.991 // 2 * PI / (1600, 400) * 1000000 * ratio gear
#define WHEEL_RADIUS 0.05
Timer tt;
Serial pc(PB_10, PB_11);
QEI enc_right(PA_9, PA_10, NC, 400, QEI::X4_ENCODING);
QEI enc_left(PA_11, PA_12, NC, 400, QEI::X4_ENCODING);

DigitalOut led(LED1);

struct Wheel
{
	int pulse;
	int lastPulse;
	float vel;
};

struct Odom
{
	float x;
	float y;
	float th;
	float v;
	float w;
	float R;
};

Wheel wheel_left, wheel_right;
Odom odom;

unsigned long pre_time[2] = {0, 0};
unsigned long current_time = 0;
unsigned long dt = 0;
float dts;

int main()
{
	tt.start();
	wheel_left.lastPulse = 0;
	wheel_right.lastPulse = 0;
	odom.x = 0;
	odom.y = 0;
	odom.th = 0;
	// odom.th = M_PI_2 / 2;

	while (1)
	{
		current_time = tt.read_high_resolution_us();
		dt = current_time - pre_time[0];
		if (dt > 10000)
		{
			dts = dt / 1000000.0;
			wheel_left.pulse = 0; //enc_left.getPulses();
			wheel_right.pulse = enc_right.getPulses();

			wheel_left.vel = (wheel_left.pulse - wheel_left.lastPulse) * RATIO / float(dt) * WHEEL_RADIUS;
			wheel_left.lastPulse = wheel_left.pulse;

			wheel_right.vel = (wheel_right.pulse - wheel_right.lastPulse) * RATIO / float(dt) * WHEEL_RADIUS;
			wheel_right.lastPulse = wheel_right.pulse;

			odom.w = (wheel_right.vel - wheel_left.vel) / (SEP_DIST);
			odom.v = (wheel_right.vel + wheel_left.vel) / 2;
			// R = DIST * (wheel_right.vel + wheel_left.vel) / (wheel_right.vel - wheel_left.vel);
			// odom.R = odom.v / odom.w;

			odom.th += odom.w * dts;
			odom.x += odom.v * cos(odom.th) * dts;
			odom.y += odom.v * sin(odom.th) * dts;

			pre_time[0] = current_time;
		}
		if (current_time - pre_time[1] > 50000)
		{
			led = !led;
			pc.printf("%.3f %.3f %.3f\n", odom.x, odom.y, odom.th);
			// pc.printf("%d %f\n", wheel_left.pulse, wheel_left.vel);
			pre_time[1] = current_time;
		}
	}
}