diff options
| -rw-r--r-- | .editorconfig | 11 | ||||
| -rw-r--r-- | doc/.gitignore | 17 | ||||
| -rw-r--r-- | doc/base.tex | 56 | ||||
| -rw-r--r-- | doc/dui.md | 100 | ||||
| -rw-r--r-- | doc/dui.tex | 3 | ||||
| -rw-r--r-- | doc/makefile | 20 | ||||
| -rw-r--r-- | zumo/.gitignore | 1 | ||||
| -rw-r--r-- | zumo/main.cpp | 13 | ||||
| -rw-r--r-- | zumo/pid.cpp | 34 | ||||
| -rw-r--r-- | zumo/pid.h | 20 | ||||
| -rw-r--r-- | zumo/pidtest.cpp | 35 | ||||
| -rw-r--r-- | zumo/pidtest.mk | 21 | ||||
| -rw-r--r-- | zumo/protocol.cpp | 39 | ||||
| -rw-r--r-- | zumo/protocol.h | 35 | ||||
| -rw-r--r-- | zumo/zumo.ino | 34 |
15 files changed, 426 insertions, 13 deletions
diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..a9383e8 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,11 @@ +root = true + +[*] +indent_style = tab +indent_size = 4 +end_of_line = lf +insert_final_newline = true + +[*.md] +indent_style = space +indent_size = 2 diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 0000000..25dcb3b --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1,17 @@ +# latex files +*.aux +*.bbl +*.bcf +*.blg +*.fdb_latexmk +*.fls +*.log +*.out +*.run.xml +*.synctex.gz +*.toc +*.synctex(busy) +*.md.tex + +# ignore output files +*.pdf diff --git a/doc/base.tex b/doc/base.tex new file mode 100644 index 0000000..9c1c908 --- /dev/null +++ b/doc/base.tex @@ -0,0 +1,56 @@ +\documentclass[11pt, a4paper, english]{article} +\usepackage[margin=1in]{geometry} + +\usepackage{float} +\usepackage{babel} +\usepackage{siunitx} +\usepackage{amsmath} +\usepackage{csquotes} +\usepackage{unicode-math} +\usepackage{fontspec} +\usepackage{tabularx} +\usepackage{booktabs} +\usepackage{needspace} +\usepackage{hyperref} +% \usepackage[backend=biber, +% bibencoding=utf8, +% style=apa +% ]{biblatex} + +\setmainfont{TeX Gyre Schola} +\setmathfont{TeX Gyre Schola Math} +\sisetup{ + group-separator = {.}, + output-decimal-marker = {,} +} + +\bigskipamount=7mm +\medskipamount=4mm +% \parindent=0mm +\parskip=1ex + +\def\tightlist{} + +\title{\doctitle} +\author{ + Bjorn Martens\\2187272 + \and + Joshua Regnier\\2183008 + \and + Loek Le Blansch\\2180996 + \and + Niels Stunnebrink\\2184532 +} + +\begin{document} +\begin{titlepage} +\maketitle +\thispagestyle{empty} +\end{titlepage} + +\tableofcontents +\newpage + +\input{\jobname.md.tex} +\end{document} + diff --git a/doc/dui.md b/doc/dui.md new file mode 100644 index 0000000..ede73c8 --- /dev/null +++ b/doc/dui.md @@ -0,0 +1,100 @@ +# Introduction + +# Problem statement + +The following is the original project description (translated to English): + +> I would like to bring to market a vehicle that can drive independently from A +> to B. The vehicle must take into account traffic rules, road signs, traffic +> lights, etc. Research is being conducted using a small cart, the Pololu Zumo +> 32U4, on which a camera module Nicla Vision is mounted. The aim is to +> investigate the most appropriate method of recognizing the road, traffic +> signs and traffic lights. This should be demonstrated with a proof of +> concept. The cart does not need to drive fast, so the image processing does +> not need to be very fast. Assume one frame per second (or faster). + +# Specifications + +# Architecture + +# Research + +## Communication between the Nicla and Zumo + +In order to make the Zumo robot both detect where it is on a road, and steer to +keep driving on said road, some sort of communication needs to exist between +the Nicla and Zumo. As mentioned earlier\footnote{dit is nog niet benoemd}, all +machine vision-related tasks will happen on the Nicla board. Because the Nicla +board is the first to know how much to steer the cart, it makes sense to have +it control the cart by giving the Nicla a 'steering wheel' of sorts. + +This section tries to answer the question "What is the best protocol to use +over the existing UART connection between the Nicla and Zumo?". After a +brainstorm session, we came up with the following specifications for the +communication protocol: + +1. **Low bandwidth** + Less data means more responsive steering +2. **As simple as possible** + The Nicla only needs to control speed and steering +3. **Easy to mock and test** + The cart should be able to be controlled using a mock driver and the Nicla's + output should be testable (preferably using unit tests) +4. **Adaptive to noisy data** + The cart should gradually change speed and steering direction as to not slip + or cause excessive motion blur for the camera module on the Nicla +5. **Adaptive to Nicla failure** + If the Nicla crashes or can't detect anything, it will stop sending control + commands. In this case, the Zumo robot should slowly come to a halt. + +Where possible, it's generally benificial to re-use existing code to save on +time. Existing code exists for a custom binary protocol and a text-based +command protocol. Both of these were designed without bandwidth or latency in +mind, and mostly focus on robustness in the case of temporary disconnects or +noise on the communication lines, so a new protocol needs to be made. + +To address specification 1 and 2, the command length is fixed at 1 byte. This +means that UARTs built-in start/stop bit will take care of message start/end +detection, since most software interfaces for UART (including Arduino) string +multiple sequential messages together even if they're not part of the same UART +packet. + +To mock messages from the Nicla to the Zumo robot, a simple USB serial to UART +cable can be used, along with a small C or Python program to convert +keyboard/mouse input into steering/speed commands. A small software layer can +be implemented on the Nicla to log the semantic meaning of the commands instead +of sending actual UART data when run in a unit test. + +A PID controller can be used to smoothly interpolate between input +speed/steering values. This would also introduce some lag between when the +Nicla knows how much to steer, and when the Zumo actually steered the wanted +amount. Smoothing the speed/steering values does make it virtually impossible +for the Nicla to make it's own input data unusable because of motion blur, so +the lag needs to be handled in some other way as directly controlling speed +values without interpolation would lead to a garbage-in-garbage-out system. The +simplest solution to motion blur is limiting the maximum speed the Zumo robot +can drive at, which is the solution we're going to use as speed is not one of +the criteria of the complete system\footnote{Problem statement +(\ref{problem-statement})}. + +In the case the Nicla module crashes or fails to detect the road or roadsigns, +it will stop sending commands. If the Zumo robot would naively continue at it's +current speed, it could drive itself into nearby walls, shoes, pets, etc. To +make sure the robot doesn't get 'lost', it needs to slow down once it hasn't +received commands for some time. As mentioned in section \ref{TODO}, the Nicla +module is able to process at about 10 frames per second, so 2 seconds is a +reasonable time-out period. + +\def\communicationConclusion{ +The complete protocol will consist of single byte commands. A byte can either +change the cart speed or steering direction, both will apply gradually. When no +commands have been received for more than 2 seconds, the Zumo robot will +gradually slow down until it is stopped. Exact specifications of commands are +provided in the protocol specification document\footnote{dit document bestaat +nog niet}. +} +\communicationConclusion + +# Conclusion + +\communicationConclusion diff --git a/doc/dui.tex b/doc/dui.tex new file mode 100644 index 0000000..612ee40 --- /dev/null +++ b/doc/dui.tex @@ -0,0 +1,3 @@ +\newcommand\doctitle{DUI} +\input{base.tex} + diff --git a/doc/makefile b/doc/makefile new file mode 100644 index 0000000..9f4dfa0 --- /dev/null +++ b/doc/makefile @@ -0,0 +1,20 @@ +.PHONY: all clean + +TARGET := $(patsubst %.md,%.pdf, $(wildcard *.md)) + +all: $(TARGET) + +garbage = $1.aux $1.bbl $1.bcf $1.blg $1.fdb_latexmk $1.fls $1.log $1.out $1.run.xml $1.synctex.gz $1.toc $1.md.tex + +%.pdf: %.svg + rsvg-convert -f pdf -o $@ $< + +%.pdf: %.tex base.tex %.md.tex + latexmk $< -shell-escape -halt-on-error -lualatex -f -g + +%.md.tex: %.md + pandoc -t latex -o $@ $< + +clean: + $(RM) $(call garbage,research) research.pdf + 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); +} |