From 995d50998c42efeaef0095769178ac061e25cf3c Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 5 May 2023 18:23:01 +0200 Subject: try different compiler flags --- zumo/makefile | 43 +++++++++++++++++++++++++------- zumo/zumo.mk | 78 ++++++++++++++++++++++++++++++++--------------------------- 2 files changed, 77 insertions(+), 44 deletions(-) diff --git a/zumo/makefile b/zumo/makefile index c388a28..08c01d7 100644 --- a/zumo/makefile +++ b/zumo/makefile @@ -2,33 +2,58 @@ PORT = /dev/ttyUSB0 C++ = avr-g++ CC = avr-gcc -LD = avr-ld +LD = avr-gcc RM = rm -f MCU = atmega32u4 TARGET = main SRCS += main.cpp -CFLAGS += -mcall-prologues -CFLAGS += -mmcu=$(MCU) -CFLAGS += -Os -CFLAGS += -g -CFLAGS += -Wl,-gc-sections -CFLAGS += -Wl,-relax +C_CPP_FLAGS += -mmcu=$(MCU) +C_CPP_FLAGS += -Wl,-gc-sections +SHARED_FLAGS += -Os + +C_CPP_FLAGS += -mcall-prologues +C_CPP_FLAGS += -g +C_CPP_FLAGS += -Wl,-relax +C_CPP_FLAGS += -MMD +C_CPP_FLAGS += -ffunction-sections +C_CPP_FLAGS += -fdata-sections + +LFLAGS += -flto +LFLAGS += -fuse-linker-plugin +LFLAGS += -lc -lm +# LFLAGS += -L/usr/avr/lib/avr5/ -L/usr/lib/gcc/avr/12.2.0/avr5 -lgcc -lm -lc -latmega32u4 +# LFLAGS += -flto -fuse-linker-plugin -mmcu=atmega32u4 -Wl,--gc-sections -Os -lc -lm + + +CFLAGS += -fno-fat-lto-objects +# CFLAGS += -std=gnu11 + +CPPFLAGS += -fpermissive +CPPFLAGS += -fno-exceptions +CPPFLAGS += -fno-threadsafe-statics +CPPFLAGS += -fno-devirtualize +# CPPFLAGS += -std=gnu++11 + + +LFLAGS += $(SHARED_FLAGS) +CFLAGS += $(C_CPP_FLAGS) $(SHARED_FLAGS) +CPPFLAGS += $(C_CPP_FLAGS) $(SHARED_FLAGS) include zumo.mk OBJS := $(patsubst %.c,%.o, $(SRCS)) OBJS := $(patsubst %.cpp,%.o, $(OBJS)) -MAKEFLAGS += -j4 +MAKEFLAGS += -j .PHONY: all clean flash all: $(TARGET).hex %.o: %.cpp - $(C++) $(CFLAGS) -o $@ -c $< + $(C++) $(CPPFLAGS) -o $@ -c $< %.o: %.c $(CC) $(CFLAGS) -o $@ -c $< diff --git a/zumo/zumo.mk b/zumo/zumo.mk index 3645513..f5ab84b 100644 --- a/zumo/zumo.mk +++ b/zumo/zumo.mk @@ -1,31 +1,39 @@ -CFLAGS += -DF_CPU=16000000 -CFLAGS += -I./lib/zumo-32u4-arduino-library/src -CFLAGS += -I./lib/fastgpio-arduino -CFLAGS += -I./lib/pushbutton-arduino -CFLAGS += -I./lib/usb-pause-arduino -CFLAGS += -I./lib/pololu-buzzer-arduino/src -CFLAGS += -I./lib/pololu-hd44780-arduino -CFLAGS += -I./lib/pololu-menu-arduino/src -CFLAGS += -I./lib/pololu-oled-arduino/src -CFLAGS += -I./lib/ArduinoCore-avr/cores/arduino -CFLAGS += -I./lib/ArduinoCore-avr/libraries/HID/src -CFLAGS += -I./lib/ArduinoCore-avr/libraries/SoftwareSerial/src -CFLAGS += -I./lib/ArduinoCore-avr/libraries/SPI/src -CFLAGS += -I./lib/ArduinoCore-avr/libraries/EEPROM/src -CFLAGS += -I./lib/ArduinoCore-avr/libraries/Wire/src -CFLAGS += -I./lib/ArduinoCore-avr/variants/circuitplay32u4 +C_CPP_FLAGS += -DF_CPU=16000000L +C_CPP_FLAGS += -D__PROG_TYPES_COMPAT__ +C_CPP_FLAGS += -DARDUINO=1819 +C_CPP_FLAGS += -DARDUINO_AVR_LEONARDO +C_CPP_FLAGS += -DARDUINO_ARCH_AVR +C_CPP_FLAGS += -DARDUINO_BOARD='"AVR_LEONARDO"' +C_CPP_FLAGS += -DARDUINO_VARIANT='"leonardo"' +C_CPP_FLAGS += -DUSB_VID=0x2341 +C_CPP_FLAGS += -DUSB_PID=0x8036 +C_CPP_FLAGS += -DUSB_PRODUCT='"Arduino Leonardo"' +C_CPP_FLAGS += -DUSB_MANUFACTURER='"Unknown"' -CFLAGS += -L/usr/avr/lib/avr5/ -L/usr/lib/gcc/avr/12.2.0/avr5 -lgcc -lm -lc -latmega32u4 -LFLAGS += -L/usr/avr/lib/avr5/ -L/usr/lib/gcc/avr/12.2.0/avr5 -lgcc -lm -lc -latmega32u4 +C_CPP_FLAGS += -I./lib/zumo-32u4-arduino-library/src +C_CPP_FLAGS += -I./lib/fastgpio-arduino +C_CPP_FLAGS += -I./lib/pushbutton-arduino +C_CPP_FLAGS += -I./lib/usb-pause-arduino +C_CPP_FLAGS += -I./lib/pololu-buzzer-arduino/src +C_CPP_FLAGS += -I./lib/pololu-hd44780-arduino +C_CPP_FLAGS += -I./lib/pololu-menu-arduino/src +C_CPP_FLAGS += -I./lib/pololu-oled-arduino/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/cores/arduino +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/libraries/HID/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/libraries/SoftwareSerial/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/libraries/SPI/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/libraries/EEPROM/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/libraries/Wire/src +C_CPP_FLAGS += -I./lib/ArduinoCore-avr/variants/leonardo -LIBS += lib/ArduinoCore-avr/cores/arduino/PluggableUSB.cpp +# LIBS += lib/ArduinoCore-avr/cores/arduino/PluggableUSB.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/CDC.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/USBCore.cpp -# LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial0.cpp -# LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial1.cpp -# LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial2.cpp -# LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial3.cpp +LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial0.cpp +LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial1.cpp +LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial2.cpp +LIBS += lib/ArduinoCore-avr/cores/arduino/HardwareSerial3.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/IPAddress.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/Print.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/Stream.cpp @@ -34,10 +42,20 @@ LIBS += lib/ArduinoCore-avr/cores/arduino/WString.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/abi.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/main.cpp LIBS += lib/ArduinoCore-avr/cores/arduino/new.cpp -LIBS += lib/ArduinoCore-avr/libraries/HID/src/HID.cpp -LIBS += lib/ArduinoCore-avr/libraries/SPI/src/SPI.cpp +# LIBS += lib/ArduinoCore-avr/libraries/HID/src/HID.cpp +# LIBS += lib/ArduinoCore-avr/libraries/SPI/src/SPI.cpp # LIBS += lib/ArduinoCore-avr/libraries/SoftwareSerial/src/SoftwareSerial.cpp LIBS += lib/ArduinoCore-avr/libraries/Wire/src/Wire.cpp +LIBS += lib/ArduinoCore-avr/cores/arduino/WInterrupts.c +LIBS += lib/ArduinoCore-avr/cores/arduino/hooks.c +LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_shift.c +LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_digital.c +LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_analog.c +LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_pulse.c +LIBS += lib/ArduinoCore-avr/cores/arduino/wiring.c +LIBS += lib/ArduinoCore-avr/cores/arduino/Tone.cpp +# LIBS += lib/ArduinoCore-avr/libraries/Wire/src/utility/twi.c + LIBS += lib/pololu-buzzer-arduino/src/PololuBuzzer.cpp LIBS += lib/pushbutton-arduino/Pushbutton.cpp LIBS += lib/zumo-32u4-arduino-library/src/QTRSensors.cpp @@ -50,16 +68,6 @@ LIBS += lib/usb-pause-arduino/USBPause.cpp LIBS += lib/pololu-hd44780-arduino/PololuHD44780.cpp LIBS += lib/fastgpio-arduino/FastGPIO.cpp LIBS += lib/pololu-oled-arduino/src/font.cpp -# LIBS += lib/ArduinoCore-avr/cores/arduino/Tone.cpp -# LIBS += lib/ArduinoCore-avr/libraries/Wire/src/utility/twi.c - -LIBS += lib/ArduinoCore-avr/cores/arduino/WInterrupts.c -LIBS += lib/ArduinoCore-avr/cores/arduino/hooks.c -LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_shift.c -LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_digital.c -LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_analog.c -# LIBS += lib/ArduinoCore-avr/cores/arduino/wiring_pulse.c -LIBS += lib/ArduinoCore-avr/cores/arduino/wiring.c SRCS += $(LIBS) -- cgit v1.2.3 From 2f8d3ac42bb0b5bf644af27b59ee8b02dbb50397 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 10 May 2023 13:24:46 +0200 Subject: add docs folder with pandoc/latex document template --- doc/.gitignore | 17 +++++++++++++++++ doc/base.tex | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ doc/makefile | 20 ++++++++++++++++++++ doc/research.md | 4 ++++ doc/research.tex | 3 +++ 5 files changed, 98 insertions(+) create mode 100644 doc/.gitignore create mode 100644 doc/base.tex create mode 100644 doc/makefile create mode 100644 doc/research.md create mode 100644 doc/research.tex 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..ecc3c8d --- /dev/null +++ b/doc/base.tex @@ -0,0 +1,54 @@ +\documentclass[11pt, a4paper, english]{article} +\usepackage[margin=1in]{geometry} + +\usepackage{float} +\usepackage{babel} +\usepackage{siunitx} +\usepackage{amsmath} +\usepackage{csquotes} +\usepackage{parskip} +\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 + +\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/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/doc/research.md b/doc/research.md new file mode 100644 index 0000000..1a616f9 --- /dev/null +++ b/doc/research.md @@ -0,0 +1,4 @@ +# test 1 + +this is a test document in markdown that will get converted into latex + diff --git a/doc/research.tex b/doc/research.tex new file mode 100644 index 0000000..af15484 --- /dev/null +++ b/doc/research.tex @@ -0,0 +1,3 @@ +\newcommand\doctitle{Research document} +\input{base.tex} + -- cgit v1.2.3 From 7b76b2c73d53a2ed31e3c887926f44dd02de44d2 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 10 May 2023 15:20:35 +0200 Subject: added research --- doc/base.tex | 6 ++-- doc/research.md | 94 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 96 insertions(+), 4 deletions(-) diff --git a/doc/base.tex b/doc/base.tex index ecc3c8d..9c1c908 100644 --- a/doc/base.tex +++ b/doc/base.tex @@ -6,7 +6,6 @@ \usepackage{siunitx} \usepackage{amsmath} \usepackage{csquotes} -\usepackage{parskip} \usepackage{unicode-math} \usepackage{fontspec} \usepackage{tabularx} @@ -27,7 +26,10 @@ \bigskipamount=7mm \medskipamount=4mm -\parindent=0mm +% \parindent=0mm +\parskip=1ex + +\def\tightlist{} \title{\doctitle} \author{ diff --git a/doc/research.md b/doc/research.md index 1a616f9..ba697a8 100644 --- a/doc/research.md +++ b/doc/research.md @@ -1,4 +1,94 @@ -# test 1 +# Problem statement -this is a test document in markdown that will get converted into latex +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). + +# TBD: The big question + +## 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 -- cgit v1.2.3 From 0a59a63fc5f4b6bcb50cdf97902c555b2d54e83d Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 11 May 2023 10:29:01 +0200 Subject: rename document and add required sections --- doc/dui.md | 100 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ doc/dui.tex | 3 ++ doc/research.md | 94 --------------------------------------------------- doc/research.tex | 3 -- 4 files changed, 103 insertions(+), 97 deletions(-) create mode 100644 doc/dui.md create mode 100644 doc/dui.tex delete mode 100644 doc/research.md delete mode 100644 doc/research.tex 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/research.md b/doc/research.md deleted file mode 100644 index ba697a8..0000000 --- a/doc/research.md +++ /dev/null @@ -1,94 +0,0 @@ -# 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). - -# TBD: The big question - -## 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/research.tex b/doc/research.tex deleted file mode 100644 index af15484..0000000 --- a/doc/research.tex +++ /dev/null @@ -1,3 +0,0 @@ -\newcommand\doctitle{Research document} -\input{base.tex} - -- cgit v1.2.3 -- cgit v1.2.3 From 4ab9fb8c71d4f519b62d63a2fcd0d5647bc80539 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 11 May 2023 12:58:16 +0200 Subject: add editorconfig --- .editorconfig | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .editorconfig 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 -- cgit v1.2.3 -- cgit v1.2.3 -- cgit v1.2.3 From 6472b77eb6582e5133a28f61a448eb6a00cd25b6 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 14 May 2023 14:05:03 +0200 Subject: initial robot driver code --- zumo/main.cpp | 13 ------------- zumo/protocol.cpp | 37 +++++++++++++++++++++++++++++++++++++ zumo/protocol.h | 35 +++++++++++++++++++++++++++++++++++ zumo/zumo.ino | 31 +++++++++++++++++++++++++++++++ 4 files changed, 103 insertions(+), 13 deletions(-) delete mode 100644 zumo/main.cpp create mode 100644 zumo/protocol.cpp create mode 100644 zumo/protocol.h create mode 100644 zumo/zumo.ino 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 -#include -#include - -void setup() { -} - -void loop() { - ledRed(1); - delay(1000); - ledRed(0); - delay(1000); -} diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp new file mode 100644 index 0000000..a0c72f0 --- /dev/null +++ b/zumo/protocol.cpp @@ -0,0 +1,37 @@ +#include + +#include "protocol.h" + +#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) { + const float MAX_MOTOR_DIFF = 0.6f; // 0 to 1 + float motor_l = 0.5f * state->speed * (+1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod; + float motor_r = 0.5f * state->speed * (-1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->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..ed63f6e --- /dev/null +++ b/zumo/zumo.ino @@ -0,0 +1,31 @@ +#include +#include +#include + +#include "protocol.h" + +dui_state_t g_dui_target_state = { + .steer = 1.0f, + .speed = 1.0f, + .current_sign = DUI_SIGN_NONE, + .speed_mod =96.0, +}; +dui_state_t g_dui_current_state = { + .steer = 0, + .speed = 0, + .current_sign = DUI_SIGN_NONE, + .speed_mod = 1.0, +}; + +void setup() { +} + +void loop() { + unsigned char cmd = 0; + while ((cmd = uart_read())) + handle_cmd(cmd, &g_dui_target_state); + + //TODO: PID controllers + sign handlers + + apply_state(&g_dui_target_state); +} -- cgit v1.2.3 From e3c22d70487985a6dcb7a5866c8baeecbbaa4103 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 14 May 2023 15:07:21 +0200 Subject: add PID controller --- zumo/pid.cpp | 36 ++++++++++++++++++++++++++++++++++++ zumo/pid.h | 7 +++++++ zumo/protocol.cpp | 8 +++++--- zumo/zumo.ino | 17 ++++++++++------- 4 files changed, 58 insertions(+), 10 deletions(-) create mode 100644 zumo/pid.cpp create mode 100644 zumo/pid.h diff --git a/zumo/pid.cpp b/zumo/pid.cpp new file mode 100644 index 0000000..594d136 --- /dev/null +++ b/zumo/pid.cpp @@ -0,0 +1,36 @@ +#include "pid.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) { + A0 = P + I*dt + D/dt; + A1 = -P - 2*D/dt; + A2 = D/dt; + } + + // https://en.wikipedia.org/wiki/PID_controller#Pseudocode + float iter(float current, float target) { + error[2] = error[1]; + error[1] = error[0]; + error[0] = target - current; + output = output + A0 * error[0] + A1 * error[1] + A2 * error[2]; + return output; + } +}; + +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(current->speed, target->speed); + current->steer = steer_pid.iter(current->steer, 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..2cdbc91 --- /dev/null +++ b/zumo/pid.h @@ -0,0 +1,7 @@ +#pragma once + +#include "protocol.h" + +/** @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/protocol.cpp b/zumo/protocol.cpp index a0c72f0..fea8a00 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -2,6 +2,9 @@ #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 @@ -22,9 +25,8 @@ void handle_cmd(unsigned char cmd, dui_state_t *state) { } void apply_state(dui_state_t *state) { - const float MAX_MOTOR_DIFF = 0.6f; // 0 to 1 - float motor_l = 0.5f * state->speed * (+1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod; - float motor_r = 0.5f * state->speed * (-1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->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; Zumo32U4Motors::setLeftSpeed((int16_t) motor_l); Zumo32U4Motors::setRightSpeed((int16_t) motor_r); diff --git a/zumo/zumo.ino b/zumo/zumo.ino index ed63f6e..f59bd36 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -3,29 +3,32 @@ #include #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 =96.0, + .speed_mod = 1.f, }; dui_state_t g_dui_current_state = { - .steer = 0, - .speed = 0, + .steer = 0.f, + .speed = 1.f, .current_sign = DUI_SIGN_NONE, - .speed_mod = 1.0, + .speed_mod = 1.f, }; void setup() { } void loop() { - unsigned char cmd = 0; + unsigned char cmd = 0x00; while ((cmd = uart_read())) handle_cmd(cmd, &g_dui_target_state); - //TODO: PID controllers + sign handlers + apply_pid(&g_dui_target_state, &g_dui_current_state); - apply_state(&g_dui_target_state); + apply_state(&g_dui_current_state); + + delay(10); } -- cgit v1.2.3 From ff7a914055cf851c994ee037342a331902f84a0c Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 13:34:31 +0200 Subject: add pidtest --- zumo/.gitignore | 1 + zumo/pid.cpp | 44 +++++++++++++++++++++----------------------- zumo/pid.h | 13 +++++++++++++ zumo/pidtest.cpp | 35 +++++++++++++++++++++++++++++++++++ zumo/pidtest.mk | 21 +++++++++++++++++++++ 5 files changed, 91 insertions(+), 23 deletions(-) create mode 100644 zumo/pidtest.cpp create mode 100644 zumo/pidtest.mk 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/pid.cpp b/zumo/pid.cpp index 594d136..9bef08f 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -1,35 +1,33 @@ #include "pid.h" -class PID { -private: - float A0, A1, A2; - float error[3] = {0}; - float dt = 0.010; - float output = 0; +PID::PID(float P, float I, float D) { + A0 = P + I*dt + D/dt; + A1 = -P - 2*D/dt; + A2 = D/dt; +} -public: - 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; +} - // https://en.wikipedia.org/wiki/PID_controller#Pseudocode - float iter(float current, float target) { - error[2] = error[1]; - error[1] = error[0]; - error[0] = target - current; - 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(current->speed, target->speed); - current->steer = steer_pid.iter(current->steer, target->steer); + 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 index 2cdbc91..fbcd063 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -2,6 +2,19 @@ #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 +#include + +#include "pid.h" + +std::random_device rd; +std::mt19937 rng(rd()); +std::uniform_real_distribution 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) + -- cgit v1.2.3 From 473ab3fd794d0baddd9a6a29ab25cea3d2217fc8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 14:46:06 +0200 Subject: pid tuned --- zumo/pid.cpp | 27 +++++++++++++++++++++++---- zumo/pid.h | 9 +++++---- zumo/pidtest.cpp | 35 +++++++++-------------------------- 3 files changed, 37 insertions(+), 34 deletions(-) diff --git a/zumo/pid.cpp b/zumo/pid.cpp index 9bef08f..fabe1da 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -1,9 +1,14 @@ #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; + A0 = P + I * dt; + A1 = -P; + A0d = D / dt; + A1d = -2 * D / dt; + A2d = D / dt; + tau = D / (P * N); + alpha = dt / (2 * tau); + reset(0.0); } // https://en.wikipedia.org/wiki/PID_controller#Pseudocode @@ -11,7 +16,17 @@ 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]; + + output = output + A0 * error[0] + A1 * error[1]; + + d1 = d0; + d0 = A0d * error[0] + A1d * error[1] + A2d * error[2]; + fd1 = fd0; + fd0 = ((alpha) / (alpha + 1)) * (d0 + d1) - ((alpha - 1) / (alpha + 1)) * fd1; + output = output + fd0; + + if (output < -1) output = -1; + if (output > 1) output = 1; return output; } @@ -19,6 +34,10 @@ void PID::reset(float value) { error[0] = 0.0; error[1] = 0.0; error[2] = 0.0; + d0 = 0; + d1 = 0; + fd0 = 0; + fd1 = 0; output = value; } diff --git a/zumo/pid.h b/zumo/pid.h index fbcd063..ef733d6 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -4,10 +4,11 @@ class PID { private: - float A0, A1, A2; - float error[3] = {0}; - float dt = 0.010; - float output = 0; + float A0, A1, A0d, A1d, A2d, tau, alpha, d0, d1, fd0, fd1; + float error[3]; + float output; + const float dt = 1.0; + const float N = 10.0; public: PID(float P, float I, float D); diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp index f5198bb..b9ce50b 100644 --- a/zumo/pidtest.cpp +++ b/zumo/pidtest.cpp @@ -3,33 +3,16 @@ #include "pid.h" -std::random_device rd; -std::mt19937 rng(rd()); -std::uniform_real_distribution 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); + P = -0.02; + I = 0.13; + D = -300; + 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); + fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D); + for (unsigned int i = 0; i < 100; i++) { + printf("%2.8f\n", test.iter(i < 50 ? 1.0 : 0.0)); + } } -- cgit v1.2.3 From ba026d8229744a01818d38552ec7271e689d19eb Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 14:50:22 +0200 Subject: update pid in arduino sketch --- zumo/pid.cpp | 9 ++++----- zumo/pid.h | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/zumo/pid.cpp b/zumo/pid.cpp index fabe1da..7c1cbda 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -41,13 +41,12 @@ void PID::reset(float value) { 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); +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->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; + current->speed_mod = speed_mod_pid.iter(target->speed_mod); } diff --git a/zumo/pid.h b/zumo/pid.h index ef733d6..beb73ac 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -11,7 +11,7 @@ private: const float N = 10.0; public: - PID(float P, float I, float D); + PID(float P = -0.02, float I = 0.13, float D = -300.0); float iter(float target); void reset(float value); }; -- cgit v1.2.3