aboutsummaryrefslogtreecommitdiff
path: root/zumo
diff options
context:
space:
mode:
Diffstat (limited to 'zumo')
-rw-r--r--zumo/.gitignore1
-rw-r--r--zumo/control.cpp43
-rw-r--r--zumo/control.h12
-rw-r--r--zumo/pidtest/.gitignore1
-rw-r--r--zumo/pidtest/makefile (renamed from zumo/pidtest.mk)4
-rw-r--r--zumo/pidtest/pidtest.cpp (renamed from zumo/pidtest.cpp)2
-rw-r--r--zumo/protocol.cpp32
-rw-r--r--zumo/protocol.h4
-rw-r--r--zumo/protocoltest/.gitignore1
-rw-r--r--zumo/protocoltest/makefile21
-rw-r--r--zumo/protocoltest/protocoltest.cpp35
-rw-r--r--zumo/zumo.ino25
12 files changed, 140 insertions, 41 deletions
diff --git a/zumo/.gitignore b/zumo/.gitignore
index e45f7a2..5761abc 100644
--- a/zumo/.gitignore
+++ b/zumo/.gitignore
@@ -1,2 +1 @@
*.o
-pidtest
diff --git a/zumo/control.cpp b/zumo/control.cpp
new file mode 100644
index 0000000..778969a
--- /dev/null
+++ b/zumo/control.cpp
@@ -0,0 +1,43 @@
+#include <Zumo32U4Motors.h>
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "protocol.h"
+#include "control.h"
+
+#define DUI_SPEED_MOD 96.0f
+#define DUI_MOTOR_DIFF 0.6f
+
+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;
+
+ Serial.print(motor_l);
+ Serial.print(" ");
+ Serial.print(motor_r);
+
+ Serial.println("");
+
+ Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
+ Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
+
+ // TODO: print sign on OLED screen
+}
+
+inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); }
+
+unsigned char uart_read() {
+ if (rx() == true) return 0x00; // return immediately if line is idle
+
+ delayMicroseconds(1200); // wait out start bit
+
+ unsigned char byte = 0x00;
+ for (unsigned int i = 0; i < 8; i++) {
+ byte = (byte << 1) | rx();
+ delayMicroseconds(1000);
+ }
+
+ delayMicroseconds(1000); // wait out stop bit
+
+ return byte;
+}
diff --git a/zumo/control.h b/zumo/control.h
new file mode 100644
index 0000000..12ecbd7
--- /dev/null
+++ b/zumo/control.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "protocol.h"
+
+#define DUI_PINOUT_NICLA_TX 13
+#define DUI_PINOUT_NICLA_RX 14
+
+/** @brief non blocking read byte */
+unsigned char uart_read();
+/** @brief apply state to motors */
+void apply_state(dui_state_t* state);
+
diff --git a/zumo/pidtest/.gitignore b/zumo/pidtest/.gitignore
new file mode 100644
index 0000000..41dc46e
--- /dev/null
+++ b/zumo/pidtest/.gitignore
@@ -0,0 +1 @@
+pidtest
diff --git a/zumo/pidtest.mk b/zumo/pidtest/makefile
index 5ffe1e1..832d60b 100644
--- a/zumo/pidtest.mk
+++ b/zumo/pidtest/makefile
@@ -5,8 +5,8 @@ CFLAGS =
LFLAGS =
TARGET = pidtest
-SRCS := pidtest.cpp pid.cpp
-OBJS := pidtest.o pid.o
+SRCS := pidtest.cpp ../pid.cpp
+OBJS := pidtest.o ../pid.o
all: pidtest
diff --git a/zumo/pidtest.cpp b/zumo/pidtest/pidtest.cpp
index b9ce50b..4d047fb 100644
--- a/zumo/pidtest.cpp
+++ b/zumo/pidtest/pidtest.cpp
@@ -1,7 +1,7 @@
#include <cstdio>
#include <random>
-#include "pid.h"
+#include "../pid.h"
int main() {
float P, I, D;
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
index fea8a00..ab8397a 100644
--- a/zumo/protocol.cpp
+++ b/zumo/protocol.cpp
@@ -1,39 +1,21 @@
-#include <Zumo32U4Motors.h>
-
#include "protocol.h"
-#define DUI_SPEED_MOD 96.0f
-#define DUI_MOTOR_DIFF 0.6f
-
#define DUI_CMD_NULL 0x00
#define DUI_CMD_SIGN_START 0x01
#define DUI_CMD_SIGN_END 0x0f
-#define DUI_CMD_STEER_START 0x10
-#define DUI_CMD_STEER_END 0x1f
-#define DUI_CMD_SPEED_START 0x20
-#define DUI_CMD_SPEED_END 0xff
+#define DUI_CMD_SPEED_START 0x10
+#define DUI_CMD_SPEED_END 0x1f
+#define DUI_CMD_STEER_START 0x20
+#define DUI_CMD_STEER_END 0xff
void handle_cmd(unsigned char cmd, dui_state_t *state) {
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);
- } 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);
} else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) {
- state->speed = ((float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_START - DUI_CMD_SPEED_END) * (float) 2 - (float) 1);
+ state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START);
+ } 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;
}
}
-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;
-
- Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
- Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
-
- // TODO: print sign on OLED screen
-}
-
-unsigned char uart_read() {
- return 0x00;
-}
diff --git a/zumo/protocol.h b/zumo/protocol.h
index 662a5ce..a1f9951 100644
--- a/zumo/protocol.h
+++ b/zumo/protocol.h
@@ -26,10 +26,6 @@ typedef struct {
float speed_mod; /** @brief global speed multiplier */
} dui_state_t;
-/** @brief non blocking read byte */
-unsigned char uart_read();
/** @brief read and apply cmd to state */
void handle_cmd(unsigned char cmd, dui_state_t *state);
-/** @brief apply state to motors */
-void apply_state(dui_state_t* state);
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);
+}
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
index f59bd36..1a64381 100644
--- a/zumo/zumo.ino
+++ b/zumo/zumo.ino
@@ -2,33 +2,42 @@
#include <Arduino.h>
#include <Wire.h>
+#include "control.h"
#include "protocol.h"
#include "pid.h"
dui_state_t g_dui_target_state = {
- .steer = 1.0f,
- .speed = 1.0f,
+ .steer = 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 = 1.f,
+ .speed = 0.f,
.current_sign = DUI_SIGN_NONE,
.speed_mod = 1.f,
};
void setup() {
+ pinMode(DUI_PINOUT_NICLA_TX, OUTPUT);
+ pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP);
+ Serial.begin(115200);
}
void loop() {
- unsigned char cmd = 0x00;
- while ((cmd = uart_read()))
- handle_cmd(cmd, &g_dui_target_state);
+ static unsigned char cmd_old = 0x00;
+ for (unsigned int i = 0; i < 1000; i++) {
+ digitalWrite(DUI_PINOUT_NICLA_TX, LOW);
+ unsigned char cmd = uart_read();
+ if (cmd == 0x00) continue;
+ if (cmd == cmd_old) handle_cmd(cmd, &g_dui_target_state);
+ cmd_old = cmd;
+ }
+ digitalWrite(DUI_PINOUT_NICLA_TX, HIGH);
apply_pid(&g_dui_target_state, &g_dui_current_state);
+ g_dui_current_state.current_sign = g_dui_target_state.current_sign;
apply_state(&g_dui_current_state);
-
- delay(10);
}