5 files changed, 28 insertions, 16 deletions
diff --git a/zumo/control.cpp b/zumo/control.cpp
index 778969a..71e7537 100644
--- a/zumo/control.cpp
+++ b/zumo/control.cpp
@@ -6,11 +6,11 @@
 #include "control.h"
 
 #define DUI_SPEED_MOD 96.0f
-#define DUI_MOTOR_DIFF 0.6f
+#define DUI_MOTOR_DIFF 0.62f
 
 void apply_state(dui_state_t *state) {
-	float motor_l = 0.5f * state->speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
-	float motor_r = 0.5f * state->speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
+	float motor_l = 0.5f * state->Speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
+	float motor_r = 0.5f * state->Speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
 
 	Serial.print(motor_l);
 	Serial.print(" ");
@@ -18,8 +18,7 @@ void apply_state(dui_state_t *state) {
 
 	Serial.println("");
 
-	Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
-	Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
+	Zumo32U4Motors::setSpeeds((int16_t) motor_l,(int16_t) motor_r);
 
 	// TODO: print sign on OLED screen
 }
@@ -38,6 +37,5 @@ unsigned char uart_read() {
 	}
 
 	delayMicroseconds(1000); // wait out stop bit
-
 	return byte;
 }
diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index 7c1cbda..17588e3 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -45,8 +45,7 @@ PID speed_pid = PID();
 PID steer_pid = PID();
 PID speed_mod_pid = PID();
 void apply_pid(dui_state_t* target, dui_state_t* current) {
-	current->speed = speed_pid.iter(target->speed);
+	current->Speed = speed_pid.iter(target->Speed);
 	current->steer = steer_pid.iter(target->steer);
 	current->speed_mod = speed_mod_pid.iter(target->speed_mod);
 }
-
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
index ab8397a..d176c24 100644
--- a/zumo/protocol.cpp
+++ b/zumo/protocol.cpp
@@ -1,5 +1,8 @@
-#include "protocol.h"
+#include <Zumo32U4.h>
 
+#include "protocol.h"
+#include <Arduino.h>
+#include <Zumo32U4LCD.h>
 #define DUI_CMD_NULL 0x00
 #define DUI_CMD_SIGN_START 0x01
 #define DUI_CMD_SIGN_END 0x0f
@@ -7,15 +10,22 @@
 #define DUI_CMD_SPEED_END 0x1f
 #define DUI_CMD_STEER_START 0x20
 #define DUI_CMD_STEER_END 0xff
+Zumo32U4OLED display;
 
 void handle_cmd(unsigned char cmd, dui_state_t *state) {
+    Serial.println(cmd,HEX);
+
 	if (cmd == DUI_CMD_NULL) return;
 	else if (DUI_CMD_SIGN_START <= cmd && cmd <= DUI_CMD_SIGN_END) {
 		state->current_sign = (dui_e_sign) (cmd - DUI_CMD_SIGN_START);
+    display.clear();
+    display.print(state->current_sign+1);
+    display.gotoXY(0, 1);
 	} else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) {
-		state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START);
+    Serial.print(" Hallo: " );
+		state->Speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START);
+   Serial.println(state->Speed);
 	} else if (DUI_CMD_STEER_START <= cmd && cmd <= DUI_CMD_STEER_END) {
 		state->steer = (float) (cmd - DUI_CMD_STEER_START) / (float) (DUI_CMD_STEER_END - DUI_CMD_STEER_START) * (float) 2 - (float) 1;
 	}
 }
-
diff --git a/zumo/protocol.h b/zumo/protocol.h
index a1f9951..5eb2343 100644
--- a/zumo/protocol.h
+++ b/zumo/protocol.h
@@ -21,11 +21,10 @@ typedef enum {
 
 typedef struct {
 	float steer; /** @brief steer value (-1 is left, +1 is right) */
-	float speed; /** @brief speed (0-15) */
+	float Speed; /** @brief speed (0-15) */
 	dui_e_sign current_sign; /** @brief last seen sign */
 	float speed_mod; /** @brief global speed multiplier */
 } dui_state_t;
 
 /** @brief read and apply cmd to state */
 void handle_cmd(unsigned char cmd, dui_state_t *state);
-
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
index 1a64381..a4f4d6b 100644
--- a/zumo/zumo.ino
+++ b/zumo/zumo.ino
@@ -2,19 +2,20 @@
 #include <Arduino.h>
 #include <Wire.h>
 
+
 #include "control.h"
 #include "protocol.h"
 #include "pid.h"
 
 dui_state_t g_dui_target_state = {
 	.steer = 0.0f,
-	.speed = 0.0f,
+	.Speed = 0.0f,
 	.current_sign = DUI_SIGN_NONE,
 	.speed_mod = 1.f,
 };
 dui_state_t g_dui_current_state = {
 	.steer = 0.f,
-	.speed = 0.f,
+	.Speed = 0.f,
 	.current_sign = DUI_SIGN_NONE,
 	.speed_mod = 1.f,
 };
@@ -22,10 +23,13 @@ dui_state_t g_dui_current_state = {
 void setup() {
 	pinMode(DUI_PINOUT_NICLA_TX, OUTPUT);
 	pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP);
-	Serial.begin(115200);
+	Serial.begin(11500);
+
 }
 
 void loop() {
+
+
 	static unsigned char cmd_old = 0x00;
 	for (unsigned int i = 0; i < 1000; i++) {
 		digitalWrite(DUI_PINOUT_NICLA_TX, LOW);
@@ -40,4 +44,6 @@ void loop() {
 	g_dui_current_state.current_sign = g_dui_target_state.current_sign;
 
 	apply_state(&g_dui_current_state);
+
+ 
 }