diff --git a/.gitmodules b/.gitmodules
index 3ac6350..7ac63a3 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:d032327861aed4c59ac883247a4977b1b7f468bbb3705f31100f7fc648f4a682
-size 1937
+oid sha256:22f514f27d3a1088dae0a5ab17c424285d140dac7b646badc26bb5419b070867
+size 2148
diff --git a/stone-dispencer-control/.$Stone-Dispencer-Circuit.drawio.bkp b/stone-dispencer-control/.$Stone-Dispencer-Circuit.drawio.bkp
new file mode 100644
index 0000000..ffd0725
--- /dev/null
+++ b/stone-dispencer-control/.$Stone-Dispencer-Circuit.drawio.bkp
@@ -0,0 +1,219 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stone-dispencer-control/Stone-Dispencer-Circuit.drawio b/stone-dispencer-control/Stone-Dispencer-Circuit.drawio
new file mode 100644
index 0000000..8d5ea86
--- /dev/null
+++ b/stone-dispencer-control/Stone-Dispencer-Circuit.drawio
@@ -0,0 +1,219 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stone-dispencer-control/firmware-stone-dispencer/.vscode/tasks.json b/stone-dispencer-control/firmware-stone-dispencer/.vscode/tasks.json
index 1b6a876..8fb5999 100644
--- a/stone-dispencer-control/firmware-stone-dispencer/.vscode/tasks.json
+++ b/stone-dispencer-control/firmware-stone-dispencer/.vscode/tasks.json
@@ -9,8 +9,12 @@
"args": ["-C", "${workspaceFolder}/build"],
"group": "build",
"presentation": {
+ "echo": true,
"reveal": "always",
- "panel": "dedicated"
+ "focus": true,
+ "panel": "shared",
+ "showReuseMessage": true,
+ "clear": true
},
"problemMatcher": "$gcc",
"windows": {
@@ -27,8 +31,12 @@
"-fx"
],
"presentation": {
+ "echo": true,
"reveal": "always",
- "panel": "dedicated"
+ "focus": true,
+ "panel": "shared",
+ "showReuseMessage": true,
+ "clear": true
},
"problemMatcher": [],
"windows": {
@@ -46,9 +54,49 @@
"interface/cmsis-dap.cfg",
"-f",
"target/${command:raspberry-pi-pico.getTarget}.cfg",
- "-c",
- "adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" verify reset exit"
+ "-c", "adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" verify",
+ "-c", "reset halt",
+ "-c", "rp2040.core1 arp_reset assert 0",
+ "-c", "rp2040.core0 arp_reset assert 0",
+ "-c", "exit"
],
+ "presentation": {
+ "echo": true,
+ "reveal": "always",
+ "focus": true,
+ "panel": "shared",
+ "showReuseMessage": true,
+ "clear": true
+ },
+ "dependsOn": ["Compile Project"],
+ "problemMatcher": [],
+ "windows": {
+ "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+ }
+ },
+ {
+ "label": "Reset",
+ "type": "process",
+ "command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+ "args": [
+ "-s",
+ "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
+ "-f",
+ "interface/cmsis-dap.cfg",
+ "-f", "target/${command:raspberry-pi-pico.getTarget}.cfg",
+ "-c", "adapter speed 5000; init; reset halt;",
+ "-c", "rp2040.core1 arp_reset assert 0",
+ "-c", "rp2040.core0 arp_reset assert 0",
+ "-c", "exit"
+ ],
+ "presentation": {
+ "echo": true,
+ "reveal": "always",
+ "focus": true,
+ "panel": "shared",
+ "showReuseMessage": true,
+ "clear": true
+ },
"problemMatcher": [],
"windows": {
"command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
diff --git a/stone-dispencer-control/firmware-stone-dispencer/CMakeLists.txt b/stone-dispencer-control/firmware-stone-dispencer/CMakeLists.txt
index a05e21b..e5a8958 100644
--- a/stone-dispencer-control/firmware-stone-dispencer/CMakeLists.txt
+++ b/stone-dispencer-control/firmware-stone-dispencer/CMakeLists.txt
@@ -30,12 +30,22 @@ include(pico_sdk_import.cmake)
project(firmware-stone-dispencer C CXX ASM)
+# Add FreeRTOS
+set(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/FreeRTOS-Kernel)
+include(cmake/FreeRTOS_Kernel_import.cmake)
+
# Initialise the Raspberry Pi Pico SDK
pico_sdk_init()
# Add executable. Default name is the project name, version 0.1
-add_executable(firmware-stone-dispencer firmware-stone-dispencer.cpp )
+add_executable(firmware-stone-dispencer
+ src/main.cpp
+ src/headSystem/init.cpp
+ src/headSystem/z-axis.cpp
+ src/headSystem/chopper.cpp
+ src/headSystem/stoneDispencer.cpp
+)
pico_set_program_name(firmware-stone-dispencer "firmware-stone-dispencer")
pico_set_program_version(firmware-stone-dispencer "0.1")
@@ -46,7 +56,14 @@ pico_enable_stdio_usb(firmware-stone-dispencer 0)
# Add the standard library to the build
target_link_libraries(firmware-stone-dispencer
- pico_stdlib)
+ pico_stdlib
+ hardware_pwm
+ FreeRTOS-Kernel-Heap4
+)
+
+target_include_directories(firmware-stone-dispencer PUBLIC
+ ${CMAKE_CURRENT_LIST_DIR}/include
+)
# Add the standard include files to the build
target_include_directories(firmware-stone-dispencer PRIVATE
diff --git a/stone-dispencer-control/firmware-stone-dispencer/FreeRTOSConfig.h b/stone-dispencer-control/firmware-stone-dispencer/FreeRTOSConfig.h
new file mode 100644
index 0000000..601b4db
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/FreeRTOSConfig.h
@@ -0,0 +1,143 @@
+/*
+ * FreeRTOS V202111.00
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
+ *
+ * See http://www.freertos.org/a00110.html
+ *----------------------------------------------------------*/
+
+/* Scheduler Related */
+#define configUSE_PREEMPTION 1
+#define configUSE_TICKLESS_IDLE 0
+#define configUSE_IDLE_HOOK 0
+#define configUSE_TICK_HOOK 0
+#define configTICK_RATE_HZ ( ( TickType_t ) 1000 )
+#define configMAX_PRIORITIES 32
+#define configMINIMAL_STACK_SIZE ( configSTACK_DEPTH_TYPE ) 256
+#define configUSE_16_BIT_TICKS 0
+
+#define configIDLE_SHOULD_YIELD 1
+
+/* Synchronization Related */
+#define configUSE_MUTEXES 1
+#define configUSE_RECURSIVE_MUTEXES 1
+#define configUSE_APPLICATION_TASK_TAG 0
+#define configUSE_COUNTING_SEMAPHORES 1
+#define configQUEUE_REGISTRY_SIZE 8
+#define configUSE_QUEUE_SETS 1
+#define configUSE_TIME_SLICING 1
+#define configUSE_NEWLIB_REENTRANT 0
+// todo need this for lwip FreeRTOS sys_arch to compile
+#define configENABLE_BACKWARD_COMPATIBILITY 1
+#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5
+
+/* System */
+#define configSTACK_DEPTH_TYPE uint32_t
+#define configMESSAGE_BUFFER_LENGTH_TYPE size_t
+
+/* Memory allocation related definitions. */
+#define configSUPPORT_STATIC_ALLOCATION 0
+#define configSUPPORT_DYNAMIC_ALLOCATION 1
+#define configTOTAL_HEAP_SIZE (1024 * 64)
+#define configAPPLICATION_ALLOCATED_HEAP (1024 * 32)
+
+/* Hook function related definitions. */
+#define configCHECK_FOR_STACK_OVERFLOW 0
+#define configUSE_MALLOC_FAILED_HOOK 0
+#define configUSE_DAEMON_TASK_STARTUP_HOOK 0
+
+/* Run time and task stats gathering related definitions. */
+#define configGENERATE_RUN_TIME_STATS 0
+#define configUSE_TRACE_FACILITY 1
+#define configUSE_STATS_FORMATTING_FUNCTIONS 0
+
+/* Co-routine related definitions. */
+#define configUSE_CO_ROUTINES 0
+#define configMAX_CO_ROUTINE_PRIORITIES 1
+
+/* Software timer related definitions. */
+#define configUSE_TIMERS 1
+#define configTIMER_TASK_PRIORITY ( configMAX_PRIORITIES - 1 )
+#define configTIMER_QUEUE_LENGTH 10
+#define configTIMER_TASK_STACK_DEPTH 1024
+
+/* Interrupt nesting behaviour configuration. */
+/*
+#define configKERNEL_INTERRUPT_PRIORITY [dependent of processor]
+#define configMAX_SYSCALL_INTERRUPT_PRIORITY [dependent on processor and application]
+#define configMAX_API_CALL_INTERRUPT_PRIORITY [dependent on processor and application]
+*/
+
+#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
+/* SMP port only */
+#define configUSE_PASSIVE_IDLE_HOOK 0
+#define configNUMBER_OF_CORES 1
+#define configTICK_CORE 0
+#define configRUN_MULTIPLE_PRIORITIES 1
+//#define configUSE_CORE_AFFINITY 1
+#endif
+
+/* RP2040 specific */
+#define configSUPPORT_PICO_SYNC_INTEROP 1
+#define configSUPPORT_PICO_TIME_INTEROP 1
+
+#include
+/* Define to trap errors during development. */
+#define configASSERT(x) assert(x)
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function. */
+#define INCLUDE_vTaskPrioritySet 1
+#define INCLUDE_uxTaskPriorityGet 1
+#define INCLUDE_vTaskDelete 1
+#define INCLUDE_vTaskSuspend 1
+#define INCLUDE_vTaskDelayUntil 1
+#define INCLUDE_vTaskDelay 1
+#define INCLUDE_xTaskGetSchedulerState 1
+#define INCLUDE_xTaskGetCurrentTaskHandle 1
+#define INCLUDE_uxTaskGetStackHighWaterMark 1
+#define INCLUDE_xTaskGetIdleTaskHandle 1
+#define INCLUDE_eTaskGetState 1
+#define INCLUDE_xTimerPendFunctionCall 1
+#define INCLUDE_xTaskAbortDelay 1
+#define INCLUDE_xTaskGetHandle 1
+#define INCLUDE_xTaskResumeFromISR 1
+#define INCLUDE_xQueueGetMutexHolder 1
+
+/* A header file that defines trace macro can be included here. */
+
+#endif /* FREERTOS_CONFIG_H */
\ No newline at end of file
diff --git a/stone-dispencer-control/firmware-stone-dispencer/cmake/FreeRTOS_Kernel_import.cmake b/stone-dispencer-control/firmware-stone-dispencer/cmake/FreeRTOS_Kernel_import.cmake
new file mode 100644
index 0000000..109a54e
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/cmake/FreeRTOS_Kernel_import.cmake
@@ -0,0 +1,61 @@
+# This is a copy of /portable/ThirdParty/GCC/RP2040/FREERTOS_KERNEL_import.cmake
+
+# This can be dropped into an external project to help locate the FreeRTOS kernel
+# It should be include()ed prior to project(). Alternatively this file may
+# or the CMakeLists.txt in this directory may be included or added via add_subdirectory
+# respectively.
+
+if (DEFINED ENV{FREERTOS_KERNEL_PATH} AND (NOT FREERTOS_KERNEL_PATH))
+ set(FREERTOS_KERNEL_PATH $ENV{FREERTOS_KERNEL_PATH})
+ message("Using FREERTOS_KERNEL_PATH from environment ('${FREERTOS_KERNEL_PATH}')")
+endif ()
+
+set(FREERTOS_KERNEL_RP2040_RELATIVE_PATH "portable/ThirdParty/GCC/RP2040")
+# undo the above
+set(FREERTOS_KERNEL_RP2040_BACK_PATH "../../../..")
+
+if (NOT FREERTOS_KERNEL_PATH)
+ # check if we are inside the FreeRTOS kernel tree (i.e. this file has been included directly)
+ get_filename_component(_ACTUAL_PATH ${CMAKE_CURRENT_LIST_DIR} REALPATH)
+ get_filename_component(_POSSIBLE_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} REALPATH)
+ if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH)
+ get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH)
+ endif()
+ if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH)
+ get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH)
+ message("Setting FREERTOS_KERNEL_PATH to ${FREERTOS_KERNEL_PATH} based on location of FreeRTOS-Kernel-import.cmake")
+ elseif (PICO_SDK_PATH AND EXISTS "${PICO_SDK_PATH}/../FreeRTOS-Kernel")
+ set(FREERTOS_KERNEL_PATH ${PICO_SDK_PATH}/../FreeRTOS-Kernel)
+ message("Defaulting FREERTOS_KERNEL_PATH as sibling of PICO_SDK_PATH: ${FREERTOS_KERNEL_PATH}")
+ endif()
+endif ()
+
+if (NOT FREERTOS_KERNEL_PATH)
+ foreach(POSSIBLE_SUFFIX Source FreeRTOS-Kernel FreeRTOS/Source)
+ # check if FreeRTOS-Kernel exists under directory that included us
+ set(SEARCH_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
+ get_filename_component(_POSSIBLE_PATH ${SEARCH_ROOT}/${POSSIBLE_SUFFIX} REALPATH)
+ if (EXISTS ${_POSSIBLE_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt)
+ get_filename_component(FREERTOS_KERNEL_PATH ${_POSSIBLE_PATH} REALPATH)
+ message("Setting FREERTOS_KERNEL_PATH to '${FREERTOS_KERNEL_PATH}' found relative to enclosing project")
+ break()
+ endif()
+ endforeach()
+endif()
+
+if (NOT FREERTOS_KERNEL_PATH)
+ message(FATAL_ERROR "FreeRTOS location was not specified. Please set FREERTOS_KERNEL_PATH.")
+endif()
+
+set(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" CACHE PATH "Path to the FreeRTOS Kernel")
+
+get_filename_component(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${FREERTOS_KERNEL_PATH})
+ message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' not found")
+endif()
+if (NOT EXISTS ${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt)
+ message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' does not contain an RP2040 port here: ${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}")
+endif()
+set(FREERTOS_KERNEL_PATH ${FREERTOS_KERNEL_PATH} CACHE PATH "Path to the FreeRTOS_KERNEL" FORCE)
+
+add_subdirectory(${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} FREERTOS_KERNEL)
diff --git a/stone-dispencer-control/firmware-stone-dispencer/firmware-stone-dispencer.cpp b/stone-dispencer-control/firmware-stone-dispencer/firmware-stone-dispencer.cpp
deleted file mode 100644
index 89b05c6..0000000
--- a/stone-dispencer-control/firmware-stone-dispencer/firmware-stone-dispencer.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include
-#include "pico/stdlib.h"
-
-
-int main()
-{
- stdio_init_all();
-
- while (true) {
- printf("Hello, world!\n");
- sleep_ms(1000);
- }
-}
diff --git a/stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp b/stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp
new file mode 100644
index 0000000..0a7e49d
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp
@@ -0,0 +1,43 @@
+#pragma once
+#include "pinConfig.hpp"
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+
+enum STONE_STATE {
+ EMPTY = 0b00, FULL = 0b10, LOW = 0b01
+};
+
+void vHeadTaskFn(void *pvParameters);
+
+class HeadSystem {
+private:
+ unsigned int motor_chopper1_slice_num;
+ unsigned int motor_chopper2_slice_num;
+ unsigned int motor_z_axis_slice_num;
+
+ bool headUp = false;
+
+ STONE_STATE stoneState;
+ xQueueHandle headStateQueue;
+ xSemaphoreHandle headSateSemaphore;
+ xTaskHandle headTaskHandle;
+
+public:
+ HeadSystem();
+
+ void initPins();
+
+ void setChopper(bool open1, bool open2);
+ void setHeadUp(bool up);
+
+ bool preLoad();
+ STONE_STATE dropStone();
+
+ STONE_STATE dropSequence();
+
+ void vHeadTask();
+
+
+};
+
diff --git a/stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp b/stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp
new file mode 100644
index 0000000..8fe98f9
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp
@@ -0,0 +1,57 @@
+#pragma once
+#include "pico/stdlib.h"
+
+#define SENSOR_STONE_PIN 6
+#define SENSOR_Z_AXIS_PIN 7
+
+#define MOTOR_CHOPPER1_PIN 11
+#define MOTOR_CHOPPER2_PIN 12
+#define MOTOR_Z_AXIS_PIN 13
+
+#define LED1_PIN 25
+#define LED2_PIN 16
+#define BUTTON_PIN 20
+
+#define GOBOTRPC_SCL_PIN 5
+#define GOBOTRPC_SDA_PIN 4
+#define GOBOTRPC_INT_PIN 2
+
+
+// Pwm Config
+
+#define PWM_TARGET_FREQ_HZ 100000.f
+#define PWM_CLK_DIV ((float)(SYS_CLK_HZ) / PWM_TARGET_FREQ_HZ)
+// 10us per Cycle -> 20ms / 10us = 2000
+#define PWM_TOTAL 11080
+
+#define PWM_CALC_CYCLE(n) ((int)((PWM_TOTAL/20.f) * (n)))
+
+#define MOTOR_MS18_MIN PWM_CALC_CYCLE(0.48f)
+#define MOTOR_MS18_MAX PWM_CALC_CYCLE(2.5f)
+#define MOTOR_MS18_MID (MOTOR_MS18_MAX + MOTOR_MS18_MIN) / 2
+
+#define MOTOR_MG996R_MIN PWM_CALC_CYCLE(0.36f)
+#define MOTOR_MG996R_MAX PWM_CALC_CYCLE(2.6f)
+#define MOTOR_MG966R_HALF_RANGE ((MOTOR_MG996R_MAX - MOTOR_MG996R_MIN) / 2)
+#define MOTOR_MG996R_MID (MOTOR_MG996R_MAX + MOTOR_MG996R_MIN) / 2
+
+#define PWM_INVERT(n) ((PWM_TOTAL + 1) - (n))
+
+// Chopper - Motor Mapping
+#define MOTOR_CHOPPER1 0
+#define MOTOR_CHOPPER2 1
+
+// Chopper States
+#define CHOPPER1_OPEN MOTOR_MS18_MID - 400
+#define CHOPPER1_CLOSE MOTOR_MS18_MID + 40
+#define CHOPPER2_OPEN MOTOR_MS18_MID + 375
+#define CHOPPER2_CLOSE MOTOR_MS18_MID + 30
+
+// Z Axis Config
+#define Z_AXIS_OFF MOTOR_MS18_MID
+#define Z_AXIS_UP MOTOR_MS18_MID + ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
+#define Z_AXIS_DOWN MOTOR_MS18_MID - ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
+#define Z_AXIS_DOWN_TIME 600
+#define Z_AXIS_UP_TIME 600
+
+#define Z_AXIS_IS_UP() (gpio_get(SENSOR_Z_AXIS_PIN) == 1)
\ No newline at end of file
diff --git a/stone-dispencer-control/firmware-stone-dispencer/lib/FreeRTOS-Kernel b/stone-dispencer-control/firmware-stone-dispencer/lib/FreeRTOS-Kernel
new file mode 160000
index 0000000..e55bde2
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/lib/FreeRTOS-Kernel
@@ -0,0 +1 @@
+Subproject commit e55bde213348c8911205f81e89016dd4ba2ba79d
diff --git a/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/chopper.cpp b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/chopper.cpp
new file mode 100644
index 0000000..0ec83ab
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/chopper.cpp
@@ -0,0 +1,64 @@
+#include "headSystem.hpp"
+#include "hardware/pwm.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+void HeadSystem::setChopper(bool open1, bool open2) {
+ unsigned setpoint1 = open1 ? CHOPPER1_OPEN : CHOPPER1_CLOSE;
+ unsigned setpoint2 = open2 ? CHOPPER2_OPEN : CHOPPER2_CLOSE;
+
+ setpoint1 = PWM_INVERT(setpoint1);
+ setpoint2 = PWM_INVERT(setpoint2);
+
+ if(MOTOR_CHOPPER1) {
+ pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint1);
+ } else {
+ pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint1);
+ }
+
+ if(MOTOR_CHOPPER2) {
+ pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint2);
+ } else {
+ pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint2);
+ }
+}
+
+STONE_STATE HeadSystem::dropStone() {
+
+ if(!gpio_get(SENSOR_STONE_PIN)) {
+ setChopper(true, false);
+ vTaskDelay(250 / portTICK_PERIOD_MS);
+ setChopper(false, false);
+ vTaskDelay(100 / portTICK_PERIOD_MS);
+
+ if(!gpio_get(SENSOR_STONE_PIN)) {
+ return EMPTY;
+ }
+ }
+
+ setChopper(false, true);
+ vTaskDelay(250 / portTICK_PERIOD_MS);
+ setChopper(false, false);
+ vTaskDelay(100 / portTICK_PERIOD_MS);
+ setChopper(true, false);
+ vTaskDelay(250 / portTICK_PERIOD_MS);
+ setChopper(false, false);
+ vTaskDelay(100 / portTICK_PERIOD_MS);
+
+
+ return gpio_get(SENSOR_STONE_PIN) ? FULL : LOW;
+}
+
+bool HeadSystem::preLoad() {
+ if(gpio_get(SENSOR_STONE_PIN)) {
+ return true;
+ }
+
+ setChopper(true, false);
+ vTaskDelay(250 / portTICK_PERIOD_MS);
+ setChopper(false, false);
+ vTaskDelay(100 / portTICK_PERIOD_MS);
+
+ return gpio_get(SENSOR_STONE_PIN);
+}
\ No newline at end of file
diff --git a/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/init.cpp b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/init.cpp
new file mode 100644
index 0000000..31fe150
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/init.cpp
@@ -0,0 +1,63 @@
+#include "headSystem.hpp"
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+
+HeadSystem::HeadSystem() {
+ initPins();
+}
+
+void vHeadTaskFn(void *pvParameters) {
+ HeadSystem * headSystem = (HeadSystem *) pvParameters;
+ headSystem->vHeadTask();
+}
+
+void HeadSystem::initPins() {
+ stoneState = EMPTY;
+
+ headStateQueue = xQueueCreate(3, sizeof(bool));
+ headSateSemaphore = xSemaphoreCreateBinary();
+ xTaskCreate(vHeadTaskFn, "Head Task", 1024, this, 1, &headTaskHandle);
+
+ gpio_init(SENSOR_STONE_PIN);
+ gpio_set_dir(SENSOR_STONE_PIN, GPIO_IN);
+ gpio_pull_up(SENSOR_STONE_PIN);
+
+ gpio_init(SENSOR_Z_AXIS_PIN);
+ gpio_set_dir(SENSOR_Z_AXIS_PIN, GPIO_IN);
+ gpio_pull_down(SENSOR_Z_AXIS_PIN);
+
+ gpio_init(LED1_PIN);
+ gpio_set_dir(LED1_PIN, GPIO_OUT);
+ gpio_init(LED2_PIN);
+ gpio_set_dir(LED2_PIN, GPIO_OUT);
+
+ gpio_set_function(MOTOR_CHOPPER1_PIN, GPIO_FUNC_PWM);
+ gpio_set_function(MOTOR_CHOPPER2_PIN, GPIO_FUNC_PWM);
+ gpio_set_function(MOTOR_Z_AXIS_PIN, GPIO_FUNC_PWM);
+
+ gpio_init(BUTTON_PIN);
+ gpio_set_dir(BUTTON_PIN, GPIO_IN);
+ gpio_pull_up(BUTTON_PIN);
+
+ motor_chopper1_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER1_PIN);
+ motor_chopper2_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER2_PIN);
+ motor_z_axis_slice_num = pwm_gpio_to_slice_num(MOTOR_Z_AXIS_PIN);
+
+ pwm_config config = pwm_get_default_config();
+ pwm_config_set_wrap(&config, PWM_TOTAL);
+ pwm_config_set_clkdiv(&config, PWM_CLK_DIV);
+
+ pwm_init(motor_chopper1_slice_num, &config, true);
+ pwm_init(motor_chopper2_slice_num, &config, true);
+ pwm_init(motor_z_axis_slice_num, &config, true);
+
+ pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, PWM_INVERT(MOTOR_MS18_MID));
+ pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, PWM_INVERT(MOTOR_MS18_MID));
+ pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(MOTOR_MS18_MID));
+
+ setChopper(false, false);
+}
\ No newline at end of file
diff --git a/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/stoneDispencer.cpp b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/stoneDispencer.cpp
new file mode 100644
index 0000000..b265dcc
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/stoneDispencer.cpp
@@ -0,0 +1,19 @@
+#include "headSystem.hpp"
+#include "FreeRTOS.h"
+#include "task.h"
+
+STONE_STATE HeadSystem::dropSequence() {
+ gpio_put(LED1_PIN, 1);
+ if(!preLoad()) {
+ this->stoneState = EMPTY;
+ return this->stoneState;
+ }
+
+ bool prev_head_state = headUp;
+ setHeadUp(false);
+ STONE_STATE res = dropStone();
+ setHeadUp(prev_head_state);
+ this->stoneState = res;
+ gpio_put(LED2_PIN, 0);
+ return res;
+}
\ No newline at end of file
diff --git a/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/z-axis.cpp b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/z-axis.cpp
new file mode 100644
index 0000000..200b5c5
--- /dev/null
+++ b/stone-dispencer-control/firmware-stone-dispencer/src/headSystem/z-axis.cpp
@@ -0,0 +1,50 @@
+#include "headSystem.hpp"
+#include "hardware/pwm.h"
+#include "pico/stdlib.h"
+#include
+
+#include "FreeRTOS.h"
+
+void HeadSystem::vHeadTask() {
+ while (1) {
+ bool new_state;
+ xQueueReceive(headStateQueue, &new_state, portMAX_DELAY);
+
+ // Units in milliseconds
+ const int READ_DELAY = 50;
+ const int MAX_TIME = 7000;
+
+ if(headUp != new_state) {
+ if(new_state && !Z_AXIS_IS_UP()) { // Head Up
+
+ pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_UP));
+ vTaskDelay(100 / portTICK_PERIOD_MS);
+
+ for(int i=0; i
+#include "pico/stdlib.h"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "hardware/pwm.h"
+
+#include "headSystem.hpp"
+
+void vMainTask(void *pvParameters) {
+ HeadSystem * headSystem = (HeadSystem *) pvParameters;
+ bool buttonState = false;
+ bool buttonLastState = false;
+
+ headSystem->setHeadUp(true);
+
+ while (1) {
+ buttonState = !gpio_get(BUTTON_PIN);
+
+ if(buttonState) {
+ STONE_STATE s = headSystem->dropSequence();
+ printf("Stone dropped: %d\n", s);
+ }
+
+ buttonLastState = buttonState;
+ vTaskDelay(pdMS_TO_TICKS(100));
+ }
+}
+
+int main()
+{
+ stdio_init_all();
+
+ HeadSystem headSystem;
+ printf("HeadSystem initialized\n");
+
+ xTaskCreate(vMainTask, "Button Task", 1024, &headSystem, 1, NULL);
+ vTaskStartScheduler();
+
+ while (1) {
+ tight_loop_contents();
+ }
+
+}
diff --git a/vacum-control/circuit/.$Vacum-Control-Circuit.drawio.bkp b/vacum-control/circuit/.$Vacum-Control-Circuit.drawio.bkp
new file mode 100644
index 0000000..0f5d0b3
--- /dev/null
+++ b/vacum-control/circuit/.$Vacum-Control-Circuit.drawio.bkp
@@ -0,0 +1,207 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/vacum-control/circuit/Vacum-Control-Circuit.drawio b/vacum-control/circuit/Vacum-Control-Circuit.drawio
index 0f5d0b3..3e68a04 100644
--- a/vacum-control/circuit/Vacum-Control-Circuit.drawio
+++ b/vacum-control/circuit/Vacum-Control-Circuit.drawio
@@ -1,18 +1,18 @@
-
+
-
+
-
+
-
+
@@ -21,79 +21,79 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -104,7 +104,7 @@
-
+
@@ -116,7 +116,7 @@
-
+
@@ -128,16 +128,16 @@
-
+
-
+
-
+
-
+
@@ -149,7 +149,7 @@
-
+
@@ -160,7 +160,7 @@
-
+
@@ -171,36 +171,45 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+