File content as of revision 1:d784b3e1fb18:

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

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

// 100us
#define DT 0.0001

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

typedef enum {
    CAL_DARK = 0,
    CAL_LIGHT_W,
    CAL_LIGHT,
    CAL_DONE_W,
    CAL_DONE
} CAL_STATE;

double light_val[6] = {0};
double dark_val[6] = {0};

IHM ihm;
RUN_STATE run_state = WAIT;
CAL_STATE cal_state = CAL_DARK;
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_sens(int i)
{
	return 1.0 - get_an(i);
}

double get_an_norm(int i)
{
    return ((get_sens(i) - dark_val[i]) / (light_val[i] - dark_val[i]));
}

double get_di(int i)
{
	return get_an_norm(i) > 0.5;
}

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_norm(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);
	mot1.period_us(100);
	mot2.period_us(100);
	mot1.write(0);
	mot2.write(0);
	while (cal_state != CAL_DONE)
	{
		switch (cal_state)
		{
			case CAL_DARK:
				ihm.LCD_clear();
				ihm.LCD_gotoxy(0, 0);
				ihm.LCD_printf("DARK");
				if (pb)
				{
					for (int i = 0; i < 6; i++)
						dark_val[i] = get_sens(i);
					cal_state = CAL_LIGHT_W;
				}
				break;
			case CAL_LIGHT_W:
				if (!pb)
					cal_state = CAL_LIGHT;
				break;
			case CAL_LIGHT:
				ihm.LCD_clear();
				ihm.LCD_gotoxy(0, 0);
				ihm.LCD_printf("LIGHT");
				if (pb)
				{
					for (int i = 0; i < 6; i++)
						light_val[i] = get_sens(i);
					cal_state = CAL_DONE_W;
				}
				break;
			case CAL_DONE_W:
				if (!pb)
					cal_state = CAL_DONE;
				break;
			default:
				break;
		}
		wait_ms(100);
	}	
	ihm.LCD_clear();
	ihm.LCD_gotoxy(0, 0);
	ihm.LCD_printf("DURUM-DURUM");
	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);
		}
	}
}