1 files changed, 14 insertions, 21 deletions

diff --git a/robot/main.c b/robot/main.c
index 21d0e5c..a708a13 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -1,33 +1,26 @@
 #include <pololu/orangutan.h>
 #include <stdlib.h>
 
+unsigned char hex_to_int(char a) {
+	if (a >= 0x30 && a <= 0x39) return a - 0x30;
+	else if (a >= 0x61 && a <= 0x66) return a - 0x61 + 10;
+	return 0;
+}
+
 int main() {
 	play("L50 c>c");
 	serial_set_baud_rate(9600);
 
-	char *buf = malloc(20);
-	unsigned int counter = 0;
-
+	char *buf = malloc(9);
 	while (1) {
-		serial_receive_blocking(buf, 1, 65e3);
+		serial_receive_blocking(buf, 8, 65e3);
+		
+		int modl = hex_to_int(buf[1]) - 1;
+		int speedl = hex_to_int(buf[2]) * 0x10 + hex_to_int(buf[3]);
+		int modr = hex_to_int(buf[5]) - 1;
+		int speedr = hex_to_int(buf[6]) * 0x10 + hex_to_int(buf[7]);
 
-		switch (buf[0]) {
-			case 0x7f: {
-				counter--;
-				lcd_goto_xy(counter, 0);
-				print(" ");
-				lcd_goto_xy(counter, 0);
-				break;
-			}
-			default: {
-				print(&buf[0]);
-				counter++;
-				if (counter > 20) {
-					counter = 0;
-					lcd_goto_xy(0, 0);
-				}
-			}
-		}
+		set_motors(modl * speedl, modr * speedr);
 	}
 
 	return 0;