summaryrefslogtreecommitdiff
path: root/zumo
diff options
context:
space:
mode:
Diffstat (limited to 'zumo')
-rw-r--r--zumo/.gitignore1
-rw-r--r--zumo/main.cpp13
-rw-r--r--zumo/pid.cpp34
-rw-r--r--zumo/pid.h20
-rw-r--r--zumo/pidtest.cpp35
-rw-r--r--zumo/pidtest.mk21
-rw-r--r--zumo/protocol.cpp39
-rw-r--r--zumo/protocol.h35
-rw-r--r--zumo/zumo.ino34
9 files changed, 219 insertions, 13 deletions
diff --git a/zumo/.gitignore b/zumo/.gitignore
index d3e1535..888e905 100644
--- a/zumo/.gitignore
+++ b/zumo/.gitignore
@@ -3,3 +3,4 @@ main
*.hex
compile_commands.json
.cache
+pidtest
diff --git a/zumo/main.cpp b/zumo/main.cpp
deleted file mode 100644
index 23d13db..0000000
--- a/zumo/main.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include <Zumo32U4.h>
-#include <Arduino.h>
-#include <Wire.h>
-
-void setup() {
-}
-
-void loop() {
- ledRed(1);
- delay(1000);
- ledRed(0);
- delay(1000);
-}
diff --git a/zumo/pid.cpp b/zumo/pid.cpp
new file mode 100644
index 0000000..9bef08f
--- /dev/null
+++ b/zumo/pid.cpp
@@ -0,0 +1,34 @@
+#include "pid.h"
+
+PID::PID(float P, float I, float D) {
+ A0 = P + I*dt + D/dt;
+ A1 = -P - 2*D/dt;
+ A2 = D/dt;
+}
+
+// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
+float PID::iter(float target) {
+ error[2] = error[1];
+ error[1] = error[0];
+ error[0] = target - output;
+ output = output + A0 * error[0] + A1 * error[1] + A2 * error[2];
+ return output;
+}
+
+void PID::reset(float value) {
+ error[0] = 0.0;
+ error[1] = 0.0;
+ error[2] = 0.0;
+ output = value;
+}
+
+PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values)
+PID steer_pid(0.1, 0.0, 0.0);
+PID speed_mod_pid(1, 1, 1);
+void apply_pid(dui_state_t* target, dui_state_t* current) {
+ current->speed = speed_pid.iter(target->speed);
+ current->steer = steer_pid.iter(target->steer);
+ // current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod);
+ current->speed_mod = target->speed_mod;
+}
+
diff --git a/zumo/pid.h b/zumo/pid.h
new file mode 100644
index 0000000..fbcd063
--- /dev/null
+++ b/zumo/pid.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "protocol.h"
+
+class PID {
+private:
+ float A0, A1, A2;
+ float error[3] = {0};
+ float dt = 0.010;
+ float output = 0;
+
+public:
+ PID(float P, float I, float D);
+ float iter(float target);
+ void reset(float value);
+};
+
+/** @brief edit `current` to be closer to `target` using PID controllers */
+void apply_pid(dui_state_t* target, dui_state_t* current);
+
diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp
new file mode 100644
index 0000000..f5198bb
--- /dev/null
+++ b/zumo/pidtest.cpp
@@ -0,0 +1,35 @@
+#include <cstdio>
+#include <random>
+
+#include "pid.h"
+
+std::random_device rd;
+std::mt19937 rng(rd());
+std::uniform_real_distribution<float> uni(0,13);
+
+auto random_integer = uni(rng);
+
+int main() {
+ float P, I, D;
+ do {
+ // P = uni(rng);
+ // I = uni(rng);
+ // D = uni(rng);
+ P = 10;
+ I = 0.1;
+ D = 10;
+ PID test(P, I, D);
+ test.reset(0.0);
+
+ float val = 0;
+ for (unsigned int i = 0; i < 100; i++) val = test.iter(1.0);
+ // if (val > 0.999 && val < 1.001) {
+ fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D);
+ test.reset(0.0);
+ for (unsigned int i = 0; i < 100; i++) {
+ printf("%2.8f\n", test.iter(1.0));
+ }
+ exit(0);
+ // }
+ } while (false);
+}
diff --git a/zumo/pidtest.mk b/zumo/pidtest.mk
new file mode 100644
index 0000000..5ffe1e1
--- /dev/null
+++ b/zumo/pidtest.mk
@@ -0,0 +1,21 @@
+CPP = g++
+LD = g++
+RM = rm -f
+CFLAGS =
+LFLAGS =
+TARGET = pidtest
+
+SRCS := pidtest.cpp pid.cpp
+OBJS := pidtest.o pid.o
+
+all: pidtest
+
+%.o: %.cpp
+ $(CPP) -c $(CFLAGS) $< -o $@
+
+$(TARGET): $(OBJS)
+ $(LD) $^ $(LFLAGS) -o $@
+
+clean:
+ $(RM) $(TARGET) $(OBJS)
+
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
new file mode 100644
index 0000000..fea8a00
--- /dev/null
+++ b/zumo/protocol.cpp
@@ -0,0 +1,39 @@
+#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
+
+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);
+ }
+}
+
+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
new file mode 100644
index 0000000..662a5ce
--- /dev/null
+++ b/zumo/protocol.h
@@ -0,0 +1,35 @@
+#pragma once
+
+typedef enum {
+ DUI_CMD_NULL,
+ DUI_CMD_SIGN,
+ DUI_CMD_SPEED,
+ DUI_CMD_STEER,
+} dui_e_cmd;
+
+typedef enum {
+ DUI_SIGN_NONE, /** @brief no sign */
+ DUI_SIGN_STOP, /** @brief stop (set speed to 0) */
+ DUI_SIGN_LEFT, /** @brief turn left (set steer to -1) */
+ DUI_SIGN_RIGHT, /** @brief turn right (set steer to +1) */
+ DUI_SIGN_SPEED_LIMIT_LOW, /** @brief slow down (speed limit 0.5) */
+ DUI_SIGN_SPEED_LIMIT_HIGH, /** @brief full speed (speed limit 1.0) */
+ DUI_SIGN_LIGHT_STOP, /** @brief traffic light red (set speed to 0) */
+ DUI_SIGN_LIGHT_FLOOR_IT, /** @brief traffic light orange (set speed to 2 temporarily) */
+ DUI_SIGN_LIGHT_GO, /** @brief traffic light green (keep current speed) */
+} dui_e_sign;
+
+typedef struct {
+ float steer; /** @brief steer value (-1 is left, +1 is right) */
+ 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 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/zumo.ino b/zumo/zumo.ino
new file mode 100644
index 0000000..f59bd36
--- /dev/null
+++ b/zumo/zumo.ino
@@ -0,0 +1,34 @@
+#include <Zumo32U4.h>
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "protocol.h"
+#include "pid.h"
+
+dui_state_t g_dui_target_state = {
+ .steer = 1.0f,
+ .speed = 1.0f,
+ .current_sign = DUI_SIGN_NONE,
+ .speed_mod = 1.f,
+};
+dui_state_t g_dui_current_state = {
+ .steer = 0.f,
+ .speed = 1.f,
+ .current_sign = DUI_SIGN_NONE,
+ .speed_mod = 1.f,
+};
+
+void setup() {
+}
+
+void loop() {
+ unsigned char cmd = 0x00;
+ while ((cmd = uart_read()))
+ handle_cmd(cmd, &g_dui_target_state);
+
+ apply_pid(&g_dui_target_state, &g_dui_current_state);
+
+ apply_state(&g_dui_current_state);
+
+ delay(10);
+}