fix protocol implementation

3 files changed, 57 insertions, 0 deletions

diff --git a/zumo/protocoltest/.gitignore b/zumo/protocoltest/.gitignore
new file mode 100644
index 0000000..fb5d35f
--- /dev/null
+++ b/zumo/protocoltest/.gitignore
@@ -0,0 +1 @@
+protocoltest
diff --git a/zumo/protocoltest/makefile b/zumo/protocoltest/makefile
new file mode 100644
index 0000000..7a69352
--- /dev/null
+++ b/zumo/protocoltest/makefile
@@ -0,0 +1,21 @@
+CPP = g++
+LD = g++
+RM = rm -f
+CFLAGS =
+LFLAGS =
+TARGET = protocoltest
+
+SRCS := $(TARGET).cpp ../protocol.cpp
+OBJS := $(TARGET).o ../protocol.o
+
+all: $(TARGET)
+
+%.o: %.cpp
+	$(CPP) -c $(CFLAGS) $< -o $@
+
+$(TARGET): $(OBJS)
+	$(LD) $^ $(LFLAGS) -o $@
+
+clean:
+	$(RM) $(TARGET) $(OBJS)
+
diff --git a/zumo/protocoltest/protocoltest.cpp b/zumo/protocoltest/protocoltest.cpp
new file mode 100644
index 0000000..461221e
--- /dev/null
+++ b/zumo/protocoltest/protocoltest.cpp
@@ -0,0 +1,35 @@
+#include <cstdio>
+#include <random>
+
+#include "../protocol.h"
+
+void print_state(dui_state_t* s) {
+	printf("{ steer = %1.3f\n  speed = %1.3f }\n", s->steer, s->speed);
+}
+
+void test_case(unsigned char cmd, dui_state_t* s) {
+	printf("\ncmd in = 0x%02x\n", cmd);
+	handle_cmd(cmd, s);
+	print_state(s);
+}
+
+int main() {
+	dui_state_t state = {
+		.steer = 0.f,
+		.speed = 1.f,
+		.current_sign = DUI_SIGN_NONE,
+		.speed_mod = 1.f,
+	};
+
+	test_case(0x00, &state);
+	test_case(0x01, &state);
+	test_case(0x02, &state);
+	test_case(0x10, &state);
+	test_case(0x15, &state);
+	test_case(0x1f, &state);
+	test_case(0x20, &state);
+	test_case(0x25, &state);
+	test_case(0x2f, &state);
+	test_case(0x88, &state);
+	test_case(0xff, &state);
+}