From 78a5148a1ecb6b7a395147c3ab889f079a4e0b1e Mon Sep 17 00:00:00 2001 From: AlexanderHD27 Date: Thu, 2 Jan 2025 19:10:59 +0100 Subject: [PATCH] Finished Head Softaware --- .gitmodules | 4 +- .../.$Stone-Dispencer-Circuit.drawio.bkp | 219 ++++++++++++++++++ .../Stone-Dispencer-Circuit.drawio | 219 ++++++++++++++++++ .../.vscode/tasks.json | 56 ++++- .../firmware-stone-dispencer/CMakeLists.txt | 21 +- .../firmware-stone-dispencer/FreeRTOSConfig.h | 143 ++++++++++++ .../cmake/FreeRTOS_Kernel_import.cmake | 61 +++++ .../firmware-stone-dispencer.cpp | 13 -- .../include/headSystem.hpp | 43 ++++ .../include/pinConfig.hpp | 57 +++++ .../lib/FreeRTOS-Kernel | 1 + .../src/headSystem/chopper.cpp | 64 +++++ .../src/headSystem/init.cpp | 63 +++++ .../src/headSystem/stoneDispencer.cpp | 19 ++ .../src/headSystem/z-axis.cpp | 50 ++++ .../firmware-stone-dispencer/src/main.cpp | 45 ++++ .../.$Vacum-Control-Circuit.drawio.bkp | 207 +++++++++++++++++ .../circuit/Vacum-Control-Circuit.drawio | 95 ++++---- 18 files changed, 1316 insertions(+), 64 deletions(-) create mode 100644 stone-dispencer-control/.$Stone-Dispencer-Circuit.drawio.bkp create mode 100644 stone-dispencer-control/Stone-Dispencer-Circuit.drawio create mode 100644 stone-dispencer-control/firmware-stone-dispencer/FreeRTOSConfig.h create mode 100644 stone-dispencer-control/firmware-stone-dispencer/cmake/FreeRTOS_Kernel_import.cmake delete mode 100644 stone-dispencer-control/firmware-stone-dispencer/firmware-stone-dispencer.cpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp create mode 160000 stone-dispencer-control/firmware-stone-dispencer/lib/FreeRTOS-Kernel create mode 100644 stone-dispencer-control/firmware-stone-dispencer/src/headSystem/chopper.cpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/src/headSystem/init.cpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/src/headSystem/stoneDispencer.cpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/src/headSystem/z-axis.cpp create mode 100644 stone-dispencer-control/firmware-stone-dispencer/src/main.cpp create mode 100644 vacum-control/circuit/.$Vacum-Control-Circuit.drawio.bkp 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 @@ - + - + - + - + - + - + - + - + - + - + + + + + + + + + +