1 files changed, 33 insertions, 16 deletions

diff --git a/client/ui_dirc.c b/client/ui_dirc.c
index cddd2c4..61abaf4 100644
--- a/client/ui_dirc.c
+++ b/client/ui_dirc.c
@@ -25,26 +25,43 @@ int w2_avg(int* samples, unsigned int sample_count) {
 	return (int) total;
 }
 
-void w2_ui_dirc() {
-	static unsigned int idx = 0;
-	int ch = 0;
-	unsigned int presses = 0;
-
-	static double drive_l = 0.f;
-	static int drive_l_avg[W2_DIRC_SPL] = {0};
+#define W2_DIRC_MOTOR_DRIVER(name) \
+int w2_dirc_motor_##name(unsigned int forward, unsigned int backward) {\
+	static unsigned int idx = 0;\
+	\
+	static double drive = 0.f;\
+	static int drive_avg[W2_DIRC_SPL] = {0};\
+	\
+	drive *= W2_DIRC_MOD;\
+	drive += W2_DIRC_ADD * forward + -W2_DIRC_ADD * backward;\
+	drive = W2_RANGE(-254, drive, 255);\
+	\
+	idx = (idx + 1) % W2_DIRC_SPL;\
+	drive_avg[idx] = (int) W2_RANGE(-254, drive * W2_DIRC_PAD, 255);\
+	\
+	return w2_avg(drive_avg, W2_DIRC_SPL);\
+}
 
+W2_DIRC_MOTOR_DRIVER(l);
+W2_DIRC_MOTOR_DRIVER(r);
 
+void w2_ui_dirc() {
+	int ch = 0;
+	unsigned int lb = 0;
+	unsigned int lf = 0;
+	unsigned int rb = 0;
+	unsigned int rf = 0;
 	while ((ch = getch()) != -1) {
-		if (ch == 'a') presses++;
+		if (ch == 'a' || ch == 's') lb++;
+		if (ch == 'q' || ch == 'w') lf++;
+		if (ch == 'd' || ch == 's') rb++;
+		if (ch == 'e' || ch == 'w') rf++;
 	}
 
-	drive_l *= W2_DIRC_MOD;
-	drive_l += W2_DIRC_ADD * presses;
-	drive_l = W2_MIN(100, drive_l);
-
-	idx = (idx + 1) % W2_DIRC_SPL;
-	drive_l_avg[idx] = (int) W2_MIN(100, drive_l * W2_DIRC_PAD);
+	int drive_l = w2_dirc_motor_l(lf, lb);
+	int drive_r = w2_dirc_motor_r(rf, rb);
 
-	mvaddstr(4, 0, "drive: ");
-	w2_ui_bar_graph(w2_avg(drive_l_avg, W2_DIRC_SPL));
+	char temp[32] = {0};
+	sprintf(temp, "l: %04i, r: %04i", drive_l, drive_r);
+	mvaddstr(4, 0, temp);
 }