File content as of revision 0:c25d26ef035b:

#include "IHM.h"
#include <math.h>

#define THRESHOLD 0.5
#define K (2.0 + get_pot())
#define SPEED 0.40
#define MAX_RATIO 2.5

// 100us
#define DT 0.0001

typedef enum {
    WAIT = 0,
    RUN,
    STOP
} RUN_STATE;

IHM ihm;
RUN_STATE run_state = WAIT;
Timer timer;

PwmOut mot1(PB_5);
PwmOut mot2(PB_4);

// AnaIn
AnalogIn ain(PB_1);
// AnaIn Select
BusOut select_analog(PA_8, PF_1, PF_0);
DigitalIn jack(PB_6);
DigitalIn pb(PB_7);

double min(double a, double b)
{
	return a < b ? a : b;
}

double max(double a, double b)
{
	return a > b ? a : b;
}

double borned(double n, double mi, double ma)
{
    return max(mi, min(ma, n));
}

double get_an(int i)
{
	select_analog = i;
    wait_us(1);
    return ain.read();
}

double get_di(int i)
{
	return get_an(i) < THRESHOLD;
}

double get_error()
{
	float n = 0;
	float facs[6] = {-3, -2, -1, 1, 2, 3};
	for (int i = 0; i < 6; i++)
		n += facs[i] * get_an(i);
	return (n);
}

double get_pot()
{
    return get_an(7);
}

void set_motors(double speed, double dir)
{
	double ratio = (MAX_RATIO - 1.0) / (MAX_RATIO + 1.0);
	double v1 = speed - dir * speed * ratio;
	double v2 = speed + dir * speed * ratio;
	mot1.write(v1);
	mot2.write(v2);
}

int sens_count()
{
	int n = 0;
    for (int i = 0; i < 6; i++)
        if (get_di(i))
            n++;
    return (n);
}

int main() {
	ihm.BAR_set(0);
	ihm.LCD_clear();
	ihm.LCD_gotoxy(0, 0);
	ihm.LCD_printf("EL TACOS");
	mot1.period_us(100);
	mot2.period_us(100);
	mot1.write(0);
	mot2.write(0);
	timer.start();
	while (1){
		// wait DT
		while (timer.read_us() < 1000);
		timer.reset();
		// RUN_STATE automata
		if (run_state == WAIT && jack)
			run_state = RUN;
		if (run_state == RUN && pb)
			run_state = STOP;
		if (run_state == STOP && !jack)
			run_state = WAIT;
		if (run_state == RUN)
		{
			if (sens_count() <= 2)
				set_motors(SPEED, borned(K * get_error() / 3.0, -1.0, 1.0));
		} else {
			mot1.write(0);
			mot2.write(0);
		}
	}
}