Merged i2c-helper in Main

stone-dispencer/.devcontainer/Dockerfile

@@ -0,0 +1,47 @@
+FROM espressif/idf
+
+ARG DEBIAN_FRONTEND=nointeractive
+ARG CONTAINER_USER=esp
+ARG USER_UID=1050
+ARG USER_GID=$USER_UID
+
+RUN apt-get update \
+  && apt install -y -q \
+  cmake \
+  git \
+  libglib2.0-0 \
+  libnuma1 \
+  libpixman-1-0 \
+  && rm -rf /var/lib/apt/lists/*
+
+# QEMU
+ENV QEMU_REL=esp_develop_8.2.0_20240122
+ENV QEMU_SHA256=e7c72ef5705ad1444d391711088c8717fc89f42e9bf6d1487f9c2a326b8cfa83
+ENV QEMU_DIST=qemu-xtensa-softmmu-${QEMU_REL}-x86_64-linux-gnu.tar.xz
+ENV QEMU_URL=https://github.com/espressif/qemu/releases/download/esp-develop-8.2.0-20240122/${QEMU_DIST}
+
+ENV LC_ALL=C.UTF-8
+ENV LANG=C.UTF-8
+
+RUN wget --no-verbose ${QEMU_URL} \
+  && echo "${QEMU_SHA256} *${QEMU_DIST}" | sha256sum --check --strict - \
+  && tar -xf $QEMU_DIST -C /opt \
+  && rm ${QEMU_DIST}
+
+ENV PATH=/opt/qemu/bin:${PATH}
+
+RUN groupadd --gid $USER_GID $CONTAINER_USER \
+  && adduser --uid $USER_UID --gid $USER_GID --disabled-password --gecos "" ${CONTAINER_USER} \
+  && usermod -a -G root $CONTAINER_USER && usermod -a -G dialout $CONTAINER_USER
+
+RUN chmod -R 775 /opt/esp/python_env/
+
+USER ${CONTAINER_USER}
+ENV USER=${CONTAINER_USER}
+WORKDIR /home/${CONTAINER_USER}
+
+RUN echo "source /opt/esp/idf/export.sh > /dev/null 2>&1" >> ~/.bashrc
+
+ENTRYPOINT [ "/opt/esp/entrypoint.sh" ]
+
+CMD ["/bin/bash", "-c"]

stone-dispencer/.devcontainer/devcontainer.json

@@ -0,0 +1,36 @@
+{
+	"name": "ESP-IDF QEMU",
+	"build": {
+		"dockerfile": "Dockerfile"
+	},
+	"customizations": {
+		"vscode": {
+			"settings": {
+				"terminal.integrated.defaultProfile.linux": "bash",
+				"idf.espIdfPath": "/opt/esp/idf",
+				"idf.customExtraPaths": "",
+				"idf.pythonBinPath": "/opt/esp/python_env/idf5.4_py3.12_env/bin/python",
+				"idf.toolsPath": "/opt/esp",
+				"idf.gitPath": "/usr/bin/git"
+			},
+			"extensions": [
+				"espressif.esp-idf-extension"
+			]
+		},
+		"codespaces": {
+			"settings": {
+				"terminal.integrated.defaultProfile.linux": "bash",
+				"idf.espIdfPath": "/opt/esp/idf",
+				"idf.customExtraPaths": "",
+				"idf.pythonBinPath": "/opt/esp/python_env/idf5.4_py3.12_env/bin/python",
+				"idf.toolsPath": "/opt/esp",
+				"idf.gitPath": "/usr/bin/git"
+			},
+			"extensions": [
+				"espressif.esp-idf-extension",
+				"espressif.esp-idf-web"
+			]
+		}
+	},
+	"runArgs": ["--privileged"]
+}

stone-dispencer/.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+    "configurations": [
+        {
+            "name": "ESP-IDF",
+            "compilerPath": "${config:idf.toolsPath}/tools/xtensa-esp-elf/esp-13.2.0_20240530/xtensa-esp-elf/bin/xtensa-esp32-elf-gcc",
+            "compileCommands": "${config:idf.buildPath}/compile_commands.json",
+            "includePath": [
+                "${config:idf.espIdfPath}/components/**",
+                "${config:idf.espIdfPathWin}/components/**",
+                "${workspaceFolder}/**"
+            ],
+            "browse": {
+                "path": [
+                    "${config:idf.espIdfPath}/components",
+                    "${config:idf.espIdfPathWin}/components",
+                    "${workspaceFolder}"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        }
+    ],
+    "version": 4
+}

stone-dispencer/.vscode/launch.json

@@ -0,0 +1,15 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "type": "gdbtarget",
+      "request": "attach",
+      "name": "Eclipse CDT GDB Adapter"
+    },
+    {
+      "type": "espidf",
+      "name": "Launch",
+      "request": "launch"
+    }
+  ]
+}

stone-dispencer/.vscode/settings.json

@@ -0,0 +1,38 @@
+{
+    "C_Cpp.intelliSenseEngine": "default",
+    "idf.adapterTargetName": "esp32",
+    "idf.customExtraPaths": "/home/alexander/.espressif/tools/xtensa-esp-elf-gdb/14.2_20240403/xtensa-esp-elf-gdb/bin:/home/alexander/.espressif/tools/riscv32-esp-elf-gdb/14.2_20240403/riscv32-esp-elf-gdb/bin:/home/alexander/.espressif/tools/xtensa-esp-elf/esp-13.2.0_20240530/xtensa-esp-elf/bin:/home/alexander/.espressif/tools/riscv32-esp-elf/esp-13.2.0_20240530/riscv32-esp-elf/bin:/home/alexander/.espressif/tools/esp32ulp-elf/2.38_20240113/esp32ulp-elf/bin:/home/alexander/.espressif/tools/openocd-esp32/v0.12.0-esp32-20240318/openocd-esp32/bin:/home/alexander/.espressif/tools/esp-rom-elfs/20240305",
+    "idf.customExtraVars": {
+        "OPENOCD_SCRIPTS": "/home/alexander/.espressif/tools/openocd-esp32/v0.12.0-esp32-20240318/openocd-esp32/share/openocd/scripts",
+        "ESP_ROM_ELF_DIR": "/home/alexander/.espressif/tools/esp-rom-elfs/20240305/"
+    },
+    "idf.espIdfPath": "/opt/esp/v5.3.2/esp-idf",
+    "idf.openOcdConfigs": [
+        "board/esp32-wrover-kit-3.3v.cfg"
+    ],
+    "idf.port": "/dev/ttyUSB1",
+    "idf.toolsPath": "/home/alexander/.espressif",
+    "idf.flashType": "JTAG",
+    "idf.openOcdLaunchArgs": [
+        "-c",
+        "adapter speed 1000"
+    ],
+    "files.associations": {
+        "*.tcc": "c",
+        "charconv": "c",
+        "chrono": "c",
+        "optional": "c",
+        "format": "c",
+        "ratio": "c",
+        "system_error": "c",
+        "array": "c",
+        "functional": "c",
+        "regex": "c",
+        "tuple": "c",
+        "type_traits": "c",
+        "utility": "c",
+        "esp_chip_info.h": "c",
+        "random": "cpp",
+        "future": "cpp"
+    },
+}

stone-dispencer/.vscode/tasks.json

@@ -0,0 +1,259 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "Build - Build project",
+      "type": "shell",
+      "command": "${config:idf.pythonBinPath} ${config:idf.espIdfPath}/tools/idf.py build",
+      "windows": {
+        "command": "${config:idf.pythonBinPathWin} ${config:idf.espIdfPathWin}\\tools\\idf.py build",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}"
+          }
+        }
+      },
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}"
+        }
+      },
+      "problemMatcher": [
+        {
+          "owner": "cpp",
+          "fileLocation": [
+            "autoDetect",
+            "${workspaceFolder}"
+          ],
+          "pattern": {
+            "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+            "file": 1,
+            "line": 2,
+            "column": 3,
+            "severity": 4,
+            "message": 5
+          }
+        }
+      ],
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      }
+    },
+    {
+      "label": "Set ESP-IDF Target",
+      "type": "shell",
+      "command": "${command:espIdf.setTarget}",
+      "problemMatcher": {
+        "owner": "cpp",
+        "fileLocation": [
+          "autoDetect",
+          "${workspaceFolder}"
+        ],
+        "pattern": {
+          "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+          "file": 1,
+          "line": 2,
+          "column": 3,
+          "severity": 4,
+          "message": 5
+        }
+      }
+    },
+    {
+      "label": "Clean - Clean the project",
+      "type": "shell",
+      "command": "${config:idf.pythonBinPath} ${config:idf.espIdfPath}/tools/idf.py fullclean",
+      "windows": {
+        "command": "${config:idf.pythonBinPathWin} ${config:idf.espIdfPathWin}\\tools\\idf.py fullclean",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}"
+          }
+        }
+      },
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}"
+        }
+      },
+      "problemMatcher": [
+        {
+          "owner": "cpp",
+          "fileLocation": [
+            "autoDetect",
+            "${workspaceFolder}"
+          ],
+          "pattern": {
+            "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+            "file": 1,
+            "line": 2,
+            "column": 3,
+            "severity": 4,
+            "message": 5
+          }
+        }
+      ]
+    },
+    {
+      "label": "Flash - Flash the device",
+      "type": "shell",
+      "command": "${config:idf.pythonBinPath} ${config:idf.espIdfPath}/tools/idf.py -p ${config:idf.port} -b ${config:idf.flashBaudRate} flash",
+      "windows": {
+        "command": "${config:idf.pythonBinPathWin} ${config:idf.espIdfPathWin}\\tools\\idf.py flash -p ${config:idf.portWin} -b ${config:idf.flashBaudRate}",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}"
+          }
+        }
+      },
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}"
+        }
+      },
+      "problemMatcher": [
+        {
+          "owner": "cpp",
+          "fileLocation": [
+            "autoDetect",
+            "${workspaceFolder}"
+          ],
+          "pattern": {
+            "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+            "file": 1,
+            "line": 2,
+            "column": 3,
+            "severity": 4,
+            "message": 5
+          }
+        }
+      ]
+    },
+    {
+      "label": "Monitor: Start the monitor",
+      "type": "shell",
+      "command": "${config:idf.pythonBinPath} ${config:idf.espIdfPath}/tools/idf.py -p ${config:idf.port} monitor",
+      "windows": {
+        "command": "${config:idf.pythonBinPathWin} ${config:idf.espIdfPathWin}\\tools\\idf.py -p ${config:idf.portWin} monitor",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}"
+          }
+        }
+      },
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}"
+        }
+      },
+      "problemMatcher": [
+        {
+          "owner": "cpp",
+          "fileLocation": [
+            "autoDetect",
+            "${workspaceFolder}"
+          ],
+          "pattern": {
+            "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+            "file": 1,
+            "line": 2,
+            "column": 3,
+            "severity": 4,
+            "message": 5
+          }
+        }
+      ],
+      "dependsOn": "Flash - Flash the device"
+    },
+    {
+      "label": "OpenOCD: Start openOCD",
+      "type": "shell",
+      "presentation": {
+        "echo": true,
+        "reveal": "never",
+        "focus": false,
+        "panel": "new"
+      },
+      "command": "openocd -s ${command:espIdf.getOpenOcdScriptValue} ${command:espIdf.getOpenOcdConfigs}",
+      "windows": {
+        "command": "openocd.exe -s ${command:espIdf.getOpenOcdScriptValue} ${command:espIdf.getOpenOcdConfigs}",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}"
+          }
+        }
+      },
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}"
+        }
+      },
+      "problemMatcher": {
+        "owner": "cpp",
+        "fileLocation": [
+          "autoDetect",
+          "${workspaceFolder}"
+        ],
+        "pattern": {
+          "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$",
+          "file": 1,
+          "line": 2,
+          "column": 3,
+          "severity": 4,
+          "message": 5
+        }
+      }
+    },
+    {
+      "label": "adapter",
+      "type": "shell",
+      "command": "${config:idf.pythonBinPath}",
+      "isBackground": true,
+      "options": {
+        "env": {
+          "PATH": "${env:PATH}:${config:idf.customExtraPaths}",
+          "PYTHONPATH": "${command:espIdf.getExtensionPath}/esp_debug_adapter/debug_adapter"
+        }
+      },
+      "problemMatcher": {
+        "background": {
+          "beginsPattern": "\bDEBUG_ADAPTER_STARTED\b",
+          "endsPattern": "DEBUG_ADAPTER_READY2CONNECT",
+          "activeOnStart": true
+        },
+        "pattern": {
+          "regexp": "(\\d+)-(\\d+)-(\\d+)\\s(\\d+):(\\d+):(\\d+),(\\d+)\\s-(.+)\\s(ERROR)",
+          "file": 8,
+          "line": 2,
+          "column": 3,
+          "severity": 4,
+          "message": 9
+        }
+      },
+      "args": [
+        "${command:espIdf.getExtensionPath}/esp_debug_adapter/debug_adapter_main.py",
+        "-e",
+        "${workspaceFolder}/build/${command:espIdf.getProjectName}.elf",
+        "-s",
+        "$OPENOCD_SCRIPTS",
+        "-dn",
+        "esp32",
+        "-om",
+        "connect_to_instance",
+        "-t",
+        "xtensa-esp32-elf-"
+
+      ],
+      "windows": {
+        "command": "${config:idf.pythonBinPathWin}",
+        "options": {
+          "env": {
+            "PATH": "${env:PATH};${config:idf.customExtraPaths}",
+            "PYTHONPATH": "${command:espIdf.getExtensionPath}/esp_debug_adapter/debug_adapter"
+          }
+        }
+      }
+    }
+  ]
+}

stone-dispencer/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(stone-dispencer)

stone-dispencer/circuit/stone-dispencer/sym-lib-table

@@ -0,0 +1,4 @@
+(sym_lib_table
+  (version 7)
+  (lib (name "ESP32-DEVKIT-V1")(type "KiCad")(uri "/home/alexander/Projects/gobot/can-interface/circuit/gobot-can-interface/external/ESP32-DEVKIT-V1/ESP32-DEVKIT-V1.kicad_sym")(options "")(descr ""))
+)

stone-dispencer/components/gobot_rpc

@@ -0,0 +1 @@
+/home/alexander/Projects/gobot/common-libaries/gobot_rpc

stone-dispencer/components/mcp2521

@@ -0,0 +1 @@
+/home/alexander/Projects/gobot/common-libaries/mcp2521

stone-dispencer/components/mcp2521_hardware_interface

@@ -0,0 +1 @@
+/home/alexander/Projects/gobot/common-libaries/mcp2521_hardware_interface

stone-dispencer/main/CMakeLists.txt

@@ -0,0 +1,5 @@
+idf_component_register(SRCS "hello_world_main.cpp"
+                    REQUIRES mcp2521
+                    REQUIRES mcp2521_hardware_interface
+                    REQUIRES driver
+                    INCLUDE_DIRS "")

stone-dispencer/main/hello_world_main.cpp

@@ -0,0 +1,315 @@
+/*
+ * SPDX-FileCopyrightText: 2010-2022 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: CC0-1.0
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include "sdkconfig.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include "driver/gpio.h"
+#include "driver/ledc.h"
+#include "time.h"
+
+#include "mcp2521.hpp"
+#include "mcp2521_hardware_handle.hpp"
+
+#define MCP2521_PIN_INT  GPIO_NUM_5
+#define MCP2521_PIN_SCLK  GPIO_NUM_18
+#define MCP2521_PIN_MISO GPIO_NUM_26
+#define MCP2521_PIN_MOSI GPIO_NUM_23
+#define MCP2521_PIN_CS   GPIO_NUM_25
+
+#define SERVO_0_PIN GPIO_NUM_2
+#define SERVO_1_PIN GPIO_NUM_4
+#define SERVO_2_PIN GPIO_NUM_19
+#define SERVO_3_PIN GPIO_NUM_21
+
+#define LED_PIN GPIO_NUM_22
+
+#define DETECTOR_PIN GPIO_NUM_32
+#define Z_AXIS_DETECT_PIN GPIO_NUM_35
+#define BUTTON_PIN GPIO_NUM_33
+
+#define MOTOR_PERIOD_US 20000
+#define MOTOR_MIN_PULSE_US 500
+#define MOTOR_MAX_PULSE_US 2500 
+
+#define MOTOR_MG996R_US 20000
+#define MOTOR_MG996R_MIN_PULSE_US 500
+#define MOTOR_MG996R_MAX_PULSE_US 2500 
+
+#define PWM_RESOLUTION 18
+
+#define PWM_MAX_VALUE (1 << PWM_RESOLUTION)
+#define MOTOR_MAX_VALUE (MOTOR_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
+#define MOTOR_MIN_VALUE (MOTOR_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
+#define MOTOR_CENTER_VALUE ((MOTOR_MAX_VALUE + MOTOR_MIN_VALUE) / 2)
+
+#define MOTOR_MG996R_MAX_VALUE (MOTOR_MG996R_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
+#define MOTOR_MG996R_MIN_VALUE (MOTOR_MG996R_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
+#define MOTOR_MG996R_CENTER_VALUE ((MOTOR_MG996R_MAX_VALUE + MOTOR_MG996R_MIN_VALUE) / 2)
+
+#define MOTOR_RANGE (MOTOR_MAX_VALUE - MOTOR_MIN_VALUE)
+#define MOTOR_HALF_RANGE (MOTOR_RANGE / 2)
+
+#define MOTOR_MG996R_RANGE (MOTOR_MG996R_MAX_VALUE - MOTOR_MG996R_MIN_VALUE)
+#define MOTOR_MG996R_HALF_RANGE (MOTOR_MG996R_RANGE / 2)
+
+#define CHOPPER_0_CLOSED (MOTOR_CENTER_VALUE - 1000)
+#define CHOPPER_1_CLOSED (MOTOR_CENTER_VALUE + 900)
+#define CHOPPER_0_OPEN (MOTOR_CENTER_VALUE - MOTOR_HALF_RANGE*0.75)
+#define CHOPPER_1_OPEN (MOTOR_CENTER_VALUE + MOTOR_HALF_RANGE*0.75)
+
+#define HEAD_MOVE_UP_TIME 250
+#define HEAD_MOVE_UP   (MOTOR_MG996R_CENTER_VALUE + MOTOR_MG996R_HALF_RANGE*0.5)
+#define HEAD_MOVE_STOP (MOTOR_MG996R_CENTER_VALUE)
+#define HEAD_MOVE_DOWN_TIME 750
+#define HEAD_MOVE_DOWN (MOTOR_MG996R_CENTER_VALUE - MOTOR_MG996R_HALF_RANGE*0.5)
+
+// Z-Axis Motor
+bool motor_stop_up_flag = false;
+SemaphoreHandle_t motor_stop_down_semaphore;
+SemaphoreHandle_t motor_stop_up_semaphore;
+
+static void IRAM_ATTR isr_motor_stop_up(void *args) {
+    if(motor_stop_up_flag) {
+        motor_stop_up_flag = false;
+        xSemaphoreGiveFromISR(motor_stop_up_semaphore, NULL);
+    }
+}
+
+static void task_fn_motor_stop_up(void *args) {
+    while(true) {
+        xSemaphoreTake(motor_stop_up_semaphore, portMAX_DELAY);
+        vTaskDelay(HEAD_MOVE_UP_TIME / portTICK_PERIOD_MS);
+        ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
+        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
+    }
+}
+
+static void task_fn_motor_stop_down(void *args) {
+    while(true) {
+        xSemaphoreTake(motor_stop_down_semaphore, portMAX_DELAY);
+        vTaskDelay(HEAD_MOVE_DOWN_TIME / portTICK_PERIOD_MS);
+        ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
+        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
+    }
+}
+
+void lower_head() {
+    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_DOWN);
+    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
+    xSemaphoreGive(motor_stop_down_semaphore);
+    vTaskDelay(500 / portTICK_PERIOD_MS);
+}
+
+void raise_head() {
+    if(!gpio_get_level(Z_AXIS_DETECT_PIN)) {
+        return;
+    }
+
+    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_UP);
+    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
+    motor_stop_up_flag = true;
+}
+
+// Dispencing mechanism
+void set_copper(bool bottom, bool top) {
+    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, bottom ? CHOPPER_1_OPEN : CHOPPER_1_CLOSED);
+    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, top ? CHOPPER_0_OPEN : CHOPPER_0_CLOSED);
+    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1);
+    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
+}
+
+enum DISPENSER_RESULT {
+    SUCESS = 0,
+    EMPTY_WARINING = 1,
+    EMPTY = 2,
+};
+
+bool load_stone() {
+    if(!gpio_get_level(DETECTOR_PIN)) {
+        // No stone loaded
+        return true;
+    }
+    
+    set_copper(true, false);
+    vTaskDelay(250 / portTICK_PERIOD_MS);
+    set_copper(false, false);
+    
+    if(!gpio_get_level(DETECTOR_PIN)) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+enum DISPENSER_RESULT dispens_stone() {
+    
+    if(!gpio_get_level(DETECTOR_PIN)) {
+        // No stone loaded
+
+        set_copper(false, true);
+        vTaskDelay(250 / portTICK_PERIOD_MS);
+        set_copper(false, false);
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+    
+        if(!gpio_get_level(DETECTOR_PIN)) {
+             // No stone loaded again
+            return EMPTY; 
+        }
+
+    }
+    lower_head();
+    set_copper(true, false);
+    vTaskDelay(250 / portTICK_PERIOD_MS);
+    set_copper(false, false);
+    vTaskDelay(100 / portTICK_PERIOD_MS);
+    set_copper(false, true);
+    vTaskDelay(250 / portTICK_PERIOD_MS);
+    set_copper(false, false);
+    raise_head();
+    
+    if(!gpio_get_level(DETECTOR_PIN)) {
+        return SUCESS;
+    } else {
+        return EMPTY_WARINING;
+    }
+}
+
+extern "C" void app_main(void)
+{
+    printf("Hello world!\n");
+
+    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
+    gpio_set_direction(DETECTOR_PIN, GPIO_MODE_INPUT);
+    gpio_set_direction(Z_AXIS_DETECT_PIN, GPIO_MODE_INPUT);
+    gpio_set_direction(BUTTON_PIN, GPIO_MODE_INPUT);
+
+    motor_stop_up_semaphore = xSemaphoreCreateBinary();
+    motor_stop_down_semaphore = xSemaphoreCreateBinary();
+    xTaskCreatePinnedToCore(task_fn_motor_stop_up, "motor_stop_up", 2048, NULL, 4, NULL, 0);
+    xTaskCreatePinnedToCore(task_fn_motor_stop_down, "motor_stop_down", 2048, NULL, 4, NULL, 0);
+
+    gpio_set_intr_type(Z_AXIS_DETECT_PIN, GPIO_INTR_ANYEDGE);
+    gpio_install_isr_service(0);
+    gpio_isr_handler_add(Z_AXIS_DETECT_PIN, isr_motor_stop_up, NULL);
+
+    spi_bus_config_t bus_config;
+    MCP2521_HardwareHandle_ESP MCP2521_HardwareHandle_ESP(
+        VSPI_HOST,
+        &bus_config,
+        MCP2521_PIN_MOSI,
+        MCP2521_PIN_MISO,
+        MCP2521_PIN_SCLK,
+        MCP2521_PIN_CS,
+        MCP2521_PIN_INT
+    );
+
+    if(0) {
+        gpio_set_direction(MCP2521_PIN_SCLK, GPIO_MODE_OUTPUT);
+        gpio_set_direction(MCP2521_PIN_MISO, GPIO_MODE_OUTPUT);
+        gpio_set_direction(MCP2521_PIN_MOSI, GPIO_MODE_OUTPUT);
+        gpio_set_direction(MCP2521_PIN_CS, GPIO_MODE_OUTPUT);
+        gpio_set_level(MCP2521_PIN_SCLK, 1);
+        gpio_set_level(MCP2521_PIN_MISO, 1);
+        gpio_set_level(MCP2521_PIN_MOSI, 1);
+        gpio_set_level(MCP2521_PIN_CS, 1);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+        gpio_set_level(MCP2521_PIN_SCLK, 0);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+        gpio_set_level(MCP2521_PIN_MOSI, 0);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+        gpio_set_level(MCP2521_PIN_MISO, 0);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+        gpio_set_level(MCP2521_PIN_CS, 0); // Works
+    }
+
+    MCP2521 mcp2521(&MCP2521_HardwareHandle_ESP);
+    mcp2521.reset();
+    mcp2521.set_mode_of_operation(MCP2521_OPERATION_MODE::NORMAL, true);
+    printf("CANSTAT: %x\n", mcp2521.read_reg(MCP2521_CANCTRL));
+    printf("MCP2521 initialized\n");
+
+    ledc_timer_config_t ledc_timer = {
+        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
+        .duty_resolution = LEDC_TIMER_18_BIT,
+        .timer_num = LEDC_TIMER_0,
+        .freq_hz = 50,
+        .clk_cfg = LEDC_AUTO_CLK
+    };
+
+    ledc_channel_config_t servo0_channel_config = {
+        .gpio_num = SERVO_0_PIN,
+        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
+        .channel = LEDC_CHANNEL_0,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = LEDC_TIMER_0,
+        .duty = 0,
+        .hpoint = 0,
+        .flags = {
+            .output_invert = 1
+        }
+    };
+
+    ledc_channel_config_t servo1_channel_config = {
+        .gpio_num = SERVO_1_PIN,
+        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
+        .channel = LEDC_CHANNEL_1,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = LEDC_TIMER_0,
+        .duty = 0,
+        .hpoint = 0,
+        .flags = {
+            .output_invert = 1
+        }
+    };
+
+    ledc_channel_config_t servo2_channel_config = {
+        .gpio_num = SERVO_2_PIN,
+        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
+        .channel = LEDC_CHANNEL_2,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = LEDC_TIMER_0,
+        .duty = 0,
+        .hpoint = 0,
+        .flags = {
+            .output_invert = 1
+        }
+    };
+    
+    ledc_channel_config_t servo3_channel_config = {
+        .gpio_num = SERVO_3_PIN,
+        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
+        .channel = LEDC_CHANNEL_3,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = LEDC_TIMER_0,
+        .duty = 0,
+        .hpoint = 0,
+        .flags = {
+            .output_invert = 1
+        }
+    };
+
+    ledc_timer_config(&ledc_timer);
+    ledc_channel_config(&servo0_channel_config);
+    ledc_channel_config(&servo1_channel_config);
+    ledc_channel_config(&servo2_channel_config);
+    ledc_channel_config(&servo3_channel_config);
+
+    raise_head();
+    set_copper(false, false);
+
+    while (true) {
+        while (!gpio_get_level(BUTTON_PIN)) {
+            vTaskDelay(50 / portTICK_PERIOD_MS);
+        }    
+        printf("Sucess: %d\n", dispens_stone());
+    }
+    
+
+}