Merge branch 'lonkaars:master' into master

17 files changed, 505 insertions, 57 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/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/pid.cpp b/zumo/pid.cpp
new file mode 100644
index 0000000..7c1cbda
--- /dev/null
+++ b/zumo/pid.cpp
@@ -0,0 +1,52 @@
+#include "pid.h"
+
+PID::PID(float P, float I, float D) {
+	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
+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];
+
+	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;
+}
+
+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;
+}
+
+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(target->speed_mod);
+}
+
diff --git a/zumo/pid.h b/zumo/pid.h
new file mode 100644
index 0000000..beb73ac
--- /dev/null
+++ b/zumo/pid.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "protocol.h"
+
+class PID {
+private:
+	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 = -0.02, float I = 0.13, float D = -300.0);
+	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..b9ce50b
--- /dev/null
+++ b/zumo/pidtest.cpp
@@ -0,0 +1,18 @@
+#include <cstdio>
+#include <random>
+
+#include "pid.h"
+
+int main() {
+	float P, I, D;
+	P = -0.02;
+	I = 0.13;
+	D = -300;
+	PID test(P, I, D);
+	test.reset(0.0);
+
+	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));
+	}
+}
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);
+}
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)