Diff: main.cpp

Revision:

1:d784b3e1fb18

Parent:

0:c25d26ef035b

--- a/main.cpp	Mon Jan 21 16:58:36 2019 +0000
+++ b/main.cpp	Tue Jan 22 08:57:38 2019 +0000
@@ -2,8 +2,8 @@
 #include <math.h>
 
 #define THRESHOLD 0.5
-#define K (2.0 + get_pot())
-#define SPEED 0.40
+#define K 2.33 //(2.0 + get_pot())
+#define SPEED get_pot()
 #define MAX_RATIO 2.5
 
 // 100us
@@ -15,8 +15,20 @@
     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);
@@ -51,9 +63,19 @@
     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(i) < THRESHOLD;
+	return get_an_norm(i) > 0.5;
 }
 
 double get_error()
@@ -61,7 +83,7 @@
 	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);
+		n += facs[i] * get_an_norm(i);
 	return (n);
 }
 
@@ -90,13 +112,52 @@
 
 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);
+	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
@@ -112,7 +173,7 @@
 		if (run_state == RUN)
 		{
 			if (sens_count() <= 2)
-				set_motors(SPEED, borned(K * get_error() / 3.0, -1.0, 1.0));
+				set_motors(SPEED, borned(-K * get_error() / 3.0, -1.0, 1.0));
 		} else {
 			mot1.write(0);
 			mot2.write(0);