#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(9);
	while (1) {
		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]);

		set_motors(modl * speedl, modr * speedr);
	}

	return 0;
}