Diff: main.cpp

Revision:

0:c25d26ef035b

Child:

1:d784b3e1fb18

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jan 21 16:58:36 2019 +0000
@@ -0,0 +1,123 @@
+#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);
+		}
+	}
+}
+
+