diff --git a/.gitignore b/.gitignore index c3f9c57..d033bf8 100644 --- a/.gitignore +++ b/.gitignore @@ -10,9 +10,15 @@ dependencies.lock **/.cache +# VS-code +settings.json + + # drawio *.dtmp *.bkp +# diagrams are mostly temporary (pdf files are tracked) +*.drawio # React diff --git a/README.md b/README.md index ba64f5e..5d1d96e 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,61 @@ +# Overview Firmware for a homemade automated electric armchair. -More details about this project: -V1: https://pfusch.zone/electric-armchair -V2: https://pfusch.zone/electric-armchair-v2 +Extensive details about this project can be found on the website: +- ~~V1: [Electric Armchair V1](https://pfusch.zone/electric-armchair)~~ +- V2: [Electric Armchair V2](https://pfusch.zone/electric-armchair-v2) + +Note: In the current version V2.2, only the esp-project in the [board_single/](board_single) folder and the custom libraries in [common/](common) are used. +The projects in the folders `board_control/` and `board_motorctl/` are no longer compatible and legacy from V2.1. + +Photo machine + +*Photo of the built frame that carries the armchair* + +## Hardware Setup / Electrical +### PCB +The firmware in this repository is designed for an ESP32 microcontroller integrated into a custom PCB developed here: [Project Work 2020](https://pfusch.zone/project-work-2020) + +### Connection Plan +A detailed diagram illustrating all components and wiring can be found in the file [connection-plan.drawio.pdf](connection-plan.drawio.pdf) + +For more details refer to the documentation on the website. + + +## Current Features +- Control Modes: + - Joystick: Control via hardware joystick mounted on the right armrest + - HTTP: Control via virtual joystick on a web interface + - Massage: Armchair shaking depending on stick position + - Auto: Execute stored driving commands sequentially +- Electric Chair Adjustment: Leg and backrest control via joystick +- Advanced Motor Control: Configurable motor fading (acceleration, deceleration limit), current limit, braking; compatible with different hardware +- Wi-Fi: + - Hosts wireless network + - Webserver with webroot in SPIFFS + - HTTP API for controlling the chair +- UART Communication between 2 boards (V2.1) +- Speed Measurement: Measures speed and direction of each tire individually using custom encoders +- Current Measurement: Monitors current of each motor +- Battery Capacity: Measures battery voltage and calculates percentage according to discharge curve +- Fan Control: Cooling fan for motor driver activated only when needed +- Display + Rotary encoder: + - Various status screens showing battery status, speed, RPM, motor current, mode, power, duty cycle, stick data + - Menu for setting various options using encoder (options are stored persistently in nvs flash) + - Menu for selecting the control mode +- Buzzer: Provides acoustic feedback when switching modes or interacting with menu + +## Planned Features +- More Sensors: + - Accelerometer + - Lidar sensor / collision detection + - GPS receiver + - Temperature sensors +- Anti-Slip Regulation +- Self-Driving Algorithm +- Lights +- Improved Web Interface +- App +- Camera @@ -19,7 +73,7 @@ yay -S esp-idf #alternatively clone the esp-idf repository from github git clone git@github.com:Jonny999999/armchair_fw ``` ### Instal node packages -For the react app packages have to be installed with npm TODO: add this to cmake? +For the react app packages have to be installed using npm. TODO: add this to cmake? ``` cd react-app npm install @@ -28,11 +82,12 @@ npm install # Building the Project -## react-webapp -For the webapp to work on the esp32 it has to be built. -When flashing, the folder react-app/build is flashed to siffs (which is used as webroot) onto the esp32. -The following command builds the react webapp and creates this folder -TODO: add this to flash target with cmake? +## React-webapp +When flashing to the ESP32, the files in the `react-app/build/` folder are written to a SPIFFS partition. +These files are then served via HTTP in the Wi-Fi network "armchair" created by the ESP32. +In HTTP control mode, you can control the armchair using a joystick on the provided website. + +Initially, or when changing the React code, you need to manually build the React app: ```bash cd react-app #compile @@ -42,7 +97,8 @@ rm build/static/js/main.8f9aec76.js.LICENSE.txt ``` Note: Use `npm start` for starting the webapp locally for testing -## esp project + +## Firmware ### Set up environment ```bash source /opt/esp-idf/export.sh @@ -65,84 +121,52 @@ idf.py flash ``` - once "connecting...' was successfully, BOOT button can be released + ### Monitor -- connect FTDI programmer to board (VCC to VCC; TX to RX; RX to TX) -- press REST and BOOT button -- release RESET button (keep pressing boot) -- run monitor command: +To view log output for debugging, follow the same steps as in the Upload section, but run: ```bash idf.py monitor ``` -- once connected release BOOT button -- press RESET button once for restart -# Hardware setup -## pcb -Used pcb developed in this project: https://pfusch.zone/project-work-2020 - -## connection plan -A diagram which shows what components are connected to which terminals of the pcb exists here: -[connection-plan.drawio.pdf](connection-plan.drawio.pdf) +# Usage / User Interface -# Planned Features -- More sensors: - - Accelerometer - - Lidar sensor - - GPS receiver -- Anti slip regulation -- Self driving algorithm -- Lights -- Improved webinterface -- App +## Encoder Functions +**When not in MENU mode**, the button (encoder click) has the following functions: +| Count | Type | Action | Description | +|-------|---------------|----------------------|---------------------------------------------------------------------------------------------| +| 1x long | switch mode | **MENU_MODE_SELECT** | Open menu for selecting the current control mode | +| 1x | control | [MASSAGE] **freeze** input | When in massage mode: lock or unlock joystick input at current position. | +| 1x short, 1x long | switch mode | **ADJUST-CHAIR** | Switch to mode where the armchair leg and backrest are controlled via joystick. | +| 2x | toggle mode | **IDLE** <=> previous| Enable/disable chair armchair (e.g., enable after startup or switch to previous mode after timeout). | +| 3x | switch mode | **JOYSTICK** | Switch to JOYSTICK mode, to control armchair using joystick (default). | +| 4x | switch mode | **HTTP** | Switch to **remote control** via web-app `http://191.168.4.1` in wifi `armchair`. | +| 5x | switch mode | **MENU_SETTINGS** | Open menu to set various options, controlled via display and rotary encoder. | +| 6x | switch mode | **MASSAGE** | Switch to MASSAGE mode where armchair shakes differently, depending on joystick position. | +| 7x | | | | +| 8x | toggle option| **deceleration limit** | Disable/enable deceleration limit (default on) => more responsive. | +| 12x | toggle option| **alt stick mapping** | Toggle between default and alternative stick mapping (reverse direction swapped). | -# Todo -**Add switch functions** -- set loglevel -- define max-speed +**When in MENU_SETTINGS mode** (5x click), the encoder controls the settings menu: (similar in MENU_MODE_SELECT) +| Encoder Event | Current Menu | Action | +|---------------|--------------|--------------------------------------------------------------| +| long press | main-menu | Exit MENU mode to previous control mode (e.g., JOYSTICK). | +| long press | value-select | Exit to main-menu without changing the value. | +| click | main-menu | Select currently highlighted menu item -> enter value-select screen. | +| click | value-select | Confirm value / run action. | +| rotate | main-menu | Scroll through menu items. | +| rotate | value-select | Change value. | +## HTTP Mode +Control the armchair via a virtual joystick on the web interface. -# Usage -## Switch functions -**Currently implemented** -| Count | Type | Action | Description | -| --- | --- | --- | --- | -| 1x | configure | [JOYSTICK] **calibrate stick** | when in joystick mode: set joystick center to current joystick pos | -| 1x | control | [MASSAGE] **freeze** input | when in massage mode: lock or unlock joystick input at current position | -| 2x | toggle mode | **IDLE** <=> previous | enable/disable chair armchair e.g. enable after startup or timeout | -| 3x | switch mode | **JOYSTICK** | switch to default mode JOYSTICK | -| 4x | toggle mode | **HTTP** <=> JOYSTICK | switch to '**remote control** via web-app `http://191.168.4.1`' or back to JOYSTICK mode | -| 5x | | | | -| 6x | toggle mode | **MASSAGE** <=> JOYSTICK | switch to MASSAGE mode or back to JOYSTICK mode | -| 7x | | | | -| 8x | toggle option | **deceleration limit** | disable/enable deceleration limit (default on) => more responsive | -| | | | | -| 12x | toggle option | **alt stick mapping** | toggle between default and alternative stick mapping (reverse swapped) | -| >1s | system | **restart** | Restart the controller when pressing the button longer than 1 second | -| 1x short, 1x long | auto command | **eject** foot support | automatically go forward and reverse for certain time with no acceleration limits, so foot support ejects | - - -## HTTP mode -Control armchair via virtual joystick on a webinterface. - -**Usage** -- Connect to wifi `armchar`, no password -- Access http://192.168.4.1 (note: **http** NOT https, some browsers automatically add https!) - -**Current Features** -- Control direction and speed with joystick - -**Todo** -- Set parameters - - max duty - - max current -- Control other modes e.g. massage -- Execute preset movement commands -- Change seating position -also see github issue +**Usage:** +- Switch to HTTP mode (4 button presses). +- Connect to WiFi `armchar`, no password. +- Access http://192.168.4.1 (note: **http** NOT https, some browsers automatically add https!). \ No newline at end of file diff --git a/board_control/main/CMakeLists.txt b/board_control/main/CMakeLists.txt index 575bfc2..3ed4d7e 100644 --- a/board_control/main/CMakeLists.txt +++ b/board_control/main/CMakeLists.txt @@ -6,6 +6,7 @@ idf_component_register( "button.cpp" "auto.cpp" "uart.cpp" + "encoder.cpp" INCLUDE_DIRS "." ) diff --git a/board_control/main/encoder.cpp b/board_control/main/encoder.cpp new file mode 100644 index 0000000..887335a --- /dev/null +++ b/board_control/main/encoder.cpp @@ -0,0 +1,81 @@ + #include "encoder.h" +extern "C" +{ +#include +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "esp_log.h" +} + +#include "encoder.hpp" + +//------------------------- +//------- variables ------- +//------------------------- +static const char * TAG = "encoder"; +uint16_t encoderCount; +rotary_encoder_btn_state_t encoderButtonState = {}; +QueueHandle_t encoderQueue = NULL; + +//encoder config +rotary_encoder_t encoderConfig = { + .pin_a = PIN_A, + .pin_b = PIN_B, + .pin_btn = PIN_BUTTON, + .code = 1, + .store = encoderCount, + .index = 0, + .btn_pressed_time_us = 20000, + .btn_state = encoderButtonState +}; + + + +//================================== +//========== encoder_init ========== +//================================== +//initialize encoder +void encoder_init(){ + encoderQueue = xQueueCreate(QUEUE_SIZE, sizeof(rotary_encoder_event_t)); + rotary_encoder_init(encoderQueue); + rotary_encoder_add(&encoderConfig); +} + + + +//================================== +//========== task_encoder ========== +//================================== +//receive and handle encoder events +void task_encoder(void *arg) { + rotary_encoder_event_t ev; //store event data + while (1) { + if (xQueueReceive(encoderQueue, &ev, portMAX_DELAY)) { + //log enocder events + switch (ev.type){ + case RE_ET_CHANGED: + ESP_LOGI(TAG, "Event type: RE_ET_CHANGED, diff: %d", ev.diff); + break; + case RE_ET_BTN_PRESSED: + ESP_LOGI(TAG, "Button pressed"); + break; + case RE_ET_BTN_RELEASED: + ESP_LOGI(TAG, "Button released"); + break; + case RE_ET_BTN_CLICKED: + ESP_LOGI(TAG, "Button clicked"); + break; + case RE_ET_BTN_LONG_PRESSED: + ESP_LOGI(TAG, "Button long-pressed"); + break; + default: + ESP_LOGW(TAG, "Unknown event type"); + break; + } + } + } +} + diff --git a/board_control/main/encoder.hpp b/board_control/main/encoder.hpp new file mode 100644 index 0000000..18881ed --- /dev/null +++ b/board_control/main/encoder.hpp @@ -0,0 +1,18 @@ +extern "C" { +#include "freertos/FreeRTOS.h" // FreeRTOS related headers +#include "freertos/task.h" +#include "encoder.h" +} + +//config +#define QUEUE_SIZE 10 +#define PIN_A GPIO_NUM_25 +#define PIN_B GPIO_NUM_26 +#define PIN_BUTTON GPIO_NUM_27 + + +//init encoder with config in encoder.cpp +void encoder_init(); + +//task that handles encoder events +void task_encoder(void *arg); diff --git a/board_control/main/main.cpp b/board_control/main/main.cpp index 260310a..5537cf9 100644 --- a/board_control/main/main.cpp +++ b/board_control/main/main.cpp @@ -18,6 +18,7 @@ extern "C" #include "uart.hpp" +#include "encoder.hpp" //========================= @@ -28,6 +29,13 @@ extern "C" //#define UART_TEST_ONLY +//========================= +//====== encoder TEST ===== +//========================= +//only start encoder task +#define ENCODER_TEST_ONLY + + //tag for logging static const char * TAG = "main"; @@ -157,7 +165,7 @@ void setLoglevels(void){ //=========== app_main ============ //================================= extern "C" void app_main(void) { -#ifndef UART_TEST_ONLY +#if !defined(ENCODER_TEST_ONLY) && !defined(UART_TEST_ONLY) //enable 5V volate regulator gpio_pad_select_gpio(GPIO_NUM_17); gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); @@ -214,24 +222,35 @@ extern "C" void app_main(void) { // vTaskDelay(2000 / portTICK_PERIOD_MS); // ESP_LOGI(TAG, "initializing http server"); // http_init_server(); - - #endif + //------------------------------------------- //--- create tasks for uart communication --- //------------------------------------------- - +#ifndef ENCODER_TEST_ONLY uart_init(); xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL); xTaskCreate(task_uartSend, "task_uartSend", 4096, NULL, 10, NULL); +#endif + + + //-------------------------------------------- + //----- create task that handles encoder ----- + //-------------------------------------------- +#ifndef UART_TEST_ONLY + encoder_init(); + xTaskCreate(task_encoder, "task_encoder", 4096, NULL, 10, NULL); +#endif + + //--- main loop --- //does nothing except for testing things //--- testing force http mode after startup --- vTaskDelay(5000 / portTICK_PERIOD_MS); - control.changeMode(controlMode_t::HTTP); + //control.changeMode(controlMode_t::HTTP); while(1){ vTaskDelay(1000 / portTICK_PERIOD_MS); //--------------------------------- diff --git a/board_single/CMakeLists.txt b/board_single/CMakeLists.txt index e827903..5cb41af 100644 --- a/board_single/CMakeLists.txt +++ b/board_single/CMakeLists.txt @@ -7,3 +7,6 @@ cmake_minimum_required(VERSION 3.5) include($ENV{IDF_PATH}/tools/cmake/project.cmake) set(EXTRA_COMPONENT_DIRS "../components ../common") project(armchair-singleBoard) + +# colored build output (errors, warnings...) +idf_build_set_property(COMPILE_OPTIONS "-fdiagnostics-color=always" APPEND) \ No newline at end of file diff --git a/board_single/main/CMakeLists.txt b/board_single/main/CMakeLists.txt index d0ac056..52ed876 100644 --- a/board_single/main/CMakeLists.txt +++ b/board_single/main/CMakeLists.txt @@ -1,12 +1,13 @@ idf_component_register( SRCS "main.cpp" - "config.cpp" "control.cpp" "button.cpp" "fan.cpp" "auto.cpp" "display.cpp" + "menu.cpp" + "encoder.cpp" INCLUDE_DIRS "." ) diff --git a/board_single/main/auto.cpp b/board_single/main/auto.cpp index 4cf4a71..a3f2d3c 100644 --- a/board_single/main/auto.cpp +++ b/board_single/main/auto.cpp @@ -1,5 +1,4 @@ #include "auto.hpp" -#include "config.hpp" //tag for logging static const char * TAG = "automatedArmchair"; @@ -8,9 +7,12 @@ static const char * TAG = "automatedArmchair"; //============================= //======== constructor ======== //============================= -automatedArmchair::automatedArmchair(void) { - //create command queue - commandQueue = xQueueCreate( 32, sizeof( commandSimple_t ) ); //TODO add max size to config? +automatedArmchair_c::automatedArmchair_c(controlledMotor *motorLeft_f, controlledMotor *motorRight_f) +{ + motorLeft = motorLeft_f; + motorRight = motorRight_f; + // create command queue + commandQueue = xQueueCreate(32, sizeof(commandSimple_t)); // TODO add max size to config? } @@ -18,7 +20,7 @@ automatedArmchair::automatedArmchair(void) { //============================== //====== generateCommands ====== //============================== -motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruction) { +motorCommands_t automatedArmchair_c::generateCommands(auto_instruction_t * instruction) { //reset instruction *instruction = auto_instruction_t::NONE; //check if previous command is finished @@ -29,10 +31,10 @@ motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruc //copy instruction to be provided to control task *instruction = cmdCurrent.instruction; //set acceleration / fading parameters according to command - motorLeft.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); - motorRight.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); - motorLeft.setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); - motorRight.setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); + motorLeft->setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); + motorRight->setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); + motorLeft->setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); + motorRight->setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); //calculate timestamp the command is finished timestampCmdFinished = esp_log_timestamp() + cmdCurrent.msDuration; //copy the new commands @@ -55,7 +57,7 @@ motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruc //======== addCommand ======== //============================ //function that adds a basic command to the queue -void automatedArmchair::addCommand(commandSimple_t command) { +void automatedArmchair_c::addCommand(commandSimple_t command) { //add command to queue if ( xQueueSend( commandQueue, ( void * )&command, ( TickType_t ) 0 ) ){ ESP_LOGI(TAG, "Successfully inserted command to queue"); @@ -64,7 +66,7 @@ void automatedArmchair::addCommand(commandSimple_t command) { } } -void automatedArmchair::addCommands(commandSimple_t commands[], size_t count) { +void automatedArmchair_c::addCommands(commandSimple_t commands[], size_t count) { for (int i = 0; i < count; i++) { ESP_LOGI(TAG, "Reading command no. %d from provided array", i); addCommand(commands[i]); @@ -77,7 +79,7 @@ void automatedArmchair::addCommands(commandSimple_t commands[], size_t count) { //=============================== //function that deletes all pending/queued commands //e.g. when switching modes -motorCommands_t automatedArmchair::clearCommands() { +motorCommands_t automatedArmchair_c::clearCommands() { //clear command queue xQueueReset( commandQueue ); ESP_LOGW(TAG, "command queue was successfully emptied"); diff --git a/board_single/main/auto.hpp b/board_single/main/auto.hpp index 8b799fb..4cf66d8 100644 --- a/board_single/main/auto.hpp +++ b/board_single/main/auto.hpp @@ -33,13 +33,13 @@ typedef struct commandSimple_t{ //------------------------------------ -//----- automatedArmchair class ----- +//----- automatedArmchair_c class ----- //------------------------------------ -class automatedArmchair { +class automatedArmchair_c { public: //--- methods --- //constructor - automatedArmchair(void); + automatedArmchair_c(controlledMotor * motorLeft, controlledMotor * motorRight); //function to generate motor commands //can be also seen as handle function //TODO: go with other approach: separate task for handling auto mode @@ -62,6 +62,8 @@ class automatedArmchair { private: //--- methods --- //--- objects --- + controlledMotor * motorLeft; + controlledMotor * motorRight; //TODO: add buzzer here //--- variables --- //queue for storing pending commands @@ -85,3 +87,50 @@ class automatedArmchair { }; +//=========== EXAMPLE USAGE ============ +//the following was once used in button.cpp to make move that ejects the leg support of armchair v1 +/** +if (trigger){ + //define commands + cmds[0] = + { + .motorCmds = { + .left = {motorstate_t::REV, 90}, + .right = {motorstate_t::REV, 90} + }, + .msDuration = 1200, + .fadeDecel = 800, + .fadeAccel = 1300, + .instruction = auto_instruction_t::NONE + }; + cmds[1] = + { + .motorCmds = { + .left = {motorstate_t::FWD, 70}, + .right = {motorstate_t::FWD, 70} + }, + .msDuration = 70, + .fadeDecel = 0, + .fadeAccel = 300, + .instruction = auto_instruction_t::NONE + }; + cmds[2] = + { + .motorCmds = { + .left = {motorstate_t::IDLE, 0}, + .right = {motorstate_t::IDLE, 0} + }, + .msDuration = 10, + .fadeDecel = 800, + .fadeAccel = 1300, + .instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE + }; + + //send commands to automatedArmchair_c command queue + armchair.addCommands(cmds, 3); + + //change mode to AUTO + control->changeMode(controlMode_t::AUTO); + return; +} +*/ diff --git a/board_single/main/button.cpp b/board_single/main/button.cpp index 0974fdc..8b76858 100644 --- a/board_single/main/button.cpp +++ b/board_single/main/button.cpp @@ -8,139 +8,145 @@ extern "C" } #include "button.hpp" +#include "encoder.hpp" +#include "display.hpp" +// tag for logging +static const char *TAG = "button"; - -//tag for logging -static const char * TAG = "button"; - - +//====================================== +//============ button task ============= +//====================================== +// task that handles the button interface/commands +void task_button(void *task_button_parameters) +{ + task_button_parameters_t *objects = (task_button_parameters_t *)task_button_parameters; + ESP_LOGI(TAG, "Initializing command-button and starting handle loop"); + // create button instance + buttonCommands commandButton(objects->control, objects->joystick, objects->encoderQueue, objects->motorLeft, objects->motorRight, objects->buzzer); + // start handle loop + commandButton.startHandleLoop(); +} //----------------------------- //-------- constructor -------- //----------------------------- -buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, evaluatedJoystick * joystick_f, controlledArmchair * control_f, buzzer_t * buzzer_f, controlledMotor * motorLeft_f, controlledMotor * motorRight_f){ - //copy object pointers - button = button_f; - joystick = joystick_f; +buttonCommands::buttonCommands( + controlledArmchair *control_f, + evaluatedJoystick *joystick_f, + QueueHandle_t encoderQueue_f, + controlledMotor *motorLeft_f, + controlledMotor *motorRight_f, + buzzer_t *buzzer_f) +{ + // copy object pointers control = control_f; - buzzer = buzzer_f; + joystick = joystick_f; + encoderQueue = encoderQueue_f; motorLeft = motorLeft_f; motorRight = motorRight_f; - //TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)? + buzzer = buzzer_f; + // TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)? } - - //---------------------------- //--------- action ----------- //---------------------------- //function that runs commands depending on a count value void buttonCommands::action (uint8_t count, bool lastPressLong){ - //--- variable declarations --- + //--- variables --- bool decelEnabled; //for different beeping when toggling - commandSimple_t cmds[8]; //array for commands for automatedArmchair + commandSimple_t cmds[8]; //array for commands for automatedArmchair_c //--- get joystick position --- + //in case joystick is used for additional cases: //joystickData_t stickData = joystick->getData(); - //--- actions based on count --- - switch (count){ - //no such command - default: - ESP_LOGE(TAG, "no command for count=%d defined", count); - buzzer->beep(3, 400, 100); - break; + //--- run actions based on count --- + switch (count) + { + // ## no command ## + default: + ESP_LOGE(TAG, "no command for count=%d and long=%d defined", count, lastPressLong); + buzzer->beep(3, 200, 100); + break; - case 1: - //restart contoller when 1x long pressed - if (lastPressLong){ - ESP_LOGW(TAG, "RESTART"); - buzzer->beep(1,1000,1); - vTaskDelay(500 / portTICK_PERIOD_MS); - //esp_restart(); - //-> define joystick center or toggle freeze input (executed in control task) - control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed - return; - } - //note: disabled joystick calibration due to accidential trigger -// -// ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); -// //-> define joystick center or toggle freeze input (executed in control task) -// control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed - break; - case 2: - //run automatic commands to lift leg support when pressed 1x short 1x long - if (lastPressLong){ - //define commands - cmds[0] = - { - .motorCmds = { - .left = {motorstate_t::REV, 90}, - .right = {motorstate_t::REV, 90} - }, - .msDuration = 1200, - .fadeDecel = 800, - .fadeAccel = 1300, - .instruction = auto_instruction_t::NONE - }; - cmds[1] = - { - .motorCmds = { - .left = {motorstate_t::FWD, 70}, - .right = {motorstate_t::FWD, 70} - }, - .msDuration = 70, - .fadeDecel = 0, - .fadeAccel = 300, - .instruction = auto_instruction_t::NONE - }; - cmds[2] = - { - .motorCmds = { - .left = {motorstate_t::IDLE, 0}, - .right = {motorstate_t::IDLE, 0} - }, - .msDuration = 10, - .fadeDecel = 800, - .fadeAccel = 1300, - .instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE - }; + case 1: + // ## switch to MENU_SETTINGS state ## + if (lastPressLong) + { + ESP_LOGW(TAG, "1x long press -> clear encoder queue and change to mode 'menu mode select'"); + buzzer->beep(5, 50, 30); + // clear encoder event queue (prevent menu from exiting immediately due to long press event just happend) + vTaskDelay(200 / portTICK_PERIOD_MS); + //TODO move encoder queue clear to changeMode() method? + rotary_encoder_event_t ev; + while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS); + control->changeMode(controlMode_t::MENU_MODE_SELECT); + } + // ## toggle joystick freeze ## + else if (control->getCurrentMode() == controlMode_t::MASSAGE) + { + control->toggleFreezeInputMassage(); + } + // ## define joystick center ## + else + { + // note: disabled joystick calibration due to accidential trigger + //joystick->defineCenter(); + } + break; - //send commands to automatedArmchair command queue - armchair.addCommands(cmds, 3); - - //change mode to AUTO - control->changeMode(controlMode_t::AUTO); - return; - } - - //toggle idle when 2x pressed + case 2: + // ## switch to ADJUST_CHAIR mode ## + if (lastPressLong) + { + ESP_LOGW(TAG, "cmd %d: switch to ADJUST_CHAIR", count); + control->changeMode(controlMode_t::ADJUST_CHAIR); + } + // ## toggle IDLE ## + else + { ESP_LOGW(TAG, "cmd %d: toggle IDLE", count); - control->toggleIdle(); //toggle between idle and previous/default mode - break; - + control->toggleIdle(); // toggle between idle and previous/default mode + } + break; case 3: + // ## switch to JOYSTICK mode ## ESP_LOGW(TAG, "cmd %d: switch to JOYSTICK", count); control->changeMode(controlMode_t::JOYSTICK); //switch to JOYSTICK mode break; case 4: - ESP_LOGW(TAG, "cmd %d: toggle between HTTP and JOYSTICK", count); - control->toggleModes(controlMode_t::HTTP, controlMode_t::JOYSTICK); //toggle between HTTP and JOYSTICK mode + // ## switch to HTTP mode ## + ESP_LOGW(TAG, "cmd %d: switch to HTTP", count); + control->changeMode(controlMode_t::HTTP); //switch to HTTP mode + break; + + case 5: + // ## switch to MENU_SETTINGS state ## + ESP_LOGW(TAG, "5x press -> clear encoder queue and change to mode 'menu settings'"); + buzzer->beep(20, 20, 10); + vTaskDelay(200 / portTICK_PERIOD_MS); + // clear encoder event queue (prevent menu from using previous events) + rotary_encoder_event_t ev; + while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS); + control->changeMode(controlMode_t::MENU_SETTINGS); break; case 6: - ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count); - control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode + // ## switch to MASSAGE mode ## + ESP_LOGW(TAG, "switch to MASSAGE"); + control->changeMode(controlMode_t::MASSAGE); //switch to MASSAGE mode break; case 8: + // ## toggle "sport-mode" ## //toggle deceleration fading between on and off //decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL); //motorRight->toggleFade(fadeType_t::DECEL); - decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL); + decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL); //TODO remove/simplify this using less functions motorRight->toggleFade(fadeType_t::ACCEL); ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled); if (decelEnabled){ @@ -151,12 +157,10 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){ break; case 12: - ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); - //-> toggle altStickMapping (executed in control task) - control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1)? + // ## toggle alternative stick mapping ## + control->toggleAltStickMapping(); break; - - } + } } @@ -165,56 +169,78 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){ //----------------------------- //------ startHandleLoop ------ //----------------------------- -//this function has to be started once in a separate task -//repeatedly evaluates and processes button events then takes the corresponding action -void buttonCommands::startHandleLoop() { +// when not in MENU_SETTINGS mode, repeatedly receives events from encoder button +// and takes the corresponding action +// this function has to be started once in a separate task +#define INPUT_TIMEOUT 500 // duration of no button events, after which action is run (implicitly also is 'long-press' time) +void buttonCommands::startHandleLoop() +{ + //-- variables -- + bool isPressed = false; + static rotary_encoder_event_t event; // store event data + // int count = 0; (from class) - while(1) { - vTaskDelay(20 / portTICK_PERIOD_MS); - //run handle function of evaluatedSwitch object - button->handle(); - - //--- count button presses and run action --- - switch(state) { - case inputState_t::IDLE: //wait for initial button press - if (button->risingEdge) { - count = 1; - buzzer->beep(1, 65, 0); - timestamp_lastAction = esp_log_timestamp(); - state = inputState_t::WAIT_FOR_INPUT; - ESP_LOGI(TAG, "first button press detected -> waiting for further events"); - } - break; - - case inputState_t::WAIT_FOR_INPUT: //wait for further presses - //button pressed again - if (button->risingEdge){ - count++; - buzzer->beep(1, 65, 0); - timestamp_lastAction = esp_log_timestamp(); - ESP_LOGI(TAG, "another press detected -> count=%d -> waiting for further events", count); - } - //timeout - else if (esp_log_timestamp() - timestamp_lastAction > 1000) { - state = inputState_t::IDLE; - buzzer->beep(count, 50, 50); - //TODO: add optional "bool wait" parameter to beep function to delay until finished beeping - ESP_LOGI(TAG, "timeout - running action function for count=%d", count); - //--- run action function --- - //check if still pressed - bool lastPressLong = false; - if (button->state == true){ - //run special case when last press was longer than timeout - lastPressLong = true; - } - //run action function with current count of button presses - action(count, lastPressLong); - } - break; + while (1) + { + //-- disable functionality when in menu mode -- + //(display task uses encoder in that mode) + if (control->getCurrentMode() == controlMode_t::MENU_SETTINGS + || control->getCurrentMode() == controlMode_t::MENU_MODE_SELECT) + { + //do nothing every loop cycle + ESP_LOGD(TAG, "in MENU_SETTINGS or MENU_MODE_SELECT mode -> button commands disabled"); + vTaskDelay(1000 / portTICK_PERIOD_MS); + continue; } - } -} - - - + //-- get events from encoder -- + if (xQueueReceive(encoderQueue, &event, INPUT_TIMEOUT / portTICK_PERIOD_MS)) + { + control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + break; + case RE_ET_BTN_PRESSED: + ESP_LOGD(TAG, "Button pressed"); + buzzer->beep(1, 65, 0); + isPressed = true; + count++; // count each pressed event + break; + case RE_ET_BTN_RELEASED: + ESP_LOGD(TAG, "Button released"); + isPressed = false; // rest stored state + break; + case RE_ET_CHANGED: // scroll through status pages when simply rotating encoder + if (event.diff > 0) + { + display_rotateStatusPage(true, true); //select NEXT status screen, stau at last element (dont rotate to first) + buzzer->beep(1, 65, 0); + } + else + { + display_rotateStatusPage(false, true); //select PREVIOUS status screen, stay at first element (dont rotate to last) + buzzer->beep(1, 65, 0); + } + break; + case RE_ET_BTN_LONG_PRESSED: + case RE_ET_BTN_CLICKED: + default: + break; + } + } + else // timeout (no event received within TIMEOUT) + { + if (count > 0) + { + //-- run action with count of presses -- + ESP_LOGI(TAG, "timeout: count=%d, lastPressLong=%d -> running action", count, isPressed); + buzzer->beep(count, 50, 50, 200); //beep count, with 200ms gap before next queued beeps can start + action(count, isPressed); // run action - if currently still on the last press is considered long + count = 0; // reset count + } + else { + ESP_LOGD(TAG, "no button event received in this cycle (count=0)"); + } + } //end queue + } //end while(1) +} //end function diff --git a/board_single/main/button.hpp b/board_single/main/button.hpp index 09de729..8b5e523 100644 --- a/board_single/main/button.hpp +++ b/board_single/main/button.hpp @@ -5,7 +5,6 @@ #include "control.hpp" #include "motorctl.hpp" #include "auto.hpp" -#include "config.hpp" #include "joystick.hpp" @@ -17,14 +16,13 @@ class buttonCommands { public: //--- constructor --- - buttonCommands ( - gpio_evaluatedSwitch * button_f, - evaluatedJoystick * joystick_f, - controlledArmchair * control_f, - buzzer_t * buzzer_f, - controlledMotor * motorLeft_f, - controlledMotor * motorRight_f - ); + buttonCommands( + controlledArmchair *control_f, + evaluatedJoystick *joystick_f, + QueueHandle_t encoderQueue_f, + controlledMotor * motorLeft_f, + controlledMotor *motorRight_f, + buzzer_t *buzzer_f); //--- functions --- //the following function has to be started once in a separate task. @@ -36,12 +34,12 @@ class buttonCommands { void action(uint8_t count, bool lastPressLong); //--- objects --- - gpio_evaluatedSwitch* button; - evaluatedJoystick* joystick; controlledArmchair * control; - buzzer_t* buzzer; + evaluatedJoystick* joystick; controlledMotor * motorLeft; controlledMotor * motorRight; + buzzer_t* buzzer; + QueueHandle_t encoderQueue; //--- variables --- uint8_t count = 0; @@ -51,3 +49,21 @@ class buttonCommands { }; + + +//====================================== +//============ button task ============= +//====================================== +// struct with variables passed to task from main +typedef struct task_button_parameters_t +{ + controlledArmchair *control; + evaluatedJoystick *joystick; + QueueHandle_t encoderQueue; + controlledMotor *motorLeft; + controlledMotor *motorRight; + buzzer_t *buzzer; +} task_button_parameters_t; + +//task that handles the button interface/commands +void task_button( void * task_button_parameters ); \ No newline at end of file diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index ef53343..a5bb2b5 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -1,8 +1,66 @@ -#include "config.hpp" +// NOTE: this file is included in main.cpp only. +// outsourced all configuration related functions and structures to this file: -//=================================== -//======= motor configuration ======= -//=================================== +extern "C" +{ +#include "esp_log.h" +} +#include "motordrivers.hpp" +#include "motorctl.hpp" +#include "joystick.hpp" +#include "http.hpp" +#include "speedsensor.hpp" +#include "buzzer.hpp" +#include "control.hpp" +#include "fan.hpp" +#include "auto.hpp" +#include "chairAdjust.hpp" +#include "display.hpp" +#include "encoder.h" + +//================================== +//======== define loglevels ======== +//================================== +void setLoglevels(void) +{ + // set loglevel for all tags: + esp_log_level_set("*", ESP_LOG_WARN); + + //--- set loglevel for individual tags --- + esp_log_level_set("main", ESP_LOG_INFO); + esp_log_level_set("buzzer", ESP_LOG_ERROR); + // esp_log_level_set("motordriver", ESP_LOG_DEBUG); + esp_log_level_set("motor-control", ESP_LOG_WARN); + // esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); + esp_log_level_set("joystickCommands", ESP_LOG_WARN); + esp_log_level_set("button", ESP_LOG_INFO); + esp_log_level_set("control", ESP_LOG_INFO); + // esp_log_level_set("fan-control", ESP_LOG_INFO); + esp_log_level_set("wifi", ESP_LOG_INFO); + esp_log_level_set("http", ESP_LOG_INFO); + // esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG); + esp_log_level_set("display", ESP_LOG_INFO); + // esp_log_level_set("current-sensors", ESP_LOG_INFO); + esp_log_level_set("speedSensor", ESP_LOG_WARN); + esp_log_level_set("chair-adjustment", ESP_LOG_INFO); + esp_log_level_set("menu", ESP_LOG_INFO); + esp_log_level_set("encoder", ESP_LOG_INFO); + + + + esp_log_level_set("TESTING", ESP_LOG_ERROR); + + + +} + +//================================== +//======== configuration =========== +//================================== + +//----------------------------------- +//------- motor configuration ------- +//----------------------------------- /* ==> currently using other driver //--- configure left motor (hardware) --- single100a_config_t configDriverLeft = { @@ -11,8 +69,8 @@ single100a_config_t configDriverLeft = { .gpio_b = GPIO_NUM_4, .ledc_timer = LEDC_TIMER_0, .ledc_channel = LEDC_CHANNEL_0, - .aEnabledPinState = false, //-> pins inverted (mosfets) - .bEnabledPinState = false, + .aEnabledPinState = false, //-> pins inverted (mosfets) + .bEnabledPinState = false, .resolution = LEDC_TIMER_11_BIT, .pwmFreq = 10000 }; @@ -24,176 +82,192 @@ single100a_config_t configDriverRight = { .gpio_b = GPIO_NUM_14, .ledc_timer = LEDC_TIMER_1, .ledc_channel = LEDC_CHANNEL_1, - .aEnabledPinState = false, //-> pin inverted (mosfet) - .bEnabledPinState = true, //-> not inverted (direct) + .aEnabledPinState = false, //-> pin inverted (mosfet) + .bEnabledPinState = true, //-> not inverted (direct) .resolution = LEDC_TIMER_11_BIT, .pwmFreq = 10000 - }; - */ + }; + */ //--- configure sabertooth driver --- (controls both motors in one instance) sabertooth2x60_config_t sabertoothConfig = { - .gpio_TX = GPIO_NUM_25, - .uart_num = UART_NUM_2 -}; + .gpio_TX = GPIO_NUM_27, + .uart_num = UART_NUM_2}; - -//TODO add motor name string -> then use as log tag? +// TODO add motor name string -> then use as log tag? //--- configure left motor (contol) --- motorctl_config_t configMotorControlLeft = { - .msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%) - .msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%) - .currentLimitEnabled = false, - .currentSensor_adc = ADC1_CHANNEL_4, //GPIO32 - .currentSensor_ratedCurrent = 50, + .name = "left", + .loggingEnabled = true, + .msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%) + .msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%) + .currentLimitEnabled = false, + .tractionControlSystemEnabled = false, + .currentSensor_adc = ADC1_CHANNEL_4, // GPIO32 + .currentSensor_ratedCurrent = 50, .currentMax = 30, - .deadTimeMs = 0 //minimum time motor is off between direction change + .currentInverted = true, + .currentSnapToZeroThreshold = 0.15, + .deadTimeMs = 0, // minimum time motor is off between direction change + .brakePauseBeforeResume = 1500, + .brakeDecel = 400, }; //--- configure right motor (contol) --- motorctl_config_t configMotorControlRight = { - .msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%) - .msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%) - .currentLimitEnabled = false, - .currentSensor_adc = ADC1_CHANNEL_5, //GPIO33 - .currentSensor_ratedCurrent = 50, + .name = "right", + .loggingEnabled = false, + .msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%) + .msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%) + .currentLimitEnabled = false, + .tractionControlSystemEnabled = false, + .currentSensor_adc = ADC1_CHANNEL_5, // GPIO33 + .currentSensor_ratedCurrent = 50, .currentMax = 30, - .deadTimeMs = 0 //minimum time motor is off between direction change + .currentInverted = false, + .currentSnapToZeroThreshold = 0.25, + .deadTimeMs = 0, // minimum time motor is off between direction change + .brakePauseBeforeResume = 1500, + .brakeDecel = 400, }; - - -//============================== -//======= control config ======= -//============================== +//------------------------------ +//------- control config ------- +//------------------------------ control_config_t configControl = { - .defaultMode = controlMode_t::JOYSTICK, //default mode after startup and toggling IDLE - //--- timeout --- - .timeoutMs = 3*60*1000, //time of inactivity after which the mode gets switched to IDLE - .timeoutTolerancePer = 5, //percentage the duty can vary between timeout checks considered still inactive - //--- http mode --- - + .defaultMode = controlMode_t::JOYSTICK, // default mode after startup and toggling IDLE + //--- timeouts --- + .timeoutSwitchToIdleMs = 5 * 60 * 1000, // time of inactivity after which the mode gets switched to IDLE + .timeoutNotifyPowerStillOnMs = 6 * 60 * 60 * 1000 // time in IDLE after which buzzer beeps in certain interval (notify "forgot to turn off") }; - - -//=============================== -//===== httpJoystick config ===== -//=============================== +//------------------------------- +//----- httpJoystick config ----- +//------------------------------- httpJoystick_config_t configHttpJoystickMain{ - .toleranceZeroX_Per = 1, //percentage around joystick axis the coordinate snaps to 0 + .toleranceZeroX_Per = 1, // percentage around joystick axis the coordinate snaps to 0 .toleranceZeroY_Per = 6, - .toleranceEndPer = 2, //percentage before joystick end the coordinate snaps to 1/-1 - .timeoutMs = 2500 //time no new data was received before the motors get turned off + .toleranceEndPer = 2, // percentage before joystick end the coordinate snaps to 1/-1 + .timeoutMs = 2500 // time no new data was received before the motors get turned off }; - - -//====================================== -//======= joystick configuration ======= -//====================================== +//-------------------------------------- +//------- joystick configuration ------- +//-------------------------------------- joystick_config_t configJoystick = { - .adc_x = ADC1_CHANNEL_0, //GPIO36 - .adc_y = ADC1_CHANNEL_3, //GPIO39 - //percentage of joystick range the coordinate of the axis snaps to 0 (0-100) - .tolerance_zeroX_per = 7, //6 - .tolerance_zeroY_per = 10, //7 - //percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100) - .tolerance_end_per = 4, - //threshold the radius jumps to 1 before the stick is at max radius (range 0-1) + .adc_x = ADC1_CHANNEL_0, // GPIO36 + .adc_y = ADC1_CHANNEL_3, // GPIO39 + // percentage of joystick range the coordinate of the axis snaps to 0 (0-100) + .tolerance_zeroX_per = 7, // 6 + .tolerance_zeroY_per = 10, // 7 + // percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100) + .tolerance_end_per = 4, + // threshold the radius jumps to 1 before the stick is at max radius (range 0-1) .tolerance_radius = 0.09, - //min and max adc values of each axis, !!!AFTER INVERSION!!! is applied: + // min and max adc values of each axis, !!!AFTER INVERSION!!! is applied: .x_min = 1710, //=> x=-1 .x_max = 2980, //=> x=1 .y_min = 1700, //=> y=-1 .y_max = 2940, //=> y=1 - //invert adc measurement - .x_inverted = true, - .y_inverted = true -}; + // invert adc measurement + .x_inverted = false, + .y_inverted = true}; - - -//============================ -//=== configure fan contol === -//============================ -fan_config_t configCooling = { +//---------------------------- +//--- configure fan contol --- +//---------------------------- +fan_config_t configFans = { .gpio_fan = GPIO_NUM_13, - .dutyThreshold = 40, - .minOnMs = 1500, - .minOffMs = 3000, - .turnOffDelayMs = 5000, + .dutyThreshold = 50, + .minOnMs = 3500, // time motor duty has to be above the threshold for fans to turn on + .minOffMs = 5000, // min time fans have to be off to be able to turn on again + .turnOffDelayMs = 3000, // time fans continue to be on after duty is below threshold }; - - -//============================================ -//======== speed sensor configuration ======== -//============================================ +//-------------------------------------------- +//-------- speed sensor configuration -------- +//-------------------------------------------- speedSensor_config_t speedLeft_config{ - .gpioPin = GPIO_NUM_5, - .degreePerGroup = 360/5, - .tireCircumferenceMeter = 210.0*3.141/1000.0, - .directionInverted = false, - .logName = "speedLeft", + .gpioPin = GPIO_NUM_5, + .degreePerGroup = 360 / 16, + .minPulseDurationUs = 3000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection + //measured wihth scope while tires in the air: + // 5-groups: 12ms + // 16-groups: 3.7ms + .tireCircumferenceMeter = 0.81, + .directionInverted = true, + .logName = "speedLeft" }; speedSensor_config_t speedRight_config{ - .gpioPin = GPIO_NUM_14, - .degreePerGroup = 360/12, - .tireCircumferenceMeter = 210.0*3.141/1000.0, - .directionInverted = true, - .logName = "speedRight", + .gpioPin = GPIO_NUM_14, + .degreePerGroup = 360 / 12, + .minPulseDurationUs = 4000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection + .tireCircumferenceMeter = 0.81, + .directionInverted = false, + .logName = "speedRight" }; -//================================= -//===== create global objects ===== -//================================= -//TODO outsource global variables to e.g. global.cpp and only config options here? -//create sabertooth motor driver instance -sabertooth2x60a sabertoothDriver(sabertoothConfig); +//------------------------- +//-------- display -------- +//------------------------- +display_config_t display_config{ + // hardware initialization + .gpio_scl = GPIO_NUM_22, + .gpio_sda = GPIO_NUM_23, + .gpio_reset = -1, // negative number disables reset feature + .width = 128, + .height = 64, + .offsetX = 2, + .flip = false, + .contrastNormal = 170, // max: 255 + // display task + .contrastReduced = 30, // max: 255 + .timeoutReduceContrastMs = 5 * 60 * 1000, // actions at certain inactivity + .timeoutSwitchToScreensaverMs = 30 * 60 * 1000 + }; -//--- controlledMotor --- -//functions for updating the duty via certain/current driver that can then be passed to controlledMotor -//-> makes it possible to easily use different motor drivers -//note: ignoring warning "capture of variable 'sabertoothDriver' with non-automatic storage duration", since sabertoothDriver object does not get destroyed anywhere - no lifetime issue -motorSetCommandFunc_t setLeftFunc = [&sabertoothDriver](motorCommand_t cmd) { - sabertoothDriver.setLeft(cmd); + +//------------------------- +//-------- encoder -------- +//------------------------- +//configure rotary encoder (next to joystick) +rotary_encoder_t encoder_config = { + .pin_a = GPIO_NUM_25, + .pin_b = GPIO_NUM_26, + .pin_btn = GPIO_NUM_21, + .code = 1, + .store = 0, //encoder count + .index = 0, + .btn_pressed_time_us = 20000, + .btn_state = RE_BTN_RELEASED //default state }; -motorSetCommandFunc_t setRightFunc = [&sabertoothDriver](motorCommand_t cmd) { - sabertoothDriver.setRight(cmd); -}; -//create controlled motor instances (motorctl.hpp) -controlledMotor motorLeft(setLeftFunc, configMotorControlLeft); -controlledMotor motorRight(setRightFunc, configMotorControlRight); - -//create speedsensor instances -speedSensor speedLeft (speedLeft_config); -speedSensor speedRight (speedRight_config); - -//create global joystic instance (joystick.hpp) -evaluatedJoystick joystick(configJoystick); - -//create global evaluated switch instance for button next to joystick -gpio_evaluatedSwitch buttonJoystick(GPIO_NUM_21, true, false); //pullup true, not inverted (switch to GND use pullup of controller) - -//create buzzer object on pin 12 with gap between queued events of 100ms -buzzer_t buzzer(GPIO_NUM_12, 100); - -//create global httpJoystick object (http.hpp) -httpJoystick httpJoystickMain(configHttpJoystickMain); - -//create global control object (control.hpp) -controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &joystick, &httpJoystickMain); - -//create global automatedArmchair object (for auto-mode) (auto.hpp) -automatedArmchair armchair; - +//----------------------------------- +//--- joystick command generation --- +//----------------------------------- +//configure parameters for motor command generation from joystick data +joystickGenerateCommands_config_t joystickGenerateCommands_config{ + //-- maxDuty -- + // max duty when both motors are at equal ratio e.g. driving straight forward + // better to be set less than 100% to have some reserve for boosting the outer tire when turning + .maxDutyStraight = 65, + //-- maxBoost -- + // boost is amount of duty added to maxDutyStraight to outer tire while turning + // => turning: inner tire gets slower, outer tire gets faster + // 0: boost = 0 (disabled) + // 100: boost = maxDutyStraight (e.g. when maxDuty is 50, outer motor can still reach 100 (50+50)) + .maxRelativeBoostPercentOfMaxDuty = 60, + // 60: when maxDuty is set above 62% (equals 0.6*62 = 38% boost) the outer tire can still reach 100% - below 62 maxDuty the boosted speed is also reduced. + // => setting this value lower prevents desired low max duty configuration from being way to fast in curves. + .dutyOffset = 5, // duty at which motors start immediately + .ratioSnapToOneThreshold = 0.9, // threshold ratio snaps to 1 to have some area of max turning before entering X-Axis-full-rotate mode + .altStickMapping = false // invert reverse direction +}; \ No newline at end of file diff --git a/board_single/main/config.h b/board_single/main/config.h new file mode 100644 index 0000000..9cc2aab --- /dev/null +++ b/board_single/main/config.h @@ -0,0 +1,6 @@ +#pragma once + +// outsourced macros / definitions + +//-- control.cpp -- +//#define JOYSTICK_LOG_IN_IDLE \ No newline at end of file diff --git a/board_single/main/config.hpp b/board_single/main/config.hpp deleted file mode 100644 index d9b8d1b..0000000 --- a/board_single/main/config.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include "motordrivers.hpp" -#include "motorctl.hpp" -#include "joystick.hpp" - -#include "gpio_evaluateSwitch.hpp" -#include "buzzer.hpp" -#include "control.hpp" -#include "fan.hpp" -#include "http.hpp" -#include "auto.hpp" -#include "speedsensor.hpp" - - -//in IDLE mode: set loglevel for evaluatedJoystick to DEBUG -//and repeatedly read joystick e.g. for manually calibrating / testing joystick -#define JOYSTICK_LOG_IN_IDLE - - -//TODO outsource global variables to e.g. global.cpp and only config options here? - -//create global controlledMotor instances for both motors -extern controlledMotor motorLeft; -extern controlledMotor motorRight; - -//create global joystic instance -extern evaluatedJoystick joystick; - -//create global evaluated switch instance for button next to joystick -extern gpio_evaluatedSwitch buttonJoystick; - -//create global buzzer object -extern buzzer_t buzzer; - -//create global control object -extern controlledArmchair control; - -//create global automatedArmchair object (for auto-mode) -extern automatedArmchair armchair; - -//create global httpJoystick object -//extern httpJoystick httpJoystickMain; - -//configuration for fans / cooling -extern fan_config_t configCooling; - -//create global objects for measuring speed -extern speedSensor speedLeft; -extern speedSensor speedRight; - diff --git a/board_single/main/control.cpp b/board_single/main/control.cpp index fc9e4e3..907c293 100644 --- a/board_single/main/control.cpp +++ b/board_single/main/control.cpp @@ -9,43 +9,98 @@ extern "C" #include "wifi.h" } -#include "config.hpp" +#include "config.h" #include "control.hpp" +#include "chairAdjust.hpp" +#include "display.hpp" // needed for getBatteryPercent() -//used definitions moved from config.hpp: -//#define JOYSTICK_TEST +//used definitions moved from config.h: +//#define JOYSTICK_LOG_IN_IDLE //tag for logging static const char * TAG = "control"; -const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO"}; +static const char * ERROR_STR = "ERR"; + +const char* controlModeStr[10] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO", "ADJUST_CHAIR", "MENU_SETTINGS", "MENU_MODE_SELECT"}; +const uint8_t controlModeMaxCount = sizeof(controlModeStr) / sizeof(char *); +#define MUTEX_TIMEOUT 10000 // restart when stuck waiting for handle() mutex + + +//========================== +//==== controlModeToStr ==== +//========================== +// convert controlMode enum or mode index to string for logging, returns "ERR" when index is out of range of existing modes +const char * controlModeToStr(int modeIndex){ + // return string when in allowed range + if (modeIndex >= 0 && modeIndex < controlModeMaxCount) + return controlModeStr[modeIndex]; + else + // log and return error when not in range + ESP_LOGE(TAG, "controlModeToStr: mode index '%d' is not in valid range - max 0-%d", modeIndex, controlModeMaxCount); + return ERROR_STR; +} +const char * controlModeToStr(controlMode_t mode){ + return controlModeToStr((int)mode); +} //----------------------------- //-------- constructor -------- //----------------------------- -controlledArmchair::controlledArmchair ( - control_config_t config_f, - buzzer_t * buzzer_f, - controlledMotor* motorLeft_f, - controlledMotor* motorRight_f, - evaluatedJoystick* joystick_f, - httpJoystick* httpJoystick_f - ){ +controlledArmchair::controlledArmchair( + control_config_t config_f, + buzzer_t *buzzer_f, + controlledMotor *motorLeft_f, + controlledMotor *motorRight_f, + evaluatedJoystick *joystick_f, + joystickGenerateCommands_config_t *joystickGenerateCommands_config_f, + httpJoystick *httpJoystick_f, + automatedArmchair_c *automatedArmchair_f, + cControlledRest *legRest_f, + cControlledRest *backRest_f, + nvs_handle_t * nvsHandle_f) +{ //copy configuration config = config_f; + joystickGenerateCommands_config = *joystickGenerateCommands_config_f; //copy object pointers buzzer = buzzer_f; motorLeft = motorLeft_f; motorRight = motorRight_f; joystick_l = joystick_f, httpJoystickMain_l = httpJoystick_f; + automatedArmchair = automatedArmchair_f; + legRest = legRest_f; + backRest = backRest_f; + nvsHandle = nvsHandle_f; //set default mode from config modePrevious = config.defaultMode; - //TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)? + // override default config value if maxDuty is found in nvs + loadMaxDuty(); + // update brake start threshold with actual max duty for motorctl + ESP_LOGW(TAG, "setting brake start threshold for both motors to %.0f", joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); + motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); + motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); + + // create semaphore for preventing race condition: mode-change operations while currently still executing certain mode + handleIteration_mutex = xSemaphoreCreateMutex(); +} + + +//======================================= +//============ control task ============= +//======================================= +// task that controls the armchair modes +// generates commands depending on current mode and sends those to corresponding task +// parameter: pointer to controlledArmchair object +void task_control( void * pvParameters ){ + controlledArmchair * control = (controlledArmchair *)pvParameters; + ESP_LOGW(TAG, "Initializing controlledArmchair and starting handle loop"); + control->startHandleLoop(); } @@ -53,177 +108,269 @@ controlledArmchair::controlledArmchair ( //---------------------------------- //---------- Handle loop ----------- //---------------------------------- -//function that repeatedly generates motor commands depending on the current mode -//also handles fading and current-limit -void controlledArmchair::startHandleLoop() { - while (1){ - ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]); +// start endless loop that repeatedly calls handle() and handleTimeout() methods +void controlledArmchair::startHandleLoop() +{ + while (1) + { + // mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed + if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE) + { + //--- handle current mode --- + ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]); + handle(); - switch(mode) { - default: - mode = controlMode_t::IDLE; - break; - - case controlMode_t::IDLE: - //copy preset commands for idling both motors - commands = cmds_bothMotorsIdle; - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); - vTaskDelay(200 / portTICK_PERIOD_MS); -#ifdef JOYSTICK_LOG_IN_IDLE - //get joystick data here (without using it) - //since loglevel is DEBUG, calculateion details is output - joystick_l->getData(); //get joystick data here -#endif - break; - - - case controlMode_t::JOYSTICK: - vTaskDelay(20 / portTICK_PERIOD_MS); - //get current joystick data with getData method of evaluatedJoystick - stickData = joystick_l->getData(); - //additionaly scale coordinates (more detail in slower area) - joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config - //generate motor commands - commands = joystick_generateCommandsDriving(stickData, altStickMapping); - //apply motor commands - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); - //TODO make motorctl.setTarget also accept motorcommand struct directly - break; - - - case controlMode_t::MASSAGE: - vTaskDelay(10 / portTICK_PERIOD_MS); - //--- read joystick --- - //only update joystick data when input not frozen - if (!freezeInput){ - stickData = joystick_l->getData(); - } - //--- generate motor commands --- - //pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function - commands = joystick_generateCommandsShaking(stickData); - //apply motor commands - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); - break; - - - case controlMode_t::HTTP: - //--- get joystick data from queue --- - //Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned - //TODO: as described above, when changing modes it might delay a few seconds for the change to apply - stickData = httpJoystickMain_l->getData(); - //scale coordinates additionally (more detail in slower area) - joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config - ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle); - //--- generate motor commands --- - //Note: timeout (no data received) is handled in getData method - commands = joystick_generateCommandsDriving(stickData, altStickMapping); - - //--- apply commands to motors --- - //TODO make motorctl.setTarget also accept motorcommand struct directly - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); - break; - - - case controlMode_t::AUTO: - vTaskDelay(20 / portTICK_PERIOD_MS); - //generate commands - commands = armchair.generateCommands(&instruction); - //--- apply commands to motors --- - //TODO make motorctl.setTarget also accept motorcommand struct directly - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); - - //process received instruction - switch (instruction) { - case auto_instruction_t::NONE: - break; - case auto_instruction_t::SWITCH_PREV_MODE: - toggleMode(controlMode_t::AUTO); - break; - case auto_instruction_t::SWITCH_JOYSTICK_MODE: - changeMode(controlMode_t::JOYSTICK); - break; - case auto_instruction_t::RESET_ACCEL_DECEL: - //enable downfading (set to default value) - motorLeft->setFade(fadeType_t::DECEL, true); - motorRight->setFade(fadeType_t::DECEL, true); - //set upfading to default value - motorLeft->setFade(fadeType_t::ACCEL, true); - motorRight->setFade(fadeType_t::ACCEL, true); - break; - case auto_instruction_t::RESET_ACCEL: - //set upfading to default value - motorLeft->setFade(fadeType_t::ACCEL, true); - motorRight->setFade(fadeType_t::ACCEL, true); - break; - case auto_instruction_t::RESET_DECEL: - //enable downfading (set to default value) - motorLeft->setFade(fadeType_t::DECEL, true); - motorRight->setFade(fadeType_t::DECEL, true); - break; - } - break; - - - //TODO: add other modes here + xSemaphoreGive(handleIteration_mutex); + } // end mutex + else { + ESP_LOGE(TAG, "mutex timeout - stuck in changeMode? -> RESTART"); + esp_restart(); } - - //--- run actions based on received button button event --- - //note: buttonCount received by sendButtonEvent method called from button.cpp - //TODO: what if variable gets set from other task during this code? -> mutex around this code - switch (buttonCount) { - case 1: //define joystick center or freeze input - if (mode == controlMode_t::JOYSTICK){ - //joystick mode: calibrate joystick - joystick_l->defineCenter(); - } else if (mode == controlMode_t::MASSAGE){ - //massage mode: toggle freeze of input (lock joystick at current values) - freezeInput = !freezeInput; - if (freezeInput){ - buzzer->beep(5, 40, 25); - } else { - buzzer->beep(1, 300, 100); - } - } - break; - - case 12: //toggle alternative joystick mapping (reverse swapped) - altStickMapping = !altStickMapping; - if (altStickMapping){ - buzzer->beep(6, 70, 50); - } else { - buzzer->beep(1, 500, 100); - } - break; - } - //--- reset button event --- (only one action per run) - if (buttonCount > 0){ - ESP_LOGI(TAG, "resetting button event/count"); - buttonCount = 0; - } - - - - //----------------------- - //------ slow loop ------ - //----------------------- - //this section is run about every 5s (+500ms) - if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) { - ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000); + //--- slow loop --- + // this section is run approx every 5s (+500ms) + if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) + { + ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000); timestamp_SlowLoopLastRun = esp_log_timestamp(); - - //run function which detects timeout (switch to idle) + //--- handle timeouts --- + // run function that detects timeouts (switch to idle, or notify "forgot to turn off") handleTimeout(); } + vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex + // TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock + } +} - }//end while(1) -}//end startHandleLoop +//------------------------------------- +//---------- Handle control ----------- +//------------------------------------- +// function that repeatedly generates motor commands and runs actions depending on the current mode +void controlledArmchair::handle() +{ + + switch (mode) + { + default: + //switch to IDLE mode when current mode is not implemented + changeMode(controlMode_t::IDLE); + break; + + //------- handle IDLE ------- + case controlMode_t::IDLE: + vTaskDelay(500 / portTICK_PERIOD_MS); + // TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass +#ifdef JOYSTICK_LOG_IN_IDLE + // get joystick data and log it + joystickData_t data joystick_l->getData(); + ESP_LOGI("JOYSTICK_LOG_IN_IDLE", "x=%.3f, y=%.3f, radius=%.3f, angle=%.3f, pos=%s, adcx=%d, adcy=%d", + data.x, data.y, data.radius, data.angle, + joystickPosStr[(int)data.position], + objects->joystick->getRawX(), objects->joystick->getRawY()); +#endif + break; + + //------- handle JOYSTICK mode ------- + case controlMode_t::JOYSTICK: + vTaskDelay(50 / portTICK_PERIOD_MS); + // get current joystick data with getData method of evaluatedJoystick + stickDataLast = stickData; + stickData = joystick_l->getData(); + // additionaly scale coordinates (more detail in slower area) + joystick_scaleCoordinatesLinear(&stickData, 0.7, 0.45); // TODO: add scaling parameters to config + // generate motor commands + // only generate when the stick data actually changed (e.g. stick stayed in center) + if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y) + { + resetTimeout(); // user input -> reset switch to IDLE timeout + commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config); + // apply motor commands + motorRight->setTarget(commands.right); + motorLeft->setTarget(commands.left); + } + else + { + vTaskDelay(20 / portTICK_PERIOD_MS); + ESP_LOGV(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]); + } + break; + + //------- handle MASSAGE mode ------- + case controlMode_t::MASSAGE: + vTaskDelay(10 / portTICK_PERIOD_MS); + //--- read joystick --- + // only update joystick data when input not frozen + stickDataLast = stickData; + if (!freezeInput) + stickData = joystick_l->getData(); + // reset timeout when joystick data changed + if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y) + resetTimeout(); // user input -> reset switch to IDLE timeout + //--- generate motor commands --- + // pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function + commands = joystick_generateCommandsShaking(stickData); + // apply motor commands + motorRight->setTarget(commands.right); + motorLeft->setTarget(commands.left); + break; + + //------- handle HTTP mode ------- + case controlMode_t::HTTP: + //--- get joystick data from queue --- + stickDataLast = stickData; + stickData = httpJoystickMain_l->getData(); // get last stored data from receive queue (waits up to 500ms for new event to arrive) + // scale coordinates additionally (more detail in slower area) + joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); // TODO: add scaling parameters to config + ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle); + //--- generate motor commands --- + // only generate when the stick data actually changed (e.g. no new data recevied via http) + if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y) + { + resetTimeout(); // user input -> reset switch to IDLE timeout + // Note: timeout (no data received) is handled in getData method + commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config); + + //--- apply commands to motors --- + motorRight->setTarget(commands.right); + motorLeft->setTarget(commands.left); + } + else + { + ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]); + } + break; + + //------- handle AUTO mode ------- + case controlMode_t::AUTO: + vTaskDelay(20 / portTICK_PERIOD_MS); + // generate commands + commands = automatedArmchair->generateCommands(&instruction); + //--- apply commands to motors --- + motorRight->setTarget(commands.right); + motorLeft->setTarget(commands.left); + + // process received instruction + switch (instruction) + { + case auto_instruction_t::NONE: + break; + case auto_instruction_t::SWITCH_PREV_MODE: + toggleMode(controlMode_t::AUTO); + break; + case auto_instruction_t::SWITCH_JOYSTICK_MODE: + changeMode(controlMode_t::JOYSTICK); + break; + case auto_instruction_t::RESET_ACCEL_DECEL: + // enable downfading (set to default value) + motorLeft->setFade(fadeType_t::DECEL, true); + motorRight->setFade(fadeType_t::DECEL, true); + // set upfading to default value + motorLeft->setFade(fadeType_t::ACCEL, true); + motorRight->setFade(fadeType_t::ACCEL, true); + break; + case auto_instruction_t::RESET_ACCEL: + // set upfading to default value + motorLeft->setFade(fadeType_t::ACCEL, true); + motorRight->setFade(fadeType_t::ACCEL, true); + break; + case auto_instruction_t::RESET_DECEL: + // enable downfading (set to default value) + motorLeft->setFade(fadeType_t::DECEL, true); + motorRight->setFade(fadeType_t::DECEL, true); + break; + } + break; + + //------- handle ADJUST_CHAIR mode ------- + case controlMode_t::ADJUST_CHAIR: + vTaskDelay(100 / portTICK_PERIOD_MS); + //--- read joystick --- + stickDataLast = stickData; + stickData = joystick_l->getData(); + //--- control armchair position with joystick input --- + // dont update when stick data did not change + if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y) + { + resetTimeout(); // user input -> reset switch to IDLE timeout + controlChairAdjustment(joystick_l->getData(), legRest, backRest); + } + break; + + //------- handle MENU modes ------- + case controlMode_t::MENU_SETTINGS: + case controlMode_t::MENU_MODE_SELECT: + // nothing to do here, display task handles the menu + vTaskDelay(500 / portTICK_PERIOD_MS); + break; + + // TODO: add other modes here + } + +} // end - handle method + + + +//--------------------------------------- +//------ toggleFreezeInputMassage ------- +//--------------------------------------- +// releases or locks joystick in place when in massage mode +bool controlledArmchair::toggleFreezeInputMassage() +{ + if (mode == controlMode_t::MASSAGE) + { + // massage mode: toggle freeze of input (lock joystick at current values) + freezeInput = !freezeInput; + if (freezeInput) + { + buzzer->beep(5, 40, 25); + ESP_LOGW(TAG, "joystick input is now locked in place"); + } + else + { + buzzer->beep(1, 300, 100); + ESP_LOGW(TAG, "joystick input gets updated again"); + } + return freezeInput; + } + else + { + ESP_LOGE(TAG, "can not freeze/unfreeze joystick input - not in MASSAGE mode!"); + return 0; + } +} + + + +//------------------------------------- +//------- toggleAltStickMapping ------- +//------------------------------------- +// toggle between normal and alternative stick mapping (joystick reverse position inverted) +bool controlledArmchair::toggleAltStickMapping() +{ + joystickGenerateCommands_config.altStickMapping = !joystickGenerateCommands_config.altStickMapping; + if (joystickGenerateCommands_config.altStickMapping) + { + buzzer->beep(6, 70, 50); + ESP_LOGW(TAG, "changed to alternative stick mapping"); + } + else + { + buzzer->beep(1, 500, 100); + ESP_LOGW(TAG, "changed to default stick mapping"); + } + return joystickGenerateCommands_config.altStickMapping; +} + + +//----------------------------------- +//--------- idleBothMotors ---------- +//----------------------------------- +// turn both motors off +void controlledArmchair::idleBothMotors(){ + motorRight->setTarget(cmd_motorIdle); + motorLeft->setTarget(cmd_motorIdle); +} //----------------------------------- @@ -232,64 +379,51 @@ void controlledArmchair::startHandleLoop() { void controlledArmchair::resetTimeout(){ //TODO mutex timestamp_lastActivity = esp_log_timestamp(); + ESP_LOGV(TAG, "timeout: activity detected, resetting timeout"); } - -//------------------------------------ -//--------- sendButtonEvent ---------- -//------------------------------------ -void controlledArmchair::sendButtonEvent(uint8_t count){ - //TODO mutex - if not replaced with queue - ESP_LOGI(TAG, "setting button event"); - buttonCount = count; -} - - - //------------------------------------ //---------- handleTimeout ----------- //------------------------------------ -//percentage the duty can vary since last timeout check and still counts as incative -//TODO: add this to config -float inactivityTolerance = 10; +// switch to IDLE when no activity (prevent accidential movement) +// notify "power still on" when in IDLE for a very long time (prevent battery drain when forgotten to turn off) +// this function has to be run repeatedly (can be slow interval) +#define TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS 5 * 60 * 1000 // beep every 5 minutes for someone to notice +#define TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT 96 // only notify/beep when below certain percentage (prevent beeping when connected to charger) +// note: timeout durations are configured in config.cpp +void controlledArmchair::handleTimeout() +{ + uint32_t noActivityDurationMs = esp_log_timestamp() - timestamp_lastActivity; + // log current inactivity and configured timeouts + ESP_LOGD(TAG, "timeout check: last activity %dmin and %ds ago - timeout IDLE after %ds - notify after power on after %dh", + noActivityDurationMs / 1000 / 60, + noActivityDurationMs / 1000 % 60, + config.timeoutSwitchToIdleMs / 1000, + config.timeoutNotifyPowerStillOnMs / 1000 / 60 / 60); -//local function that checks whether two values differ more than a given tolerance -bool validateActivity(float dutyOld, float dutyNow, float tolerance){ - float dutyDelta = dutyNow - dutyOld; - if (fabs(dutyDelta) < tolerance) { - return false; //no significant activity detected - } else { - return true; //there was activity + // -- timeout switch to IDLE -- + // timeout to IDLE when not idling already + if (mode != controlMode_t::IDLE && noActivityDurationMs > config.timeoutSwitchToIdleMs) + { + ESP_LOGW(TAG, "timeout check: [TIMEOUT], no activity for more than %ds -> switch to IDLE", config.timeoutSwitchToIdleMs / 1000); + changeMode(controlMode_t::IDLE); + //TODO switch to previous status-screen when activity detected } -} -//function that evaluates whether there is no activity/change on the motor duty for a certain time. If so, a switch to IDLE is issued. - has to be run repeatedly in a slow interval -void controlledArmchair::handleTimeout(){ - //check for timeout only when not idling already - if (mode != controlMode_t::IDLE) { - //get current duty from controlled motor objects - float dutyLeftNow = motorLeft->getStatus().duty; - float dutyRightNow = motorRight->getStatus().duty; - - //activity detected on any of the two motors - if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance) - || validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance) - ){ - ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset"); - //reset last duty and timestamp - dutyLeft_lastActivity = dutyLeftNow; - dutyRight_lastActivity = dutyRightNow; - resetTimeout(); - } - //no activity on any motor and msTimeout exceeded - else if (esp_log_timestamp() - timestamp_lastActivity > config.timeoutMs){ - ESP_LOGI(TAG, "timeout check: [TIMEOUT], no activity for more than %.ds -> switch to idle", config.timeoutMs/1000); - //toggle to idle mode - toggleIdle(); - } - else { - ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f s ago, timeout after %d s", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000, config.timeoutMs/1000); + // -- timeout notify "forgot to turn off" -- + // repeatedly notify via buzzer when in IDLE for a very long time to prevent battery drain ("forgot to turn off") + // also battery charge-level has to be below certain threshold to prevent beeping in case connected to charger + // note: ignores user input while in IDLE (e.g. encoder rotation) + else if ((esp_log_timestamp() - timestamp_lastModeChange) > config.timeoutNotifyPowerStillOnMs && getBatteryPercent() < TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT) + { + // beep in certain intervals + if ((esp_log_timestamp() - timestamp_lastTimeoutBeep) > TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS) + { + ESP_LOGW(TAG, "timeout: [TIMEOUT] in IDLE since %.3f hours -> beeping", (float)(esp_log_timestamp() - timestamp_lastModeChange) / 1000 / 60 / 60); + // TODO dont beep at certain time ranges (e.g. at night) + timestamp_lastTimeoutBeep = esp_log_timestamp(); + buzzer->beep(6, 100, 50); } } } @@ -300,130 +434,143 @@ void controlledArmchair::handleTimeout(){ //----------- changeMode ------------ //----------------------------------- //function to change to a specified control mode -void controlledArmchair::changeMode(controlMode_t modeNew) { - //reset timeout timer - resetTimeout(); +void controlledArmchair::changeMode(controlMode_t modeNew) +{ + // variable to store configured accel limit before entering massage mode, to restore it later + static uint32_t massagePreviousAccel = motorLeft->getFade(fadeType_t::ACCEL); + static uint32_t massagePreviousDecel = motorLeft->getFade(fadeType_t::DECEL); - //exit if target mode is already active - if (mode == modeNew) { + // exit if target mode is already active + if (mode == modeNew) + { ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]); return; } - //copy previous mode - modePrevious = mode; + // mutex to wait for current handle iteration (control-task) to finish + // prevents race conditions where operations when changing mode are run but old mode gets handled still + ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish..."); + if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE) + { + // copy previous mode + modePrevious = mode; + // store time changed (needed for timeout) + timestamp_lastModeChange = esp_log_timestamp(); - ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]); + ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]); - //========== commands change FROM mode ========== - //run functions when changing FROM certain mode - switch(modePrevious){ - default: - ESP_LOGI(TAG, "noting to execute when changing FROM this mode"); - break; + //========== commands change FROM mode ========== + // run functions when changing FROM certain mode + switch (modePrevious) + { + default: + ESP_LOGI(TAG, "noting to execute when changing FROM this mode"); + break; + case controlMode_t::IDLE: #ifdef JOYSTICK_LOG_IN_IDLE - case controlMode_t::IDLE: - ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'"); - esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config - break; + ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'"); + esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); // FIXME: loglevel from config #endif + buzzer->beep(1, 200, 100); + break; - case controlMode_t::HTTP: - ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi"); - //stop http server - ESP_LOGI(TAG, "disabling http server..."); - http_stop_server(); - - //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp) - //stop wifi - //TODO: decide whether ap or client is currently used - which has to be disabled? - //ESP_LOGI(TAG, "deinit wifi..."); - //wifi_deinit_client(); - //wifi_deinit_ap(); - ESP_LOGI(TAG, "done stopping http mode"); + case controlMode_t::HTTP: + ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap"); + wifi_stop_ap(); break; case controlMode_t::MASSAGE: ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input"); - //TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here... - //enable downfading (set to default value) - motorLeft->setFade(fadeType_t::DECEL, true); - motorRight->setFade(fadeType_t::DECEL, true); - //set upfading to default value - motorLeft->setFade(fadeType_t::ACCEL, true); - motorRight->setFade(fadeType_t::ACCEL, true); - //reset frozen input state + // TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here... + // enable downfading (set to default value) + motorLeft->setFade(fadeType_t::DECEL, massagePreviousDecel); + motorRight->setFade(fadeType_t::DECEL, massagePreviousDecel); + // restore previously set acceleration limit + motorLeft->setFade(fadeType_t::ACCEL, massagePreviousAccel); + motorRight->setFade(fadeType_t::ACCEL, massagePreviousAccel); + // reset frozen input state freezeInput = false; break; case controlMode_t::AUTO: ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default"); - //TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here... - //enable downfading (set to default value) + // TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here... + // enable downfading (set to default value) motorLeft->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, true); - //set upfading to default value + // set upfading to default value motorLeft->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true); break; - } + case controlMode_t::ADJUST_CHAIR: + ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors..."); + // prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving + legRest->setState(REST_OFF); + backRest->setState(REST_OFF); + break; + } - //========== commands change TO mode ========== - //run functions when changing TO certain mode - switch(modeNew){ + //========== commands change TO mode ========== + // run functions when changing TO certain mode + switch (modeNew) + { default: ESP_LOGI(TAG, "noting to execute when changing TO this mode"); break; - case controlMode_t::IDLE: - buzzer->beep(1, 1500, 0); -#ifdef JOYSTICK_LOG_IN_IDLE - esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); -#endif - break; + case controlMode_t::IDLE: + ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep"); + idleBothMotors(); + buzzer->beep(1, 900, 0); + break; case controlMode_t::HTTP: - ESP_LOGW(TAG, "switching to http mode -> enabling http and wifi"); - //start wifi - //TODO: decide wether ap or client should be started - ESP_LOGI(TAG, "init wifi..."); + ESP_LOGW(TAG, "switching to HTTP mode -> starting wifi-ap"); + wifi_start_ap(); + break; - //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp) - //wifi_init_client(); - //wifi_init_ap(); + case controlMode_t::ADJUST_CHAIR: + ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode: turning both motors off, beep"); + idleBothMotors(); + buzzer->beep(3, 100, 50); + break; - //wait for wifi - //ESP_LOGI(TAG, "waiting for wifi..."); - //vTaskDelay(1000 / portTICK_PERIOD_MS); - - //start http server - ESP_LOGI(TAG, "init http server..."); - http_init_server(); - ESP_LOGI(TAG, "done initializing http mode"); + case controlMode_t::MENU_SETTINGS: + idleBothMotors(); break; case controlMode_t::MASSAGE: ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading"); - uint32_t shake_msFadeAccel = 500; //TODO: move this to config + uint32_t shake_msFadeAccel = 350; // TODO: move this to config + uint32_t shake_msFadeDecel = 0; // TODO: move this to config - //disable downfading (max. deceleration) - motorLeft->setFade(fadeType_t::DECEL, false); - motorRight->setFade(fadeType_t::DECEL, false); - //reduce upfading (increase acceleration) - motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel); - motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel); + // save currently set normal acceleration config (for restore when leavinge MASSAGE again) + massagePreviousAccel = motorLeft->getFade(fadeType_t::ACCEL); + massagePreviousDecel = motorLeft->getFade(fadeType_t::DECEL); + // disable downfading (max. deceleration) + motorLeft->setFade(fadeType_t::DECEL, shake_msFadeDecel, false); + motorRight->setFade(fadeType_t::DECEL, shake_msFadeDecel, false); + // reduce upfading (increase acceleration) but do not update nvs + motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel, false); + motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel, false); break; + } + //--- update mode to new mode --- + mode = modeNew; + + // unlock mutex for control task to continue handling modes + xSemaphoreGive(handleIteration_mutex); + } // end mutex + else + { + ESP_LOGE(TAG, "mutex timeout - stuck in handle() loop? -> RESTART"); + esp_restart(); } - - //--- update mode to new mode --- - //TODO: add mutex - mode = modeNew; } - //TODO simplify the following 3 functions? can be replaced by one? //----------------------------------- @@ -445,13 +592,13 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo //switch to secondary mode when primary is already active if (mode == modePrimary){ ESP_LOGW(TAG, "toggleModes: switching from primaryMode %s to secondarMode %s", controlModeStr[(int)mode], controlModeStr[(int)modeSecondary]); - buzzer->beep(2,200,100); + //buzzer->beep(2,200,100); changeMode(modeSecondary); //switch to secondary mode } //switch to primary mode when any other mode is active else { - ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); - buzzer->beep(4,200,100); + ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); + //buzzer->beep(4,200,100); changeMode(modePrimary); } } @@ -466,14 +613,67 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){ //switch to previous mode when primary is already active if (mode == modePrimary){ - ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]); + ESP_LOGW(TAG, "toggleMode: switching from primaryMode '%s' to previousMode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]); //buzzer->beep(2,200,100); changeMode(modePrevious); //switch to previous mode } //switch to primary mode when any other mode is active else { - ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); + ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); //buzzer->beep(4,200,100); changeMode(modePrimary); } } + + + + +//----------------------------- +//-------- loadMaxDuty -------- +//----------------------------- +// update local config value when maxDuty is stored in nvs +void controlledArmchair::loadMaxDuty(void) +{ + // default value is already loaded (constructor) + // read from nvs + uint16_t valueRead; + esp_err_t err = nvs_get_u16(*nvsHandle, "c-maxDuty", &valueRead); + switch (err) + { + case ESP_OK: + ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, valueRead/100.0); + joystickGenerateCommands_config.maxDutyStraight = (float)(valueRead/100.0); + break; + case ESP_ERR_NVS_NOT_FOUND: + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight); + break; + default: + ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); + } +} + + +//----------------------------------- +//---------- writeMaxDuty ----------- +//----------------------------------- +// write provided value to nvs to be persistent and update local variable in joystickGenerateCommmands_config struct +// note: duty percentage gets stored as uint with factor 100 (to get more precision) +void controlledArmchair::writeMaxDuty(float newValue){ + // check if unchanged + if(joystickGenerateCommands_config.maxDutyStraight == newValue){ + ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue); + return; + } + // update nvs value + ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, newValue) ; + esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100)); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed writing"); + err = nvs_commit(*nvsHandle); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed committing updates"); + else + ESP_LOGI(TAG, "nvs: successfully committed updates"); + // update variable + joystickGenerateCommands_config.maxDutyStraight = newValue; +} \ No newline at end of file diff --git a/board_single/main/control.hpp b/board_single/main/control.hpp index 8ed1e66..fa4d20d 100644 --- a/board_single/main/control.hpp +++ b/board_single/main/control.hpp @@ -1,30 +1,55 @@ #pragma once +extern "C" +{ +#include "nvs_flash.h" +#include "nvs.h" +} #include "motordrivers.hpp" #include "motorctl.hpp" #include "buzzer.hpp" #include "http.hpp" #include "auto.hpp" +#include "speedsensor.hpp" +#include "chairAdjust.hpp" +//percentage stick has to be moved in the opposite driving direction of current motor direction for braking to start +#define BRAKE_START_STICK_PERCENTAGE 95 //-------------------------------------------- //---- struct, enum, variable declarations --- //-------------------------------------------- //enum that decides how the motors get controlled -enum class controlMode_t {IDLE, JOYSTICK, MASSAGE, HTTP, MQTT, BLUETOOTH, AUTO}; +enum class controlMode_t {IDLE, JOYSTICK, MASSAGE, HTTP, MQTT, BLUETOOTH, AUTO, ADJUST_CHAIR, MENU_SETTINGS, MENU_MODE_SELECT}; //string array representing the mode enum (for printing the state as string) -extern const char* controlModeStr[7]; +extern const char* controlModeStr[10]; +extern const uint8_t controlModeMaxCount; //--- control_config_t --- //struct with config parameters typedef struct control_config_t { controlMode_t defaultMode; //default mode after startup and toggling IDLE //timeout options - uint32_t timeoutMs; //time of inactivity after which the mode gets switched to IDLE - float timeoutTolerancePer; //percentage the duty can vary between timeout checks considered still inactive + uint32_t timeoutSwitchToIdleMs; //time of inactivity after which the mode gets switched to IDLE + uint32_t timeoutNotifyPowerStillOnMs; } control_config_t; +//========================== +//==== controlModeToStr ==== +//========================== +// convert controlMode enum or index to string for logging +const char * controlModeToStr(controlMode_t mode); +const char * controlModeToStr(int modeIndex); + + +//======================================= +//============ control task ============= +//======================================= +//task that controls the armchair modes and initiates commands generation and applies them to driver +//parameter: pointer to controlledArmchair object +void task_control( void * controlledArmchair ); + //================================== @@ -41,11 +66,16 @@ class controlledArmchair { controlledMotor* motorLeft_f, controlledMotor* motorRight_f, evaluatedJoystick* joystick_f, - httpJoystick* httpJoystick_f + joystickGenerateCommands_config_t* joystickGenerateCommands_config_f, + httpJoystick* httpJoystick_f, + automatedArmchair_c* automatedArmchair, + cControlledRest * legRest, + cControlledRest * backRest, + nvs_handle_t * nvsHandle_f ); //--- functions --- - //task that repeatedly generates motor commands depending on the current mode + //endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex void startHandleLoop(); //function that changes to a specified control mode @@ -63,32 +93,87 @@ class controlledArmchair { //function that restarts timer which initiates the automatic timeout (switch to IDLE) after certain time of inactivity void resetTimeout(); - //function for sending a button event (e.g. from button task at event) to control task - //TODO: use queue instead? - void sendButtonEvent(uint8_t count); + //methods to get the current or previous control mode + controlMode_t getCurrentMode() const {return mode;}; + controlMode_t getPreviousMode() const {return modePrevious;}; + const char *getCurrentModeStr() const { return controlModeStr[(int)mode]; }; + + //--- mode specific --- + // releases or locks joystick in place when in massage mode, returns true when input is frozen + bool toggleFreezeInputMassage(); + // toggle between normal and alternative stick mapping (joystick reverse position inverted), returns true when alt mapping is active + bool toggleAltStickMapping(); + + // configure max dutycycle (in joystick or http mode) + void setMaxDuty(float maxDutyNew) { + writeMaxDuty(maxDutyNew); + motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100); + motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100); + }; + float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; }; + // configure max boost (in joystick or http mode) + void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; }; + float getMaxRelativeBoostPer() const {return joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty; }; + + uint32_t getInactivityDurationMs() {return esp_log_timestamp() - timestamp_lastActivity;}; private: //--- functions --- + //generate motor commands or run actions depending on the current mode + void handle(); + //function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval void handleTimeout(); + void loadMaxDuty(); //load stored value for maxDuty from nvs + void writeMaxDuty(float newMaxDuty); //write new value for maxDuty to nvs + + void idleBothMotors(); //turn both motors off + //--- objects --- buzzer_t* buzzer; controlledMotor* motorLeft; controlledMotor* motorRight; httpJoystick* httpJoystickMain_l; evaluatedJoystick* joystick_l; + joystickGenerateCommands_config_t joystickGenerateCommands_config; + automatedArmchair_c *automatedArmchair; + cControlledRest * legRest; + cControlledRest * backRest; + //handle for using the nvs flash (persistent config variables) + nvs_handle_t * nvsHandle; + + //--- constants --- + //command preset for idling motors + const motorCommand_t cmd_motorIdle = { + .state = motorstate_t::IDLE, + .duty = 0 + }; + const motorCommands_t cmds_bothMotorsIdle = { + .left = cmd_motorIdle, + .right = cmd_motorIdle + }; + const joystickData_t joystickData_center = { + .position = joystickPos_t::CENTER, + .x = 0, + .y = 0, + .radius = 0, + .angle = 0 + }; //---variables --- //struct for motor commands returned by generate functions of each mode - motorCommands_t commands; + motorCommands_t commands = cmds_bothMotorsIdle; //struct with config parameters control_config_t config; + //mutex to prevent race condition between handle() and changeMode() + SemaphoreHandle_t handleIteration_mutex; + //store joystick data - joystickData_t stickData; - bool altStickMapping; //alternative joystick mapping (reverse mapped differently) + joystickData_t stickData = joystickData_center; + joystickData_t stickDataLast = joystickData_center; //variables for http mode uint32_t http_timestamp_lastData = 0; @@ -97,7 +182,7 @@ class controlledArmchair { bool freezeInput = false; //variables for AUTO mode - auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair + auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair_c //variable to store button event uint8_t buttonCount = 0; @@ -108,23 +193,13 @@ class controlledArmchair { //variable to store mode when toggling IDLE mode controlMode_t modePrevious; //default mode - //command preset for idling motors - const motorCommand_t cmd_motorIdle = { - .state = motorstate_t::IDLE, - .duty = 0 - }; - const motorCommands_t cmds_bothMotorsIdle = { - .left = cmd_motorIdle, - .right = cmd_motorIdle - }; - //variable for slow loop uint32_t timestamp_SlowLoopLastRun = 0; - //variables for detecting timeout (switch to idle, after inactivity) - float dutyLeft_lastActivity = 0; - float dutyRight_lastActivity = 0; + //variables for detecting timeout (switch to idle, or notify "forgot to turn off" after inactivity + uint32_t timestamp_lastModeChange = 0; uint32_t timestamp_lastActivity = 0; + uint32_t timestamp_lastTimeoutBeep = 0; }; diff --git a/board_single/main/display.cpp b/board_single/main/display.cpp index cfabf74..2923b4d 100644 --- a/board_single/main/display.cpp +++ b/board_single/main/display.cpp @@ -1,30 +1,27 @@ #include "display.hpp" extern "C"{ #include +#include "esp_ota_ops.h" } +#include "menu.hpp" -//# -//# SSD1306 Configuration -//# -#define GPIO_RANGE_MAX 33 -#define I2C_INTERFACE y -//# SSD1306_128x32 is not set -#define SSD1306_128x64 y -#define OFFSETX 0 -//# FLIP is not set -#define SCL_GPIO 22 -#define SDA_GPIO 23 -#define RESET_GPIO 15 //FIXME remove this -#define I2C_PORT_0 y -//# I2C_PORT_1 is not set -//# end of SSD1306 Configuration + +//=== content config === +#define STARTUP_MSG_TIMEOUT 2600 #define ADC_BATT_VOLTAGE ADC1_CHANNEL_6 #define BAT_CELL_COUNT 7 +// continously vary display contrast from 0 to 250 in OVERVIEW status screen +//#define BRIGHTNESS_TEST + +// if display and driver support hardware scrolling the SCREENSAVER status-screen will be smoother: +//#define HARDWARE_SCROLL_AVAILABLE - +//=== variables === +// every function can access the display configuration from config.cpp +static display_config_t displayConfig; //-------------------------- @@ -32,178 +29,603 @@ extern "C"{ //-------------------------- //TODO duplicate code: getVoltage also defined in currentsensor.cpp -> outsource this //local function to get average voltage from adc -float getVoltage1(adc1_channel_t adc, uint32_t samples){ +int readAdc(adc1_channel_t adc, uint32_t samples){ //measure voltage - int measure = 0; + uint32_t measure = 0; for (int j=0; j tmp) + va_list args; + va_start(args, format); + len = vsnprintf(tmp, sizeof(tmp), format, args); + va_end(args); + + // define max available digits + int maxLen = MAX_LEN_NORMAL; + if (isLarge) + maxLen = MAX_LEN_LARGE; + + // determine required spaces + int numSpaces = (maxLen - len) / 2; + if (numSpaces < 0) // limit to 0 in case string is too long already + numSpaces = 0; + + // add certain spaces around string (-> buf) + snprintf(buf, MAX_LEN_NORMAL*2, "%*s%s%*s", numSpaces, "", tmp, maxLen - numSpaces - len, ""); + ESP_LOGV(TAG, "print center - isLarge=%d, value='%s', needed-spaces=%d, resulted-string='%s'", isLarge, tmp, numSpaces, buf); + + // show line on display + if (isLarge) + ssd1306_display_text_x3(display, line, buf, maxLen, inverted); + else + ssd1306_display_text(display, line, buf, maxLen, inverted); +} + + + +//================================= +//===== scaleUsingLookupTable ===== +//================================= +//scale/inpolate an input value to output value between several known points (two arrays) +//notes: the lookup values must be in ascending order. If the input value is lower/larger than smalles/largest value, output is set to first/last element of output elements +float scaleUsingLookupTable(const float lookupInput[], const float lookupOutput[], int count, float input){ + // check limit case (set to min/max) + if (input <= lookupInput[0]) { + ESP_LOGV(TAG, "lookup: %.2f is lower than lowest value -> returning min", input); + return lookupOutput[0]; + } else if (input >= lookupInput[count -1]) { + ESP_LOGV(TAG, "lookup: %.2f is larger than largest value -> returning max", input); + return lookupOutput[count -1]; + } + + // find best matching range and + // scale input linear to output in matched range + for (int i = 1; i < count; ++i) + { + if (input <= lookupInput[i]) //best match + { + float voltageRange = lookupInput[i] - lookupInput[i - 1]; + float voltageOffset = input - lookupInput[i - 1]; + float percentageRange = lookupOutput[i] - lookupOutput[i - 1]; + float percentageOffset = lookupOutput[i - 1]; + float output = percentageOffset + (voltageOffset / voltageRange) * percentageRange; + ESP_LOGV(TAG, "lookup: - input=%.3f => output=%.3f", input, output); + ESP_LOGV(TAG, "lookup - matched range: %.2fV-%.2fV => %.1f-%.1f", lookupInput[i - 1], lookupInput[i], lookupOutput[i - 1], lookupOutput[i]); + return output; + } + } + ESP_LOGE(TAG, "lookup - unknown range"); + return 0.0; //unknown range +} + + +//================================== +//======= getBatteryVoltage ======== +//================================== +// apparently the ADC in combination with the added filter and voltage +// divider is slightly non-linear -> using lookup table +const float batteryAdcValues[] = {1732, 2418, 2509, 2600, 2753, 2853, 2889, 2909, 2936, 2951, 3005, 3068, 3090, 3122}; +const float batteryVoltages[] = {14.01, 20, 21, 22, 24, 25.47, 26, 26.4, 26.84, 27, 28, 29.05, 29.4, 30}; + float getBatteryVoltage(){ -#define BAT_VOLTAGE_CONVERSION_FACTOR 11.9 - float voltageRead = getVoltage1(ADC_BATT_VOLTAGE, 1000); - float battVoltage = voltageRead * 11.9; //note: factor comes from simple test with voltmeter - ESP_LOGD(TAG, "batteryVoltage - voltageAdc=%f, voltageConv=%f, factor=%.2f", voltageRead, battVoltage, BAT_VOLTAGE_CONVERSION_FACTOR); + // check if lookup table is configured correctly + int countAdc = sizeof(batteryAdcValues) / sizeof(float); + int countVoltages = sizeof(batteryVoltages) / sizeof(float); + if (countAdc != countVoltages) + { + ESP_LOGE(TAG, "getBatteryVoltage - count of configured adc-values do not match count of voltages"); + return 0; + } + + //read adc + int adcRead = readAdc(ADC_BATT_VOLTAGE, 1000); + + //convert adc to voltage using lookup table + float battVoltage = scaleUsingLookupTable(batteryAdcValues, batteryVoltages, countAdc, adcRead); + ESP_LOGD(TAG, "batteryVoltage - adcRaw=%d => voltage=%.3f, scaled using lookuptable with %d elements", adcRead, battVoltage, countAdc); return battVoltage; } - //---------------------------------- //------- getBatteryPercent -------- //---------------------------------- -//TODO find better/more accurate table? -//configure discharge curve of one cell with corresponding known voltage->chargePercent values -const float voltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20}; -const float percentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0}; +// TODO find better/more accurate table? +// configure discharge curve of one cell with corresponding known voltage->chargePercent values +const float cellVoltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20}; +const float cellPercentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0}; -float getBatteryPercent(float voltage){ - float cellVoltage = voltage/BAT_CELL_COUNT; - int size = sizeof(voltageLevels) / sizeof(voltageLevels[0]); - int sizePer = sizeof(percentageLevels) / sizeof(percentageLevels[0]); - //check if configured correctly - if (size != sizePer) { +float getBatteryPercent() +{ + // check if lookup table is configured correctly + int sizeVoltage = sizeof(cellVoltageLevels) / sizeof(cellVoltageLevels[0]); + int sizePer = sizeof(cellPercentageLevels) / sizeof(cellPercentageLevels[0]); + if (sizeVoltage != sizePer) + { ESP_LOGE(TAG, "getBatteryPercent - count of configured percentages do not match count of voltages"); return 0; } - if (cellVoltage <= voltageLevels[0]) { - return 0.0; - } else if (cellVoltage >= voltageLevels[size - 1]) { - return 100.0; + + //get current battery voltage + float voltage = getBatteryVoltage(); + float cellVoltage = voltage / BAT_CELL_COUNT; + + //convert voltage to battery percentage using lookup table + float percent = scaleUsingLookupTable(cellVoltageLevels, cellPercentageLevels, sizeVoltage, cellVoltage); + ESP_LOGD(TAG, "batteryPercentage - Battery=%.3fV, Cell=%.3fV => percentage=%.3f, scaled using lookuptable with %d elements", voltage, cellVoltage, percent, sizePer); + return percent; +} + + +//############################# +//#### showScreen Overview #### +//############################# +//shows overview on entire display: +//Battery percentage, voltage, current, mode, rpm, speed +#define STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL 400 +void showStatusScreenOverview(display_task_parameters_t *objects) +{ + //-- battery percentage -- + // TODO update when no load (currentsensors = ~0A) only + //-- large batt percent -- + displayTextLine(&dev, 0, true, false, "B:%02.0f%%", getBatteryPercent()); + + //-- voltage and current -- + displayTextLine(&dev, 3, false, false, "%04.1fV %04.1f:%04.1fA", + getBatteryVoltage(), + fabs(objects->motorLeft->getCurrentA()), + fabs(objects->motorRight->getCurrentA())); + + //-- control state -- + //print large line + displayTextLine(&dev, 4, true, false, "%s ", objects->control->getCurrentModeStr()); + + //-- speed and RPM -- + displayTextLine(&dev, 7, false, false, "%3.1fkm/h %03.0f:%03.0fR", + fabs((objects->speedLeft->getKmph() + objects->speedRight->getKmph()) / 2), + objects->speedLeft->getRpm(), + objects->speedRight->getRpm()); + + // debug speed sensors + ESP_LOGD(TAG, "%3.1fkm/h %03.0f:%03.0fR", + fabs((objects->speedLeft->getKmph() + objects->speedRight->getKmph()) / 2), + objects->speedLeft->getRpm(), + objects->speedRight->getRpm()); + vTaskDelay(STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL / portTICK_PERIOD_MS); + + //-- brightness test -- +#ifdef BRIGHTNESS_TEST + // continously vary brightness/contrast for testing + displayConfig.contrastNormal += 10; + if (displayConfig.contrastNormal > 255) + displayConfig.contrastNormal = 0; + ssd1306_contrast(&dev, displayConfig.contrastNormal); + vTaskDelay(100 / portTICK_PERIOD_MS); + ESP_LOGW(TAG, "TEST BRIGHTNESS, setting to %d", displayConfig.contrastNormal); +#endif +} + + +//############################ +//##### showScreen Speed ##### +//############################ +// shows speed of each motor in km/h large in two lines and RPM in last line +#define STATUS_SCREEN_SPEED_UPDATE_INTERVAL 300 +void showStatusScreenSpeed(display_task_parameters_t * objects) +{ + // title + displayTextLine(&dev, 0, false, false, "Speed L,R - km/h"); + // show km/h large in two lines + displayTextLine(&dev, 1, true, false, "%+.2f", objects->speedLeft->getKmph()); + displayTextLine(&dev, 4, true, false, "%+.2f", objects->speedRight->getKmph()); + // show both rotational speeds in one line + displayTextLineCentered(&dev, 7, false, false, "%+04.0f:%+04.0f RPM", + objects->speedLeft->getRpm(), + objects->speedRight->getRpm()); + vTaskDelay(STATUS_SCREEN_SPEED_UPDATE_INTERVAL / portTICK_PERIOD_MS); +} + + + +//############################# +//#### showScreen Joystick #### +//############################# +// shows speed of each motor in km/h large in two lines and RPM in last line +#define STATUS_SCREEN_JOYSTICK_UPDATE_INTERVAL 100 +void showStatusScreenJoystick(display_task_parameters_t * objects) +{ + // print all joystick data + joystickData_t data = objects->joystick->getData(); + displayTextLine(&dev, 0, false, false, "joystick status:"); + displayTextLine(&dev, 1, false, false, "x = %.3f ", data.x); + displayTextLine(&dev, 2, false, false, "y = %.3f ", data.y); + displayTextLine(&dev, 3, false, false, "radius = %.3f", data.radius); + displayTextLine(&dev, 4, false, false, "angle = %-06.3f ", data.angle); + displayTextLine(&dev, 5, false, false, "pos=%-12s ", joystickPosStr[(int)data.position]); + displayTextLine(&dev, 6, false, false, "adc: %d:%d ", objects->joystick->getRawX(), objects->joystick->getRawY()); + displayTextLine(&dev, 7, false, false, "mode=%s ", objects->control->getCurrentModeStr()); + vTaskDelay(STATUS_SCREEN_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS); +} + + +//############################# +//##### showScreen motors ##### +//############################# +// shows speed of each motor in km/h large in two lines and RPM in last line +#define STATUS_SCREEN_MOTORS_UPDATE_INTERVAL 150 +void showStatusScreenMotors(display_task_parameters_t *objects) +{ + displayTextLine(&dev, 0, true, false, "%-4.0fW ", fabs(objects->motorLeft->getCurrentA()) * getBatteryVoltage()); + displayTextLine(&dev, 3, true, false, "%-4.0fW ", fabs(objects->motorRight->getCurrentA()) * getBatteryVoltage()); + //displayTextLine(&dev, 0, true, false, "L:%02.0f%%", objects->motorLeft->getStatus().duty); + //displayTextLine(&dev, 3, true, false, "R:%02.0f%%", objects->motorRight->getStatus().duty); + displayTextLineCentered(&dev, 6, false, false, "%+03.0f%% | %+03.0f%% DTY", + objects->motorLeft->getStatus().duty, + objects->motorRight->getStatus().duty); + displayTextLineCentered(&dev, 7, false, false, "%+04.0f | %+04.0f RPM", + objects->speedLeft->getRpm(), + objects->speedRight->getRpm()); + vTaskDelay(STATUS_SCREEN_MOTORS_UPDATE_INTERVAL / portTICK_PERIOD_MS); +} + + +// ################################ +// #### showScreen Screensaver #### +// ################################ +// show inactivity duration and battery perventage scrolling across screen the entire screen to prevent burn in +#define STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS 10 * 1000 +#define STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL 500 +#define DISPLAY_HORIZONTAL_CHARACTER_COUNT 16 +#define DISPLAY_VERTICAL_LINE_COUNT 8 +void showStatusScreenScreensaver(display_task_parameters_t *objects) +{ + //-- variables for line rotation -- + static int msPassed = 0; + static int currentLine = 0; + static bool lineChanging = false; + // clear display once when rotating to next line + if (lineChanging) + { + ssd1306_clear_screen(&dev, false); + lineChanging = false; + } + //-- print 2 lines scrolling horizontally -- +#ifdef HARDWARE_SCROLL_AVAILABLE // when display supports hardware scrolling -> only the content has to be updated + // note: scrolling is enabled at screen change (display_selectStatusPage()) + // update text every iteration to prevent empty screen at start + displayTextLine(&dev, currentLine, false, false, "IDLE since:"); + displayTextLine(&dev, currentLine + 1, false, false, "%.1fh, B:%02.0f%%", + (float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60, + getBatteryPercent()); + // note: scrolling is disabled at screen change (display_selectStatusPage()) +#else // custom implementation to scroll the text 1 character to the right every iteration (also wraps over the end to beginning) + static int offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT; + char buf1[64], buf2[64]; + // scroll text left to right (taken window of the string moves to the left => offset 16->0, 16->0 ...) + offset -= 1; + if (offset < 0) + offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT - 1; // 0 = no crop -> start over with crop + // note: these strings have to be symetrical and 2x display character count long + snprintf(buf1, 64, "IDLE since: IDLE since: "); + snprintf(buf2, 64, "%.1fh, B:%02.0f%% %.1fh, B:%02.0f%% ", + (float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60, + getBatteryPercent(), + (float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60, + getBatteryPercent()); + // print strings on display while limiting to certain window (ignore certain count of characters at start) + displayTextLine(&dev, currentLine, false, false, "%s", buf1 + offset); + displayTextLine(&dev, currentLine + 1, false, false, "%s", buf2 + offset); +#endif + //-- handle line rotation -- + // to not block the display task for several seconds returning every e.g. 500ms here + // -> ensures detection of activity (exit condition) in task loop is handled regularly + if (msPassed > STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS) // switch to next line is due + { + msPassed = 0; // rest seconds count + // increment / rotate to next line + if (++currentLine >= DISPLAY_VERTICAL_LINE_COUNT - 1) // rotate to next line + currentLine = 0; + lineChanging = true; // clear screen in next run + } + //-- wait update interval -- + // wait and increment passed time after each run + vTaskDelay(STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL / portTICK_PERIOD_MS); + msPassed += STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL; +} + + +//######################## +//#### showStartupMsg #### +//######################## +//shows welcome message and information about current version +void showStartupMsg(){ + const esp_app_desc_t * desc = esp_ota_get_app_description(); + + //show message + displayTextLine(&dev, 0, true, false, "START"); + //show git-tag + displayTextLine(&dev, 4, false, false, "%s", desc->version); + //show build-date (note: date,time of last clean build) + displayTextLine(&dev, 6, false, false, "%s", desc->date); + //show build-time + displayTextLine(&dev, 7, false, false, "%s", desc->time); +} + + + +//============================ +//===== selectStatusPage ===== +//============================ +void display_selectStatusPage(displayStatusPage_t newStatusPage) +{ + // get number of available screens + const displayStatusPage_t max = STATUS_SCREEN_SCREENSAVER; + const uint8_t maxItems = (uint8_t)max; + // limit to available pages + if (newStatusPage > maxItems) newStatusPage = (displayStatusPage_t)(maxItems); + else if (newStatusPage < 0) newStatusPage = (displayStatusPage_t)0; + + //-- run commands when switching FROM certain mode -- + switch (selectedStatusPage) + { +#ifdef HARDWARE_SCROLL_AVAILABLE + case STATUS_SCREEN_SCREENSAVER: + ssd1306_hardware_scroll(&dev, SCROLL_STOP); // disable scrolling when exiting screensaver + break; +#endif + default: + break; } - //scale voltage linear to percent in matched range - for (int i = 1; i < size; ++i) { - if (cellVoltage <= voltageLevels[i]) { - float voltageRange = voltageLevels[i] - voltageLevels[i - 1]; - float voltageOffset = cellVoltage - voltageLevels[i - 1]; - float percentageRange = percentageLevels[i] - percentageLevels[i - 1]; - float percentageOffset = percentageLevels[i - 1]; - float percent = percentageOffset + (voltageOffset / voltageRange) * percentageRange; - ESP_LOGD(TAG, "getBatPercent - cellVoltage=%.3f => percentage=%.3f", cellVoltage, percent); - ESP_LOGD(TAG, "getBatPercent - matched range: %.2fV-%.2fV => %.1f%%-%.1f%%", voltageLevels[i-1], voltageLevels[i], percentageLevels[i-1], percentageLevels[i]); - return percent; + ESP_LOGW(TAG, "switching statusPage from %d to %d", (int)selectedStatusPage, (int)newStatusPage); + selectedStatusPage = newStatusPage; + + //-- run commands when switching TO certain mode -- + switch (selectedStatusPage) + { + case STATUS_SCREEN_SCREENSAVER: + ssd1306_clear_screen(&dev, false); // clear screen when switching +#ifdef HARDWARE_SCROLL_AVAILABLE + ssd1306_hardware_scroll(&dev, SCROLL_RIGHT); +#endif + break; + default: + break; + } +} + +//============================ +//===== rotateStatusPage ===== +//============================ +// select next/previous status screen and rotate to start/end (uses all available in struct) +void display_rotateStatusPage(bool reverseDirection, bool noRotate) +{ + // get number of available screens + const displayStatusPage_t max = STATUS_SCREEN_SCREENSAVER; + const uint8_t maxItems = (uint8_t)max - 1; // screensaver is not relevant + + if (reverseDirection == false) // rotate next + { + if (selectedStatusPage >= maxItems) // already at last item + { + if (noRotate) + return; // stay at last item when rotating disabled + display_selectStatusPage((displayStatusPage_t)0); // rotate to first item + } + else + // select next screen + display_selectStatusPage((displayStatusPage_t)((int)selectedStatusPage + 1)); + ssd1306_clear_screen(&dev, false); // clear screen when switching + } + else // rotate back + { + if (selectedStatusPage <= 0) // already at first item + { + if (noRotate) + return; // stay at first item when rotating disabled + display_selectStatusPage((displayStatusPage_t)(maxItems)); // rotate to last item + } + else + // select previous screen + display_selectStatusPage((displayStatusPage_t)((int)selectedStatusPage - 1)); + ssd1306_clear_screen(&dev, false); // clear screen when switching + } +} + + +//========================== +//=== handleStatusScreen === +//========================== +//show currently selected status screen on display +//function is repeatedly called by display task when not in MENU mode +void handleStatusScreen(display_task_parameters_t *objects) +{ + switch (selectedStatusPage) + { + default: + case STATUS_SCREEN_OVERVIEW: + showStatusScreenOverview(objects); + break; + case STATUS_SCREEN_SPEED: + showStatusScreenSpeed(objects); + break; + case STATUS_SCREEN_JOYSTICK: + showStatusScreenJoystick(objects); + break; + case STATUS_SCREEN_MOTORS: + showStatusScreenMotors(objects); + break; + case STATUS_SCREEN_SCREENSAVER: + showStatusScreenScreensaver(objects); + break; + } + + //--- handle timeouts --- + uint32_t inactiveMs = objects->control->getInactivityDurationMs(); + //-- screensaver -- + // handle switch to screensaver when no user input for a long time + if (inactiveMs > displayConfig.timeoutSwitchToScreensaverMs) // timeout - switch to screensaver is due + { + if (selectedStatusPage != STATUS_SCREEN_SCREENSAVER) + { // switch/log only once at change + ESP_LOGW(TAG, "no activity for more than %d min, switching to screensaver", inactiveMs / 1000 / 60); + display_selectStatusPage(STATUS_SCREEN_SCREENSAVER); } } - ESP_LOGE(TAG, "getBatteryPercent - unknown voltage range"); - return 0.0; //unknown range + else if (selectedStatusPage == STATUS_SCREEN_SCREENSAVER) // exit screensaver when there was recent activity + { + ESP_LOGW(TAG, "recent activity detected, disabling screensaver"); + display_selectStatusPage(STATUS_SCREEN_OVERVIEW); + } + + //-- reduce brightness -- + // handle brightness reduction when no user input for some time + static bool brightnessIsReduced = false; + if (inactiveMs > displayConfig.timeoutReduceContrastMs) // threshold exceeded - reduction of brightness is due + { + if (!brightnessIsReduced) // change / log only once at change + { + // reduce display brightness (less burn in) + ESP_LOGW(TAG, "no activity for more than %d min, reducing display brightness to %d/255", inactiveMs / 1000 / 60, displayConfig.contrastReduced); + ssd1306_contrast(&dev, displayConfig.contrastReduced); + brightnessIsReduced = true; + } + } + else if (brightnessIsReduced) // threshold not exceeded anymore, but still reduced + { + // increase display brighness again + ESP_LOGW(TAG, "recent activity detected, increasing brightness again"); + ssd1306_contrast(&dev, displayConfig.contrastNormal); + brightnessIsReduced = false; + } } -float getBatteryPercent(){ - float voltage = getBatteryVoltage(); - return getBatteryPercent(voltage); -} - - - //============================ //======= display task ======= //============================ -#define VERY_SLOW_LOOP_INTERVAL 30000 -#define SLOW_LOOP_INTERVAL 1000 -#define FAST_LOOP_INTERVAL 200 -//TODO: separate taks for each loop? +// TODO: separate task for each loop? +void display_task(void *pvParameters) +{ + ESP_LOGW(TAG, "Initializing display and starting handle loop"); + //get struct with pointers to all needed global objects from task parameter + display_task_parameters_t *objects = (display_task_parameters_t *)pvParameters; -void display_task( void * pvParameters ){ - char buf[20]; - char buf1[20]; - int len, len1; - int countFastloop = SLOW_LOOP_INTERVAL; - int countSlowLoop = VERY_SLOW_LOOP_INTERVAL; + // initialize display + display_init(objects->displayConfig); + // TODO check if successfully initialized - display_init(); - //TODO check if successfully initialized + // show startup message + showStartupMsg(); + vTaskDelay(STARTUP_MSG_TIMEOUT / portTICK_PERIOD_MS); + ssd1306_clear_screen(&dev, false); - //welcome msg - strcpy(buf, "Hello"); - ssd1306_display_text_x3(&dev, 0, buf, 5, false); - vTaskDelay(1000 / portTICK_PERIOD_MS); + // repeatedly update display with content depending on current mode + while (1) + { + switch (objects->control->getCurrentMode()) + { + case controlMode_t::MENU_SETTINGS: + // uses encoder events to control menu (settings) and updates display + handleMenu_settings(objects, &dev); + break; + case controlMode_t::MENU_MODE_SELECT: + // uses encoder events to control menu (mode select) and updates display + handleMenu_modeSelect(objects, &dev); + break; + default: + // show selected status screen in any other mode + handleStatusScreen(objects); + break; + } // end mode switch-case + // TODO add pages and menus here + } // end while(1) +} // end display-task - //update stats - while(1){ - if (countFastloop >= SLOW_LOOP_INTERVAL / FAST_LOOP_INTERVAL){ - //---- very slow loop ---- - if (countSlowLoop >= VERY_SLOW_LOOP_INTERVAL/SLOW_LOOP_INTERVAL){ - //clear display - workaround for bugged line order after a few minutes - countSlowLoop = 0; - ssd1306_clear_screen(&dev, false); - } - //---- slow loop ---- - countSlowLoop ++; - countFastloop = 0; - //--- battery stats --- - //TODO update only when no load (currentsensors = ~0A) - float battVoltage = getBatteryVoltage(); - float battPercent = getBatteryPercent(battVoltage); - len = snprintf(buf, sizeof(buf), "Bat:%.1fV %.2fV", battVoltage, battVoltage/BAT_CELL_COUNT); - len1 = snprintf(buf1, sizeof(buf1), "B:%02.0f%%", battPercent); - ssd1306_display_text_x3(&dev, 0, buf1, len1, false); - ssd1306_display_text(&dev, 3, buf, len, false); - ssd1306_display_text(&dev, 4, buf, len, true); - } - - //---- fast loop ---- - //update speed/rpm - float sLeft = speedLeft.getKmph(); - float rLeft = speedLeft.getRpm(); - float sRight = speedRight.getKmph(); - float rRight = speedRight.getRpm(); - len = snprintf(buf, sizeof(buf), "L:%.1f R:%.1fkm/h", fabs(sLeft), fabs(sRight)); - ssd1306_display_text(&dev, 5, buf, len, false); - len = snprintf(buf, sizeof(buf), "L:%4.0f R:%4.0fRPM", rLeft, rRight); - ssd1306_display_text(&dev, 6, buf, len, false); - //debug speed sensors - ESP_LOGD(TAG, "%s", buf); - //TODO show currentsensor values - - vTaskDelay(FAST_LOOP_INTERVAL / portTICK_PERIOD_MS); - countFastloop++; - } - //TODO add pages and menus @@ -309,15 +731,4 @@ void display_task( void * pvParameters ){ //// Fade Out - //ssd1306_fadeout(&dev); - -#if 0 - // Fade Out - for(int contrast=0xff;contrast>0;contrast=contrast-0x20) { - ssd1306_contrast(&dev, contrast); - vTaskDelay(40); - } -#endif - -} - + //ssd1306_fadeout(&dev); \ No newline at end of file diff --git a/board_single/main/display.hpp b/board_single/main/display.hpp index 98c8b79..791246e 100644 --- a/board_single/main/display.hpp +++ b/board_single/main/display.hpp @@ -1,18 +1,80 @@ +#pragma once + extern "C" { #include #include #include +#include #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_log.h" +#include "nvs_flash.h" +#include "nvs.h" #include "ssd1306.h" #include "font8x8_basic.h" } -#include "config.hpp" +#include "joystick.hpp" +#include "control.hpp" +#include "speedsensor.hpp" + +// configuration for initializing display (passed to task as well) +typedef struct display_config_t { + // initialization + gpio_num_t gpio_scl; + gpio_num_t gpio_sda; + int gpio_reset; // negative number means reset pin is not connected or not used + int width; + int height; + int offsetX; + bool flip; + // display-task + int contrastNormal; + int contrastReduced; + uint32_t timeoutReduceContrastMs; + uint32_t timeoutSwitchToScreensaverMs; +} display_config_t; + + +// struct with variables passed to task from main() +typedef struct display_task_parameters_t { + display_config_t displayConfig; + controlledArmchair * control; + evaluatedJoystick * joystick; + QueueHandle_t encoderQueue; + controlledMotor * motorLeft; + controlledMotor * motorRight; + speedSensor * speedLeft; + speedSensor * speedRight; + buzzer_t *buzzer; + nvs_handle_t * nvsHandle; +} display_task_parameters_t; + + +// enum for selecting the currently shown status page (display content when not in MENU_SETTINGS mode) +typedef enum displayStatusPage_t {STATUS_SCREEN_OVERVIEW=0, STATUS_SCREEN_SPEED, STATUS_SCREEN_JOYSTICK, STATUS_SCREEN_MOTORS, STATUS_SCREEN_SCREENSAVER, __NUMBER_OF_AVAILABLE_SCREENS} displayStatusPage_t; //note: SCREENSAVER has to be last one since it is ignored by rotate and used to determine count + +// get precise battery voltage (using lookup table) +float getBatteryVoltage(); + +// get battery charge level in percent (using lookup table as discharge curve) +float getBatteryPercent(); + +// function to select one of the defined status screens which are shown on display when not in MENU_SETTINGS or MENU_SELECT_MODE mode +void display_selectStatusPage(displayStatusPage_t newStatusPage); +// select next/previous status screen to be shown, when noRotate is set is stays at first/last screen +void display_rotateStatusPage(bool reverseDirection = false, bool noRotate = false); //task that inititialized the display, displays welcome message //and releatedly updates the display with certain content void display_task( void * pvParameters ); + +//abstracted function for printing one line on the display, using a format string directly +//and options: Large-font (3 lines, max 5 digits), or inverted color +void displayTextLine(SSD1306_t *display, int line, bool large, bool inverted, const char *format, ...); + +//abstracted function for printing a string CENTERED on the display, using a format string +//adds spaces left and right to fill the line (if not too long already) +void displayTextLineCentered(SSD1306_t *display, int line, bool isLarge, bool inverted, const char *format, ...); \ No newline at end of file diff --git a/board_single/main/encoder.cpp b/board_single/main/encoder.cpp new file mode 100644 index 0000000..2912a7c --- /dev/null +++ b/board_single/main/encoder.cpp @@ -0,0 +1,75 @@ +extern "C" +{ +#include +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "esp_log.h" + +#include "encoder.h" +} + +#include "encoder.hpp" + +//------------------------- +//------- variables ------- +//------------------------- +static const char * TAG = "encoder"; + + + +//================================== +//========== encoder_init ========== +//================================== +//initialize encoder //TODO pass config to this function +QueueHandle_t encoder_init(rotary_encoder_t * encoderConfig) +{ + QueueHandle_t encoderQueue = xQueueCreate(QUEUE_SIZE, sizeof(rotary_encoder_event_t)); + rotary_encoder_init(encoderQueue); + rotary_encoder_add(encoderConfig); + if (encoderQueue == NULL) + ESP_LOGE(TAG, "Error initializing encoder or queue"); + else + ESP_LOGW(TAG, "Initialized encoder and encoderQueue"); + return encoderQueue; +} + + + +//================================== +//====== task_encoderExample ======= +//================================== +//receive and handle all available encoder events +void task_encoderExample(void * arg) { + //get queue with encoder events from task parameter: + QueueHandle_t encoderQueue = (QueueHandle_t)arg; + static rotary_encoder_event_t ev; //store event data + while (1) { + if (xQueueReceive(encoderQueue, &ev, portMAX_DELAY)) { + //log enocder events + switch (ev.type){ + case RE_ET_CHANGED: + ESP_LOGI(TAG, "Event type: RE_ET_CHANGED, diff: %d", ev.diff); + break; + case RE_ET_BTN_PRESSED: + ESP_LOGI(TAG, "Button pressed"); + break; + case RE_ET_BTN_RELEASED: + ESP_LOGI(TAG, "Button released"); + break; + case RE_ET_BTN_CLICKED: + ESP_LOGI(TAG, "Button clicked"); + break; + case RE_ET_BTN_LONG_PRESSED: + ESP_LOGI(TAG, "Button long-pressed"); + break; + default: + ESP_LOGW(TAG, "Unknown event type"); + break; + } + } + } +} + diff --git a/board_single/main/encoder.hpp b/board_single/main/encoder.hpp new file mode 100644 index 0000000..79c9198 --- /dev/null +++ b/board_single/main/encoder.hpp @@ -0,0 +1,17 @@ +extern "C" { +#include "freertos/FreeRTOS.h" // FreeRTOS related headers +#include "freertos/task.h" +#include "encoder.h" +} + +//config +#define QUEUE_SIZE 10 + +//init encoder with pointer to encoder config +QueueHandle_t encoder_init(rotary_encoder_t * encoderConfig); + + +//task that handles encoder events +//note: queue obtained from encoder_init() has to be passed to that task +void task_encoderExample(void *encoderQueue); +//example: xTaskCreate(&task_encoderExample, "task_buzzer", 2048, encoderQueue, 2, NULL); \ No newline at end of file diff --git a/board_single/main/fan.cpp b/board_single/main/fan.cpp index 2ff094e..b65be24 100644 --- a/board_single/main/fan.cpp +++ b/board_single/main/fan.cpp @@ -12,6 +12,28 @@ extern "C" static const char * TAG = "fan-control"; +//======================================= +//============== fan task =============== +//======================================= +//task that controlls fans for cooling the drivers +//turns fan on/off depending on motor duty history +void task_fans( void * task_fans_parameters ){ + //get configuration struct from task parameter + task_fans_parameters_t *objects = (task_fans_parameters_t *)task_fans_parameters; + + //create fan instances with config defined in config.cpp + ESP_LOGI(TAG, "Initializing fans and starting fan handle loop"); + controlledFan fan(objects->fan_config, objects->motorLeft, objects->motorRight); + + //repeatedly run fan handle function in a slow loop + while(1){ + fan.handle(); + vTaskDelay(500 / portTICK_PERIOD_MS); + } +} + + + //----------------------------- //-------- constructor -------- //----------------------------- diff --git a/board_single/main/fan.hpp b/board_single/main/fan.hpp index ca1de2b..bce828a 100644 --- a/board_single/main/fan.hpp +++ b/board_single/main/fan.hpp @@ -16,7 +16,22 @@ typedef struct fan_config_t { uint32_t minOnMs; uint32_t minOffMs; uint32_t turnOffDelayMs; -} fan_config; +} fan_config_t; + + +// struct with variables passed to task from main +typedef struct task_fans_parameters_t { + fan_config_t fan_config; + controlledMotor * motorLeft; + controlledMotor * motorRight; +} task_fans_parameters_t; + + +//==================================== +//========== motorctl task =========== +//==================================== +//note: pointer to task_fans_parameters_t has to be passed as task-parameter (config, motor objects) +void task_fans( void * task_fans_parameters ); diff --git a/board_single/main/main.cpp b/board_single/main/main.cpp index 693e1c2..1c3a538 100644 --- a/board_single/main/main.cpp +++ b/board_single/main/main.cpp @@ -1,12 +1,10 @@ -#include "hal/uart_types.h" -#include "motordrivers.hpp" -#include "types.hpp" extern "C" { #include #include #include #include +#include "nvs.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "driver/gpio.h" @@ -14,98 +12,94 @@ extern "C" #include "sdkconfig.h" #include "esp_spiffs.h" -#include "driver/ledc.h" - //custom C files #include "wifi.h" } +#include + //custom C++ files -#include "config.hpp" +//folder common +#include "uart_common.hpp" +#include "motordrivers.hpp" +#include "http.hpp" +#include "speedsensor.hpp" +#include "motorctl.hpp" + +//folder single_board #include "control.hpp" #include "button.hpp" -#include "http.hpp" - -#include "uart_common.hpp" - #include "display.hpp" +#include "encoder.hpp" -//tag for logging +//only extends this file (no library): +//outsourced all configuration related structures +#include "config.cpp" + + + +//================================ +//======== declarations ========== +//================================ +//--- declare all pointers to shared objects --- +controlledMotor *motorLeft; +controlledMotor *motorRight; + +// TODO initialize driver in createOjects like everything else +// (as in 6e9b3d96d96947c53188be1dec421bd7ff87478e) +// issue with laggy encoder wenn calling methods via pointer though +//sabertooth2x60a *sabertoothDriver; +sabertooth2x60a sabertoothDriver(sabertoothConfig); + +evaluatedJoystick *joystick; + +buzzer_t *buzzer; + +controlledArmchair *control; + +automatedArmchair_c *automatedArmchair; + +httpJoystick *httpJoystickMain; + +speedSensor *speedLeft; +speedSensor *speedRight; + +cControlledRest *legRest; +cControlledRest *backRest; + + +//--- lambda functions motor-driver --- +// functions for updating the duty via currently used motor driver (hardware) that can then be passed to controlledMotor +//-> makes it possible to easily use different motor drivers +motorSetCommandFunc_t setLeftFunc = [&sabertoothDriver](motorCommand_t cmd) +{ + //TODO why encoder lag when call via pointer? + sabertoothDriver.setLeft(cmd); +}; +motorSetCommandFunc_t setRightFunc = [&sabertoothDriver](motorCommand_t cmd) +{ + sabertoothDriver.setRight(cmd); +}; + +//--- lambda function http-joystick --- +// function that initializes the http server requires a function pointer to function that handels each url +// the httpd_uri config struct does not accept a pointer to a method of a class instance, directly +// thus this lambda function is necessary: +// declare pointer to receiveHttpData method of httpJoystick class +esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData; +esp_err_t on_joystick_url(httpd_req_t *req) +{ + // run pointer to receiveHttpData function of httpJoystickMain instance + return (httpJoystickMain->*pointerToReceiveFunc)(req); +} + +//--- tag for logging --- static const char * TAG = "main"; +//-- handle passed to tasks for accessing nvs -- +nvs_handle_t nvsHandle; -//==================================== -//========== motorctl task =========== -//==================================== -//task for handling the motors (ramp, current limit, driver) -void task_motorctl( void * pvParameters ){ - ESP_LOGI(TAG, "starting handle loop..."); - while(1){ - motorRight.handle(); - motorLeft.handle(); - //10khz -> T=100us - vTaskDelay(10 / portTICK_PERIOD_MS); - } -} - - - -//====================================== -//============ buzzer task ============= -//====================================== -//TODO: move the task creation to buzzer class (buzzer.cpp) -//e.g. only have function buzzer.createTask() in app_main -void task_buzzer( void * pvParameters ){ - ESP_LOGI("task_buzzer", "Start of buzzer task..."); - //run function that waits for a beep events to arrive in the queue - //and processes them - buzzer.processQueue(); -} - - - -//======================================= -//============ control task ============= -//======================================= -//task that controls the armchair modes and initiates commands generation and applies them to driver -void task_control( void * pvParameters ){ - ESP_LOGI(TAG, "Initializing controlledArmchair and starting handle loop"); - //start handle loop (control object declared in config.hpp) - control.startHandleLoop(); -} - - - -//====================================== -//============ button task ============= -//====================================== -//task that handles the button interface/commands -void task_button( void * pvParameters ){ - ESP_LOGI(TAG, "Initializing command-button and starting handle loop"); - //create button instance - buttonCommands commandButton(&buttonJoystick, &joystick, &control, &buzzer, &motorLeft, &motorRight); - //start handle loop - commandButton.startHandleLoop(); -} - - - -//======================================= -//============== fan task =============== -//======================================= -//task that controlls fans for cooling the drivers -void task_fans( void * pvParameters ){ - ESP_LOGI(TAG, "Initializing fans and starting fan handle loop"); - //create fan instances with config defined in config.cpp - controlledFan fan(configCooling, &motorLeft, &motorRight); - //repeatedly run fan handle function in a slow loop - while(1){ - fan.handle(); - vTaskDelay(500 / portTICK_PERIOD_MS); - } -} - //================================= @@ -113,7 +107,6 @@ void task_fans( void * pvParameters ){ //================================= //initialize spi flash filesystem (used for webserver) void init_spiffs(){ - ESP_LOGI(TAG, "init spiffs"); esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = { .base_path = "/spiffs", .partition_label = NULL, @@ -131,29 +124,54 @@ void init_spiffs(){ -//================================== -//======== define loglevels ======== -//================================== -void setLoglevels(void){ - //set loglevel for all tags: - esp_log_level_set("*", ESP_LOG_WARN); - //--- set loglevel for individual tags --- - esp_log_level_set("main", ESP_LOG_INFO); - //esp_log_level_set("buzzer", ESP_LOG_INFO); - //esp_log_level_set("motordriver", ESP_LOG_DEBUG); - //esp_log_level_set("motor-control", ESP_LOG_INFO); - //esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); - //esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); - esp_log_level_set("button", ESP_LOG_INFO); - esp_log_level_set("control", ESP_LOG_INFO); - //esp_log_level_set("fan-control", ESP_LOG_INFO); - esp_log_level_set("wifi", ESP_LOG_INFO); - esp_log_level_set("http", ESP_LOG_INFO); - //esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG); - esp_log_level_set("display", ESP_LOG_INFO); - //esp_log_level_set("current-sensors", ESP_LOG_INFO); - //esp_log_level_set("speedSensor", ESP_LOG_INFO); +//================================= +//========= createObjects ========= +//================================= +//create all shared objects +//their references can be passed to the tasks that need access in main + +//Note: the configuration structures (e.g. configMotorControlLeft) are outsourced to file 'config.cpp' + +void createObjects() +{ + // create sabertooth motor driver instance + // sabertooth2x60a sabertoothDriver(sabertoothConfig); + // with configuration above + //sabertoothDriver = new sabertooth2x60a(sabertoothConfig); + + // create speedsensor instances + // with configurations from config.cpp + speedLeft = new speedSensor(speedLeft_config); + speedRight = new speedSensor(speedRight_config); + + // create controlled motor instances (motorctl.hpp) + // with configurations from config.cpp + motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle, speedLeft, &motorRight); //note: ptr to ptr of controlledMotor since it isnt defined yet + motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle, speedRight, &motorLeft); + + // create joystick instance (joystick.hpp) + joystick = new evaluatedJoystick(configJoystick, &nvsHandle); + + // create httpJoystick object (http.hpp) + httpJoystickMain = new httpJoystick(configHttpJoystickMain); + http_init_server(on_joystick_url); + + // create buzzer object on pin 12 with gap between queued events of 1ms + buzzer = new buzzer_t(GPIO_NUM_12, 1); + + // create objects for controlling the chair position + // gpio_up, gpio_down, name + legRest = new cControlledRest(GPIO_NUM_2, GPIO_NUM_15, "legRest"); + backRest = new cControlledRest(GPIO_NUM_16, GPIO_NUM_4, "backRest"); + + // create control object (control.hpp) + // with configuration from config.cpp + control = new controlledArmchair(configControl, buzzer, motorLeft, motorRight, joystick, &joystickGenerateCommands_config, httpJoystickMain, automatedArmchair, legRest, backRest, &nvsHandle); + + // create automatedArmchair_c object (for auto-mode) (auto.hpp) + automatedArmchair = new automatedArmchair_c(motorLeft, motorRight); + } @@ -163,70 +181,119 @@ void setLoglevels(void){ //=========== app_main ============ //================================= extern "C" void app_main(void) { - //enable 5V volate regulator + ESP_LOGW(TAG, "===== BOOT (pre main) Completed =====\n"); + + ESP_LOGW(TAG, "===== INITIALIZING COMPONENTS ====="); + //--- define log levels --- + setLoglevels(); + + //--- enable 5V volate regulator --- ESP_LOGW(TAG, "enabling 5V regulator..."); gpio_pad_select_gpio(GPIO_NUM_17); gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); gpio_set_level(GPIO_NUM_17, 1); - //---- define log levels ---- - setLoglevels(); + //--- initialize nvs-flash and netif --- + ESP_LOGW(TAG,"initializing NVS..."); + wifi_initNvs(); //needed for wifi and persistent config variables + ESP_LOGW(TAG,"initializing NETIF..."); + wifi_initNetif(); // needed for wifi + + //--- initialize spiffs --- + ESP_LOGW(TAG, "initializing SPIFFS..."); + init_spiffs(); // used by httpd server + + //--- initialize and start wifi --- + // Note: now started only when switching to HTTP mode in control.cpp + // ESP_LOGW(TAG,"starting wifi..."); + // wifi_start_client(); //connect to existing wifi (dropped) + // wifi_start_ap(); //start access point + + //--- initialize encoder --- + const QueueHandle_t encoderQueue = encoder_init(&encoder_config); + + //--- open nvs-flash --- + // note: nvs already initialized in wifi_initNvs() + ESP_LOGW(TAG, "opening NVS-handle..."); + esp_err_t err = nvs_open("storage", NVS_READWRITE, &nvsHandle); // this handle is passed to all tasks for accessing nvs + if (err != ESP_OK) + ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err)); + + printf("\n"); + + + + //--- create all objects --- + ESP_LOGW(TAG, "===== CREATING SHARED OBJECTS ====="); + + //initialize sabertooth object in STACK (due to performance issues in heap) + ///sabertoothDriver = static_cast(alloca(sizeof(sabertooth2x60a))); + ///new (sabertoothDriver) sabertooth2x60a(sabertoothConfig); + + //create all class instances used below in HEAP + createObjects(); + + printf("\n"); + + + + //--- create tasks --- + ESP_LOGW(TAG, "===== CREATING TASKS ====="); //---------------------------------------------- //--- create task for controlling the motors --- //---------------------------------------------- - //task that receives commands, handles ramp and current limit and executes commands using the motordriver function - xTaskCreate(&task_motorctl, "task_motor-control", 2*4096, NULL, 6, NULL); + //task for each motor that handles to following: + //receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr) + xTaskCreate(&task_motorctl, "task_ctl-left-motor", 2*4096, motorLeft, 6, NULL); + xTaskCreate(&task_motorctl, "task_ctl-right-motor", 2*4096, motorRight, 6, NULL); //------------------------------ //--- create task for buzzer --- //------------------------------ - xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL); + //task that processes queued beeps + //note: pointer to shard object 'buzzer' is passed as task parameter: + xTaskCreate(&task_buzzer, "task_buzzer", 2048, buzzer, 2, NULL); //------------------------------- //--- create task for control --- //------------------------------- //task that generates motor commands depending on the current mode and sends those to motorctl task - xTaskCreate(&task_control, "task_control", 4096, NULL, 5, NULL); + //note: pointer to shared object 'control' is passed as task parameter: + xTaskCreate(&task_control, "task_control", 4096, control, 5, NULL); //------------------------------ //--- create task for button --- //------------------------------ - //task that evaluates and processes the button input and runs the configured commands - xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL); + //task that handles button/encoder events in any mode except 'MENU_SETTINGS' and 'MENU_MODE_SELECT' (e.g. switch modes by pressing certain count) + task_button_parameters_t button_param = {control, joystick, encoderQueue, motorLeft, motorRight, buzzer}; + xTaskCreate(&task_button, "task_button", 4096, &button_param, 3, NULL); //----------------------------------- //--- create task for fan control --- //----------------------------------- - //task that evaluates and processes the button input and runs the configured commands - xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL); - + //task that controls cooling fans of the motor driver + task_fans_parameters_t fans_param = {configFans, motorLeft, motorRight}; + xTaskCreate(&task_fans, "task_fans", 2048, &fans_param, 1, NULL); //----------------------------------- //----- create task for display ----- //----------------------------------- - //task that handles the display - xTaskCreate(&display_task, "display_task", 3*2048, NULL, 1, NULL); + //task that handles the display (show stats, handle menu in 'MENU_SETTINGS' and 'MENU_MODE_SELECT' mode) + display_task_parameters_t display_param = {display_config, control, joystick, encoderQueue, motorLeft, motorRight, speedLeft, speedRight, buzzer, &nvsHandle}; + xTaskCreate(&display_task, "display_task", 3*2048, &display_param, 3, NULL); + + vTaskDelay(200 / portTICK_PERIOD_MS); //wait for all tasks to finish initializing + printf("\n"); - //beep at startup - buzzer.beep(3, 70, 50); - //--- initialize nvs-flash and netif (needed for wifi) --- - wifi_initNvs_initNetif(); - - //--- initialize spiffs --- - init_spiffs(); - - //--- initialize and start wifi --- - //FIXME: run wifi_init_client or wifi_init_ap as intended from control.cpp when switching state - //currently commented out because of error "assert failed: xQueueSemaphoreTake queue.c:1549 (pxQueue->uxItemSize == 0)" when calling control->changeMode from button.cpp - //when calling control.changeMode(http) from main.cpp it worked without error for some reason? - ESP_LOGI(TAG,"starting wifi..."); - //wifi_init_client(); //connect to existing wifi - wifi_init_ap(); //start access point - ESP_LOGI(TAG,"done starting wifi"); + //--- startup finished --- + ESP_LOGW(TAG, "===== STARTUP FINISHED =====\n"); + buzzer->beep(3, 70, 50); + //--- testing encoder --- + //xTaskCreate(&task_encoderExample, "task_buzzer", 2048, encoderQueue, 2, NULL); //--- testing http server --- // wifi_init_client(); //connect to existing wifi @@ -235,15 +302,17 @@ extern "C" void app_main(void) { // http_init_server(); - //--- testing force http mode after startup --- - //control.changeMode(controlMode_t::HTTP); + //--- testing force specific mode after startup --- + //control->changeMode(controlMode_t::MENU_SETTINGS); //--- main loop --- //does nothing except for testing things while(1){ - vTaskDelay(5000 / portTICK_PERIOD_MS); + vTaskDelay(portMAX_DELAY); + //vTaskDelay(5000 / portTICK_PERIOD_MS); + //--------------------------------- //-------- TESTING section -------- //--------------------------------- diff --git a/board_single/main/menu.cpp b/board_single/main/menu.cpp new file mode 100644 index 0000000..19aaf27 --- /dev/null +++ b/board_single/main/menu.cpp @@ -0,0 +1,1086 @@ +extern "C"{ +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "esp_log.h" + +#include "ssd1306.h" +} + +#include "menu.hpp" +#include "encoder.hpp" +#include "motorctl.hpp" + + +//--- variables --- +static const char *TAG = "menu"; +static menuState_t menuState = MAIN_MENU; +static int value = 0; + + + +//================================ +//===== CONFIGURE MENU ITEMS ===== +//================================ +// Instructions / Behavior: +// - when line4 * and line5 * are empty the value is printed large +// - when 3rd element is not NULL (pointer to defaultValue function) return int value of that function is shown in line 2 +// - when 2nd element is NULL (pointer to currentValue function): instead of current value "click to confirm is shown" in line 3 + +//######################### +//#### center Joystick #### +//######################### +void item_centerJoystick_action(display_task_parameters_t * objects, SSD1306_t * display, int value){ + ESP_LOGW(TAG, "defining joystick center"); + objects->joystick->defineCenter(); + objects->buzzer->beep(3, 60, 40); +} +menuItem_t item_centerJoystick = { + item_centerJoystick_action, // function action + NULL, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 0, // valueMax + 0, // valueIncrement + "Center Joystick ", // title + "Center Joystick ", // line1 (above value) + "", // line2 (above value) + "defines current ", // line4 * (below value) + "pos as center ", // line5 * + "", // line6 + "=>long to cancel", // line7 +}; + +// ############################ +// #### calibrate Joystick #### +// ############################ +// continously show/update joystick data on display +#define CALIBRATE_JOYSTICK_UPDATE_INTERVAL 50 +void item_calibrateJoystick_action(display_task_parameters_t *objects, SSD1306_t *display, int value) +{ + //--- variables --- + bool running = true; + joystickCalibrationMode_t mode = X_MIN; + rotary_encoder_event_t event; + int valueNow = 0; + + //-- pre loop instructions -- + ESP_LOGW(TAG, "starting joystick calibration sequence"); + ssd1306_clear_screen(display, false); + + //-- show static lines -- + // show first line (title) + displayTextLine(display, 0, false, true, "calibrate stick"); + // show last line (info) + displayTextLineCentered(display, 7, false, true, " click: confirm "); + // show initital state + displayTextLineCentered(display, 1, true, false, "%s", "X-min"); + + //-- loop until all positions are defined -- + while (running && objects->control->getCurrentMode() == controlMode_t::MENU_SETTINGS) + { + // repeatedly print adc value depending on currently selected axis + switch (mode) + { + case X_MIN: + case X_MAX: + displayTextLineCentered(display, 4, true, false, "%d", valueNow = objects->joystick->getRawX()); // large + break; + case Y_MIN: + case Y_MAX: + displayTextLineCentered(display, 4, true, false, "%d", valueNow = objects->joystick->getRawY()); // large + break; + case X_CENTER: + case Y_CENTER: + displayTextLine(display, 4, false, false, " x = %d", objects->joystick->getRawX()); + displayTextLine(display, 5, false, false, " y = %d", objects->joystick->getRawY()); + displayTextLine(display, 6, false, false, "release & click!"); + break; + } + + // handle encoder event + // save and next when button clicked, exit when long pressed + if (xQueueReceive(objects->encoderQueue, &event, CALIBRATE_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS)) + { + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + case RE_ET_BTN_CLICKED: + objects->buzzer->beep(2, 120, 50); + switch (mode) + { + case X_MIN: + // save x min position + ESP_LOGW(TAG, "calibrate-stick: saving X_MIN"); + objects->joystick->writeCalibration(mode, valueNow); + displayTextLineCentered(display, 1, true, false, "%s", "X-max"); + mode = X_MAX; + break; + case X_MAX: + // save x max position + ESP_LOGW(TAG, "calibrate-stick: saving X_MAX"); + objects->joystick->writeCalibration(mode, valueNow); + displayTextLineCentered(display, 1, true, false, "%s", "Y-min"); + mode = Y_MIN; + break; + case Y_MIN: + // save y min position + ESP_LOGW(TAG, "calibrate-stick: saving Y_MIN"); + objects->joystick->writeCalibration(mode, valueNow); + displayTextLineCentered(display, 1, true, false, "%s", "Y-max"); + mode = Y_MAX; + break; + case Y_MAX: + // save y max position + ESP_LOGW(TAG, "calibrate-stick: saving Y_MAX"); + objects->joystick->writeCalibration(mode, valueNow); + displayTextLineCentered(display, 1, true, false, "%s", "CENTR"); + mode = X_CENTER; + break; + case X_CENTER: + case Y_CENTER: + // save center position + ESP_LOGW(TAG, "calibrate-stick: saving CENTER -> finished"); + objects->joystick->defineCenter(); + // finished + running = false; + break; + } + break; + case RE_ET_BTN_LONG_PRESSED: + //exit to main-menu + objects->buzzer->beep(1, 1000, 10); + ESP_LOGW(TAG, "aborting calibration sqeuence"); + running = false; + case RE_ET_CHANGED: + case RE_ET_BTN_PRESSED: + case RE_ET_BTN_RELEASED: + break; + } + } + } +} + +menuItem_t item_calibrateJoystick = { + item_calibrateJoystick_action, // function action + NULL, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 0, // valueMax + 0, // valueIncrement + "Calibrate Stick ", // title + " Calibrate ", // line1 (above value) + " Joystick ", // line2 (above value) + " click to start ", // line4 * (below value) + " sequence ", // line5 * + " ", // line6 + "=>long to cancel", // line7 +}; + + +//######################## +//#### debug Joystick #### +//######################## +//continously show/update joystick data on display +#define DEBUG_JOYSTICK_UPDATE_INTERVAL 50 +void item_debugJoystick_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + //--- variables --- + bool running = true; + rotary_encoder_event_t event; + + //-- pre loop instructions -- + ESP_LOGW(TAG, "showing joystick debug page"); + ssd1306_clear_screen(display, false); + // show title + displayTextLine(display, 0, false, true, " - debug stick - "); + // show info line + displayTextLineCentered(display, 7, false, true, "click to exit"); + + //-- show/update values -- + // stop when button pressed or control state changes (timeouts to IDLE) + while (running && objects->control->getCurrentMode() == controlMode_t::MENU_SETTINGS) + { + // repeatedly print all joystick data + joystickData_t data = objects->joystick->getData(); + displayTextLine(display, 1, false, false, "x = %.3f ", data.x); + displayTextLine(display, 2, false, false, "y = %.3f ", data.y); + displayTextLine(display, 3, false, false, "radius = %.3f", data.radius); + displayTextLine(display, 4, false, false, "angle = %-06.3f ", data.angle); + displayTextLine(display, 5, false, false, "pos=%-12s ", joystickPosStr[(int)data.position]); + + // exit when button pressed + if (xQueueReceive(objects->encoderQueue, &event, DEBUG_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS)) + { + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + case RE_ET_BTN_CLICKED: + case RE_ET_BTN_LONG_PRESSED: + running = false; + objects->buzzer->beep(1, 100, 10); + break; + case RE_ET_CHANGED: + case RE_ET_BTN_PRESSED: + case RE_ET_BTN_RELEASED: + break; + } + } + } +} + +menuItem_t item_debugJoystick = { + item_debugJoystick_action, // function action + NULL, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 0, // valueMax + 0, // valueIncrement + "Debug joystick ", // title + "Debug joystick ", // line1 (above value) + "", // line2 (above value) + "", // line4 * (below value) + "debug screen ", // line5 * + "prints values ", // line6 + "=>long to cancel", // line7 +}; + + +//######################## +//##### set max duty ##### +//######################## +void maxDuty_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->control->setMaxDuty(value); +} +int maxDuty_currentValue(display_task_parameters_t * objects) +{ + return (int)objects->control->getMaxDuty(); +} +menuItem_t item_maxDuty = { + maxDuty_action, // function action + maxDuty_currentValue, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 1, // valueMin + 100, // valueMax + 1, // valueIncrement + "Set max Duty ", // title + "", // line1 (above value) + " set max-duty: ", // line2 (above value) + "", // line4 * (below value) + "", // line5 * + " 1-100 ", // line6 + " percent ", // line7 +}; + + +//################################## +//##### set max relative boost ##### +//################################## +void maxRelativeBoost_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->control->setMaxRelativeBoostPer(value); +} +int maxRelativeBoost_currentValue(display_task_parameters_t * objects) +{ + return (int)objects->control->getMaxRelativeBoostPer(); +} +menuItem_t item_maxRelativeBoost = { + maxRelativeBoost_action, // function action + maxRelativeBoost_currentValue, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 150, // valueMax + 1, // valueIncrement + "Set max Boost ", // title + "Set max Boost % ", // line1 (above value) + "for outer tire ", // line2 (above value) + "", // line4 * (below value) + "", // line5 * + " % of max duty ", // line6 + "added on turning", // line7 +}; + + +//###################### +//##### accelLimit ##### +//###################### +void item_accelLimit_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->motorLeft->setFade(fadeType_t::ACCEL, (uint32_t)value); + objects->motorRight->setFade(fadeType_t::ACCEL, (uint32_t)value); +} +int item_accelLimit_value(display_task_parameters_t * objects) +{ + return objects->motorLeft->getFade(fadeType_t::ACCEL); +} +int item_accelLimit_default(display_task_parameters_t * objects) +{ + return objects->motorLeft->getFadeDefault(fadeType_t::ACCEL); +} +menuItem_t item_accelLimit = { + item_accelLimit_action, // function action + item_accelLimit_value, // function get initial value or NULL(show in line 2) + item_accelLimit_default, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 10000, // valueMax + 100, // valueIncrement + "Accel limit ", // title + " Fade up time ", // line1 (above value) + "", // line2 <= showing "default = %d" + "", // line4 * (below value) + "", // line5 * + "milliseconds ", // line6 + "from 0 to 100% ", // line7 +}; + + +// ###################### +// ##### decelLimit ##### +// ###################### +void item_decelLimit_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->motorLeft->setFade(fadeType_t::DECEL, (uint32_t)value); + objects->motorRight->setFade(fadeType_t::DECEL, (uint32_t)value); +} +int item_decelLimit_value(display_task_parameters_t * objects) +{ + return objects->motorLeft->getFade(fadeType_t::DECEL); +} +int item_decelLimit_default(display_task_parameters_t * objects) +{ + return objects->motorLeft->getFadeDefault(fadeType_t::DECEL); +} +menuItem_t item_decelLimit = { + item_decelLimit_action, // function action + item_decelLimit_value, // function get initial value or NULL(show in line 2) + item_decelLimit_default, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 10000, // valueMax + 100, // valueIncrement + "Decel limit ", // title + " Fade down time ", // line1 (above value) + "", // line2 <= showing "default = %d" + "", // line4 * (below value) + "", // line5 * + "milliseconds ", // line6 + "from 100 to 0% ", // line7 +}; + + +// ###################### +// ##### brakeDecel ##### +// ###################### +void item_brakeDecel_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->motorLeft->setBrakeDecel((uint32_t)value); + objects->motorRight->setBrakeDecel((uint32_t)value); +} +int item_brakeDecel_value(display_task_parameters_t * objects) +{ + return objects->motorLeft->getBrakeDecel(); +} +int item_brakeDecel_default(display_task_parameters_t * objects) +{ + return objects->motorLeft->getBrakeDecelDefault(); +} +menuItem_t item_brakeDecel = { + item_brakeDecel_action, // function action + item_brakeDecel_value, // function get initial value or NULL(show in line 2) + item_brakeDecel_default, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 10000, // valueMax + 100, // valueIncrement + "Brake decel. ", // title + " Fade down time ", // line1 (above value) + "", // line2 <= showing "default = %d" + "", // line4 * (below value) + "", // line5 * + "milliseconds ", // line6 + "from 100 to 0% ", // line7 +}; + + +//############################### +//### select motorControlMode ### +//############################### +void item_motorControlMode_action(display_task_parameters_t *objects, SSD1306_t *display, int value) +{ + switch (value) + { + case 1: + default: + objects->motorLeft->setControlMode(motorControlMode_t::DUTY); + objects->motorRight->setControlMode(motorControlMode_t::DUTY); + break; + case 2: + objects->motorLeft->setControlMode(motorControlMode_t::CURRENT); + objects->motorRight->setControlMode(motorControlMode_t::CURRENT); + break; + case 3: + objects->motorLeft->setControlMode(motorControlMode_t::SPEED); + objects->motorRight->setControlMode(motorControlMode_t::SPEED); + break; + } +} +int item_motorControlMode_value(display_task_parameters_t *objects) +{ + return 1; // initial value shown / changed from //TODO get actual mode +} +menuItem_t item_motorControlMode = { + item_motorControlMode_action, // function action + item_motorControlMode_value, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 1, // valueMin + 3, // valueMax + 1, // valueIncrement + "Control mode ", // title + " sel. motor ", // line1 (above value) + " control mode ", // line2 (above value) + "1: DUTY (defaul)", // line4 * (below value) + "2: CURRENT", // line5 * + "3: SPEED", // line6 + "", // line7 +}; + +//################################### +//##### Traction Control System ##### +//################################### +void tractionControlSystem_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + if (value == 1){ + objects->motorLeft->enableTractionControlSystem(); + objects->motorRight->enableTractionControlSystem(); + ESP_LOGW(TAG, "enabled Traction Control System"); + } else { + objects->motorLeft->disableTractionControlSystem(); + objects->motorRight->disableTractionControlSystem(); + ESP_LOGW(TAG, "disabled Traction Control System"); + } +} +int tractionControlSystem_currentValue(display_task_parameters_t * objects) +{ + return (int)objects->motorLeft->getTractionControlSystemStatus(); +} +menuItem_t item_tractionControlSystem = { + tractionControlSystem_action, // function action + tractionControlSystem_currentValue, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 1, // valueMax + 1, // valueIncrement + "TCS / ASR ", // title + "Traction Control", // line1 (above value) + " System ", // line2 (above value) + "1: enable ", // line4 * (below value) + "0: disable ", // line5 * + "note: requires ", // line6 + "speed ctl-mode ", // line7 +}; + + +//##################### +//####### RESET ####### +//##################### +void item_reset_action(display_task_parameters_t *objects, SSD1306_t *display, int value) +{ + objects->buzzer->beep(1, 2000, 0); + // close and erase NVS + ESP_LOGW(TAG, "closing and ERASING non-volatile-storage..."); + nvs_close(*(objects->nvsHandle)); + ESP_ERROR_CHECK(nvs_flash_erase()); + // show message restarting + ssd1306_clear_screen(display, false); + displayTextLineCentered(display, 0, false, true, ""); + displayTextLineCentered(display, 1, true, true, "RE-"); + displayTextLineCentered(display, 4, true, true, "START"); + displayTextLineCentered(display, 7, false, true, ""); + vTaskDelay(1000 / portTICK_PERIOD_MS); // wait for buzzer to beep + // restart + ESP_LOGW(TAG, "RESTARTING"); + esp_restart(); +} +menuItem_t item_reset = { + item_reset_action, // function action + NULL, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 0, // valueMax + 0, // valueIncrement + "RESET defaults ", // title + " reset nvs ", // line1 (above value) + " and restart ", // line2 <= showing "default = %d" + "reset all stored", // line4 * (below value) + " parameters ", // line5 * + "", // line6 + "=>long to cancel", // line7 +}; + + +//############################### +//##### select statusScreen ##### +//############################### +void item_statusScreen_action(display_task_parameters_t *objects, SSD1306_t *display, int value) +{ + switch (value) + { + case 1: + default: + display_selectStatusPage(STATUS_SCREEN_OVERVIEW); + break; + case 2: + display_selectStatusPage(STATUS_SCREEN_SPEED); + break; + case 3: + display_selectStatusPage(STATUS_SCREEN_JOYSTICK); + break; + case 4: + display_selectStatusPage(STATUS_SCREEN_MOTORS); + break; + } +} +int item_statusScreen_value(display_task_parameters_t *objects) +{ + return 1; // initial value shown / changed from +} +menuItem_t item_statusScreen = { + item_statusScreen_action, // function action + item_statusScreen_value, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + 1, // valueMin + 4, // valueMax + 1, // valueIncrement + "Status Screen ", // title + " Select ", // line1 (above value) + " Status Screen ", // line2 (above value) + "1: Overview", // line4 * (below value) + "2: Speeds", // line5 * + "3: Joystick", // line6 + "4: Motors", // line7 +}; + +//##################### +//###### example ###### +//##################### +void item_example_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + return; +} +int item_example_value(display_task_parameters_t * objects){ + return 53; //initial value shown / changed from +} +int item_example_valueDefault(display_task_parameters_t * objects){ + return 931; // optionally shown in line 2 as "default = %d" +} +menuItem_t item_example = { + item_example_action, // function action + item_example_value, // function get initial value or NULL(show in line 2) + NULL, // function get default value or NULL(dont set value, show msg) + -255, // valueMin + 255, // valueMax + 2, // valueIncrement + "example-item-max", // title + "line 1 - above ", // line1 (above value) + "line 2 - above ", // line2 (above value) + "line 4 - below ", // line4 * (below value) + "line 5 - below ", // line5 * + "line 6 - below ", // line6 + "line 7 - last ", // line7 +}; + +menuItem_t item_last = { + item_example_action, // function action + item_example_value, // function get initial value or NULL(show in line 2) + item_example_valueDefault, // function get default value or NULL(dont set value, show msg) + -500, // valueMin + 4500, // valueMax + 50, // valueIncrement + "set large number", // title + "line 1 - above ", // line1 (above value) + "line 2 - above ", // line2 (above value) + "", // line4 * (below value) + "", // line5 * + "line 6 - below ", // line6 + "line 7 - last ", // line7 +}; + + +//#################################################### +//### store all configured menu items in one array ### +//#################################################### +const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_brakeDecel, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last}; +const int itemCount = 12; + + + + +//-------------------------- +//------ showItemList ------ +//-------------------------- +//function that renders main menu to display (one update) +//list of all menu items with currently selected highlighted +#define SELECTED_ITEM_LINE 4 +#define FIRST_ITEM_LINE 1 +#define LAST_ITEM_LINE 7 +void showItemList(SSD1306_t *display, int selectedItem) +{ + //-- show title line -- + displayTextLine(display, 0, false, true, " --- menu --- "); //inverted + + //-- show item list -- + for (int line = FIRST_ITEM_LINE; line <= LAST_ITEM_LINE; line++) + { // loop through all lines + int printItemIndex = selectedItem - SELECTED_ITEM_LINE + line; + // TODO: when reaching end of items, instead of showing "empty" change position of selected item to still use the entire screen + if (printItemIndex < 0 || printItemIndex >= itemCount) // out of range of available items + { + // no item in this line + displayTextLineCentered(display, line, false, false, "---"); + } + else + { + if (printItemIndex == selectedItem) + { + // selected item -> add '> ' and print inverted + displayTextLine(display, line, false, true, "> %-14s", menuItems[printItemIndex].title); // inverted + } + else + { + // not the selected item -> print normal + displayTextLine(display, line, false, false, "%-16s", menuItems[printItemIndex].title); + } + } + // logging + ESP_LOGD(TAG, "showItemList: loop - line=%d, item=%d, (selected=%d '%s')", line, printItemIndex, selectedItem, menuItems[selectedItem].title); + } +} + + + +//---------------------------------- +//--- getNextSelectableModeIndex --- +//---------------------------------- +// local function that returns index of the next (or previous) selectable control-mode index +// used for mode select menu. offset defines the step size (e.g. get 3rd next menu index) +int getNextSelectableModeIndex(int modeIndex, bool reverseDirection = false, uint8_t offset = 1) +{ + // those modes are selectable via mode-select menu - NOTE: Add other new modes here + static const controlMode_t selectableModes[] = {controlMode_t::IDLE, + controlMode_t::JOYSTICK, + controlMode_t::MASSAGE, + controlMode_t::HTTP, + controlMode_t::ADJUST_CHAIR, + controlMode_t::MENU_SETTINGS}; + static const int selectableModesCount = sizeof(selectableModes) / sizeof(controlMode_t); + + // when step size is greater than 1 define new modeIndex by recursively calling the function first + if (offset > 1){ + modeIndex = getNextSelectableModeIndex(modeIndex, reverseDirection, offset - 1); + } + + // search next mode that is present in selectableModes + bool rotatedAlready = false; + while (1) + { + // try next/previous item + if (reverseDirection) + modeIndex--; + else + modeIndex++; + + // go back to start/end if last/first possible mode reached + if ((!reverseDirection && modeIndex >= controlModeMaxCount) || (reverseDirection && modeIndex < 0)) + { + // prevent deadlock when no match was found for some reason + if (rotatedAlready) + { + ESP_LOGE(TAG, "search for selectable mode failed - no matching mode found"); + return 0; + } + // go to start/end + if (reverseDirection) + modeIndex = controlModeMaxCount - 1; + else + modeIndex = 0; + rotatedAlready = true; + } + // check if current mode index is present in allowed / selectable modes + for (int j = 0; j < selectableModesCount; j++) + { + if (modeIndex == (int)selectableModes[j]) + // index matches one in the selectable modes -> success + return modeIndex; + } + ESP_LOGV(TAG, "mode index %d is no selectable mode -> trying next", modeIndex); + } +} + + + +//-------------------------- +//------ showModeList ------ +//-------------------------- +//function that renders mode-select menu (one update) +void showModeList(SSD1306_t *display, int selectedMode) +{ + // TODO add blinking of a line to indicate selecting + + // line 1 " - select mode -" + // line 2 " 2nd prev mode " + // line 3 " prev mode " + // line 4 "SEL MODE LARGE 1/3" + // line 5 "SEL MODE LARGE 2/3" + // line 6 "SEL MODE LARGE 4/3" + // line 7 " next mode " + // line 8 " 2nd next mode " + + // print title (0) + displayTextLine(display, 0, false, true, "- select mode -"); // inverted + // print 2nd mode before (1) + displayTextLineCentered(display, 1, false, false, "%s", controlModeToStr(getNextSelectableModeIndex(selectedMode, true, 2))); + // print mode before (2) + displayTextLineCentered(display, 2, false, false, "%s", controlModeToStr(getNextSelectableModeIndex(selectedMode, true))); + // print selected mode large (3-5) + displayTextLineCentered(display, 3, true, false, "%s", controlModeToStr(selectedMode)); + // print mode after (6) + displayTextLineCentered(display, 6, false, false, "%s", controlModeToStr(getNextSelectableModeIndex(selectedMode))); + // print mode after (7) + displayTextLineCentered(display, 7, false, false, "%s", controlModeToStr(getNextSelectableModeIndex(selectedMode, false, 2))); + // print message (6) + //displayTextLineCentered(display, 7, false, true, "click to confirm"); +} + + + +//----------------------------- +//--- showValueSelectStatic --- +//----------------------------- +// function that renders lines that do not update of value-select screen to display (initial update) +// shows configured text of currently selected item +void showValueSelectStatic(display_task_parameters_t * objects, SSD1306_t *display, int selectedItem) +{ + //-- show title line -- + displayTextLine(display, 0, false, true, " -- set value -- "); // inverted + + //-- show text above value -- + displayTextLine(display, 1, false, false, "%-16s", menuItems[selectedItem].line1); + + //-- show line 2 or default value --- + if (menuItems[selectedItem].defaultValue != NULL){ + displayTextLineCentered(display, 2, false, false, "default = %d", menuItems[selectedItem].defaultValue(objects)); + } + else + { + // displayTextLine(display, 2, false, false, "previous=%d", menuItems[selectedItem].currentValue(objects)); // <= show previous value + displayTextLine(display, 2, false, false, "%-16s", menuItems[selectedItem].line2); + } + + //-- show value and other configured lines -- + // print value large, if two description lines are empty + if (strlen(menuItems[selectedItem].line4) == 0 && strlen(menuItems[selectedItem].line5) == 0) + { + // print less lines: line5 and line6 only (due to large value) + //displayTextLineCentered(display, 3, true, false, "%d", value); //large centered (value shown in separate function) + displayTextLine(display, 6, false, false, "%-16s", menuItems[selectedItem].line6); + displayTextLine(display, 7, false, false, "%-16s", menuItems[selectedItem].line7); + } + else + { + //displayTextLineCentered(display, 3, false, false, "%d", value); //centered (value shown in separate function) + // print description lines 4 to 7 + displayTextLine(display, 4, false, false, "%-16s", menuItems[selectedItem].line4); + displayTextLine(display, 5, false, false, "%-16s", menuItems[selectedItem].line5); + displayTextLine(display, 6, false, false, "%-16s", menuItems[selectedItem].line6); + displayTextLine(display, 7, false, false, "%-16s", menuItems[selectedItem].line7); + } + + //-- show info msg instead of value -- + //when pointer to default value func not defined (set value not used, action only) + if (menuItems[selectedItem].currentValue == NULL) + { + //show static text + displayTextLineCentered(display, 3, false, true, "%s", "click to confirm"); + } + // otherwise value gets updated in next iteration of menu-handle function +} + + +//----------------------------- +//----- updateValueSelect ----- +//----------------------------- +// update line with currently set value only (increses performance significantly) +void updateValueSelect(SSD1306_t *display, int selectedItem) +{ + // print value large, if 2 description lines are empty + if (strlen(menuItems[selectedItem].line4) == 0 && strlen(menuItems[selectedItem].line5) == 0) + { + // print large and centered value in line 3-5 + displayTextLineCentered(display, 3, true, false, "%d", value); // large centered + } + else + { + //print value centered in line 3 + displayTextLineCentered(display, 3, false, false, "%d", value); // centered + } +} + + + +//=========================== +//=== handleMenu_settings === +//=========================== +//controls menu with encoder input and displays the text on oled display +//function is repeatedly called by display task when in menu state +#define QUEUE_TIMEOUT 3000 //timeout no encoder event - to not block the display loop and actually handle menu-timeout +#define MENU_TIMEOUT 60000 //inactivity timeout (switch to IDLE mode) note: should be smaller than IDLE timeout in control task +void handleMenu_settings(display_task_parameters_t * objects, SSD1306_t *display) +{ + static uint32_t lastActivity = 0; + static int selectedItem = 0; + rotary_encoder_event_t event; // store event data + + //--- handle different menu states --- + switch (menuState) + { + //------------------------- + //---- State MAIN MENU_SETTINGS ---- + //------------------------- + case MAIN_MENU: + // update display + showItemList(display, selectedItem); // shows list of items with currently selected one on display + // wait for encoder event + if (xQueueReceive(objects->encoderQueue, &event, QUEUE_TIMEOUT / portTICK_PERIOD_MS)) + { + // reset menu- and control-timeout on any encoder event + lastActivity = esp_log_timestamp(); + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + case RE_ET_CHANGED: + //--- scroll in list --- + if (event.diff < 0) + { + if (selectedItem != itemCount - 1) + { + objects->buzzer->beep(1, 20, 0); + selectedItem++; + ESP_LOGD(TAG, "showing next item: %d '%s'", selectedItem, menuItems[selectedItem].title); + } + //note: display will update at start of next run + } + else + { + if (selectedItem != 0) + { + objects->buzzer->beep(1, 20, 0); + selectedItem--; + ESP_LOGD(TAG, "showing previous item: %d '%s'", selectedItem, menuItems[selectedItem].title); + } + //note: display will update at start of next run + } + break; + + case RE_ET_BTN_CLICKED: + //--- switch to edit value page --- + objects->buzzer->beep(1, 50, 10); + ESP_LOGI(TAG, "Button pressed - switching to state SET_VALUE"); + // change state (menu to set value) + menuState = SET_VALUE; + // clear display + ssd1306_clear_screen(display, false); + //update static content of set-value screen once at change only + showValueSelectStatic(objects, display, selectedItem); + // get currently configured value, when value-select feature is actually used in this item + if (menuItems[selectedItem].currentValue != NULL) + value = menuItems[selectedItem].currentValue(objects); + else + value = 0; + break; + + case RE_ET_BTN_LONG_PRESSED: + //--- exit menu mode --- + // change to previous mode (e.g. JOYSTICK) + objects->buzzer->beep(12, 15, 8); + objects->control->toggleMode(controlMode_t::MENU_SETTINGS); //currently already in MENU_SETTINGS -> changes to previous mode + ssd1306_clear_screen(display, false); + break; + + case RE_ET_BTN_RELEASED: + case RE_ET_BTN_PRESSED: + break; + } + } + break; + + //------------------------- + //---- State SET VALUE ---- + //------------------------- + case SET_VALUE: + // update currently selected value + // note: static lines are updated at mode change + if (menuItems[selectedItem].currentValue != NULL) // dont update when set-value not used for this item + updateValueSelect(display, selectedItem); + + // wait for encoder event + if (xQueueReceive(objects->encoderQueue, &event, QUEUE_TIMEOUT / portTICK_PERIOD_MS)) + { + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + case RE_ET_CHANGED: + //-- change value -- + // no need to increment value when item configured to not show value + if (menuItems[selectedItem].currentValue != NULL) + { + objects->buzzer->beep(1, 25, 10); + // increment value + if (event.diff < 0) + value += menuItems[selectedItem].valueIncrement; + else + value -= menuItems[selectedItem].valueIncrement; + // limit to min/max range + if (value > menuItems[selectedItem].valueMax) + value = menuItems[selectedItem].valueMax; + if (value < menuItems[selectedItem].valueMin) + value = menuItems[selectedItem].valueMin; + } + break; + case RE_ET_BTN_CLICKED: + //-- apply value -- + ESP_LOGI(TAG, "Button pressed - running action function with value=%d for item '%s'", value, menuItems[selectedItem].title); + objects->buzzer->beep(2, 50, 50); + menuItems[selectedItem].action(objects, display, value); + menuState = MAIN_MENU; + break; + case RE_ET_BTN_LONG_PRESSED: + //-- exit value select to main menu -- + objects->buzzer->beep(2, 100, 50); + ssd1306_clear_screen(display, false); + menuState = MAIN_MENU; + break; + case RE_ET_BTN_PRESSED: + case RE_ET_BTN_RELEASED: + break; + } + // reset menu- and control-timeout on any encoder event + lastActivity = esp_log_timestamp(); + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + } + break; + } + + + //-------------------- + //--- menu timeout --- + //-------------------- + //close menu and switch to IDLE mode when no encoder event occured within MENU_TIMEOUT + if (esp_log_timestamp() - lastActivity > MENU_TIMEOUT) + { + ESP_LOGW(TAG, "TIMEOUT - no activity for more than %ds -> closing menu, switching to IDLE", MENU_TIMEOUT/1000); + // reset menu + selectedItem = 0; + menuState = MAIN_MENU; + ssd1306_clear_screen(display, false); + // change control mode + objects->control->changeMode(controlMode_t::IDLE); + return; + } +} + + + +//============================= +//=== handleMenu_modeSelect === +//============================= +//controls menu for selecting the control mode with encoder input and displays the text on oled display +//function is repeatedly called by display task when in menu state +#define MENU_MODE_SEL_TIMEOUT 10000 // inactivity timeout (switch to IDLE mode) note: should be smaller than IDLE timeout in control task +void handleMenu_modeSelect(display_task_parameters_t *objects, SSD1306_t *display) +{ + static uint32_t lastActivity = 0; + static bool firstRun = true; // track if last mode was already obtained when menu got opened + static int selectedMode = (int)controlMode_t::IDLE; + rotary_encoder_event_t event; // store encoder event data + + // get current mode when run for first time since last select + if (firstRun) + { + firstRun = false; + ssd1306_clear_screen(display, false); // clear screen initially (no artefacts of previous content) + selectedMode = (int)objects->control->getPreviousMode(); // store previous mode (since current mode is MENU) + ESP_LOGI(TAG, "started mode-select menu, previous active is %s", controlModeStr[(int)selectedMode]); + } + + // renders list of modes with currently selected one on display + showModeList(display, selectedMode); + // wait for encoder event + if (xQueueReceive(objects->encoderQueue, &event, QUEUE_TIMEOUT / portTICK_PERIOD_MS)) + { + // reset menu- and control-timeout on any encoder event + lastActivity = esp_log_timestamp(); + objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout + switch (event.type) + { + case RE_ET_CHANGED: + //--- scroll in list --- + if (event.diff < 0) + { + selectedMode = getNextSelectableModeIndex(selectedMode); + objects->buzzer->beep(1, 20, 0); + ESP_LOGD(TAG, "showing next item: %d '%s'", selectedMode, controlModeToStr(selectedMode)); + } + // note: display will update at start of next run + else + { + selectedMode = getNextSelectableModeIndex(selectedMode, true); + objects->buzzer->beep(1, 20, 0); + ESP_LOGD(TAG, "showing previous item: %d '%s'", selectedMode, controlModeToStr(selectedMode)); + // note: display will update at start of next run + } + break; + + case RE_ET_BTN_CLICKED: + //--- confirm mode and exit --- + objects->buzzer->beep(1, 50, 10); + ESP_LOGI(TAG, "Button pressed - confirming selected mode '%s'", controlModeToStr(selectedMode)); + objects->control->changeMode((controlMode_t)selectedMode); //note: changeMode may take some time since it waits for control-handle loop iteration to finish which has quite large delay in menu state + // clear display + ssd1306_clear_screen(display, false); + // reset first run + firstRun = true; + return; // function wont be called again due to mode change + + case RE_ET_BTN_LONG_PRESSED: + //--- exit to previous mode --- + // change to previous mode (e.g. JOYSTICK) + objects->buzzer->beep(12, 15, 8); + objects->control->changeMode(objects->control->getPreviousMode()); + // clear display + ssd1306_clear_screen(display, false); + // reset first run + firstRun = true; + return; // function wont be called again due to mode change + break; + + case RE_ET_BTN_RELEASED: + case RE_ET_BTN_PRESSED: + break; + } + } + + //--- menu timeout --- + // close menu and switch to IDLE mode when no encoder event occured within MENU_TIMEOUT + if (esp_log_timestamp() - lastActivity > MENU_MODE_SEL_TIMEOUT) + { + ESP_LOGW(TAG, "TIMEOUT - no activity for more than %ds -> closing menu, switching to IDLE", MENU_TIMEOUT / 1000); + // clear display + ssd1306_clear_screen(display, false); + // change control mode + objects->control->changeMode(controlMode_t::IDLE); + // reset first run + firstRun = true; + return; // function wont be called again due to mode change + } +} \ No newline at end of file diff --git a/board_single/main/menu.hpp b/board_single/main/menu.hpp new file mode 100644 index 0000000..6def104 --- /dev/null +++ b/board_single/main/menu.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "display.hpp" + + +//--- menuState_t --- +// modes the menu can be in +typedef enum { + MAIN_MENU = 0, + SET_VALUE +} menuState_t; + + +//--- menuItem_t --- +// struct describes one menu element (all defined in menu.cpp) +typedef struct +{ + void (*action)(display_task_parameters_t * objects, SSD1306_t * display, int value); // pointer to function run when confirmed + int (*currentValue)(display_task_parameters_t * objects); // pointer to function to get currently configured value + int (*defaultValue)(display_task_parameters_t * objects); // pointer to function to get currently configured value + int valueMin; // min allowed value + int valueMax; // max allowed value + int valueIncrement; // amount changed at one encoder tick (+/-) + const char title[17]; // shown in list + const char line1[17]; // above value + const char line2[17]; // above value + const char line4[17]; // below value * + const char line5[17]; // below value * + const char line6[17]; // below value + const char line7[17]; // below value +} menuItem_t; + +//controls menu for changing settings with encoder input and displays the text on oled display (has to be repeatedly called by display task) +void handleMenu_settings(display_task_parameters_t * objects, SSD1306_t *display); + +//controls menu for selecting the control mode with encoder input and displays the text on oled display (has to be repeatedly called by display task) +void handleMenu_modeSelect(display_task_parameters_t * objects, SSD1306_t *display); \ No newline at end of file diff --git a/board_single/sdkconfig b/board_single/sdkconfig index 6e3dc02..1fd5ec0 100644 --- a/board_single/sdkconfig +++ b/board_single/sdkconfig @@ -140,10 +140,10 @@ CONFIG_I2C_INTERFACE=y # CONFIG_SPI_INTERFACE is not set # CONFIG_SSD1306_128x32 is not set CONFIG_SSD1306_128x64=y -CONFIG_OFFSETX=0 +CONFIG_OFFSETX=2 # CONFIG_FLIP is not set CONFIG_SCL_GPIO=22 -CONFIG_SDA_GPIO=21 +CONFIG_SDA_GPIO=23 CONFIG_RESET_GPIO=15 CONFIG_I2C_PORT_0=y # CONFIG_I2C_PORT_1 is not set @@ -1246,6 +1246,17 @@ CONFIG_WPA_MBEDTLS_CRYPTO=y # CONFIG_WPA_MBO_SUPPORT is not set # CONFIG_WPA_DPP_SUPPORT is not set # end of Supplicant + +# +# Rotary encoders +# +CONFIG_RE_MAX=1 +CONFIG_RE_INTERVAL_US=1000 +CONFIG_RE_BTN_DEAD_TIME_US=40000 +CONFIG_RE_BTN_PRESSED_LEVEL_0=y +# CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set +CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000 +# end of Rotary encoders # end of Component config # diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 00a73f9..bdbe43d 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -10,6 +10,7 @@ idf_component_register( "joystick.cpp" "http.cpp" "speedsensor.cpp" + "chairAdjust.cpp" INCLUDE_DIRS "." PRIV_REQUIRES nvs_flash mdns json spiffs esp_http_server diff --git a/common/buzzer.cpp b/common/buzzer.cpp index 0850116..f698ce1 100644 --- a/common/buzzer.cpp +++ b/common/buzzer.cpp @@ -2,6 +2,19 @@ static const char *TAG_BUZZER = "buzzer"; +//====================================== +//============ buzzer task ============= +//====================================== +// Task that repeatedly handles the buzzer object (process Queued beeps) +void task_buzzer(void * param_buzzerObject){ + ESP_LOGI("task_buzzer", "Start of buzzer task..."); + buzzer_t * buzzer = (buzzer_t *)param_buzzerObject; + //run function that waits for a beep events to arrive in the queue + //and processes them + buzzer->processQueue(); +} + + //============================ //========== init ============ //============================ @@ -33,12 +46,18 @@ buzzer_t::buzzer_t(gpio_num_t gpio_pin_f, uint16_t msGap_f){ //=========== beep =========== //============================ //function to add a beep command to the queue +//use default/configured gap when no custom pause duration is given: void buzzer_t::beep(uint8_t count, uint16_t msOn, uint16_t msOff){ + beep(count, msOn, msOff, msGap); +} + +void buzzer_t::beep(uint8_t count, uint16_t msOn, uint16_t msOff, uint16_t msDelayFinished){ //create entry struct with provided data struct beepEntry entryInsert = { - count = count, - msOn = msOn, - msOff = msOff + count, + msOn, + msOff, + msDelayFinished }; // Send a pointer to a struct AMessage object. Don't block if the @@ -69,7 +88,7 @@ void buzzer_t::processQueue(){ // otherwise waits for at least 7 weeks if( xQueueReceive( beepQueue, &entryRead, portMAX_DELAY ) ) { - ESP_LOGW(TAG_BUZZER, "Read entry from queue: count=%d, msOn=%d, msOff=%d", entryRead.count, entryRead.msOn, entryRead.msOff); + ESP_LOGI(TAG_BUZZER, "Read entry from queue: count=%d, msOn=%d, msOff=%d", entryRead.count, entryRead.msOn, entryRead.msOff); //beep requested count with requested delays for (int i = entryRead.count; i--;){ @@ -83,7 +102,7 @@ void buzzer_t::processQueue(){ vTaskDelay(entryRead.msOff / portTICK_PERIOD_MS); } //wait for minimum gap between beep events - vTaskDelay(msGap / portTICK_PERIOD_MS); + vTaskDelay(entryRead.msDelay / portTICK_PERIOD_MS); } }else{ //wait for queue to become available vTaskDelay(50 / portTICK_PERIOD_MS); diff --git a/common/buzzer.hpp b/common/buzzer.hpp index ce6c2c6..7f9476e 100644 --- a/common/buzzer.hpp +++ b/common/buzzer.hpp @@ -27,24 +27,27 @@ class buzzer_t { //--- functions --- void processQueue(); //has to be run once in a separate task, waits for and processes queued events + //add entry to queue processing beeps, last parameter is optional to delay the next entry + void beep(uint8_t count, uint16_t msOn, uint16_t msOff, uint16_t msDelayFinished); void beep(uint8_t count, uint16_t msOn, uint16_t msOff); //void clear(); (TODO - not implemented yet) //void createTask(); (TODO - not implemented yet) //--- variables --- - uint16_t msGap; //gap between beep entries (when multiple queued) private: //--- functions --- void init(); //--- variables --- + uint16_t msGap; //gap between beep entries (when multiple queued) gpio_num_t gpio_pin; struct beepEntry { uint8_t count; uint16_t msOn; uint16_t msOff; + uint16_t msDelay; }; //queue for queueing up multiple events while one is still processing @@ -53,4 +56,9 @@ class buzzer_t { }; - +//====================================== +//============ buzzer task ============= +//====================================== +// Task that repeatedly handles the buzzer object (process Queued beeps) +// Note: pointer to globally initialized buzzer object has to be passed as task-parameter +void task_buzzer(void * param_buzzerObject); \ No newline at end of file diff --git a/common/chairAdjust.cpp b/common/chairAdjust.cpp new file mode 100644 index 0000000..06b2b0d --- /dev/null +++ b/common/chairAdjust.cpp @@ -0,0 +1,115 @@ +extern "C" +{ +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "esp_log.h" +#include +} + +#include "chairAdjust.hpp" + + + +//--- gloabl variables --- +// strings for logging the rest state +const char* restStateStr[] = {"REST_OFF", "REST_DOWN", "REST_UP"}; + +//--- local variables --- +//tag for logging +static const char * TAG = "chair-adjustment"; + + + +//============================= +//======== constructor ======== +//============================= +cControlledRest::cControlledRest(gpio_num_t gpio_up_f, gpio_num_t gpio_down_f, const char * name_f){ + strcpy(name, name_f); + gpio_up = gpio_up_f; + gpio_down = gpio_down_f; + init(); +} + + + +//==================== +//======= init ======= +//==================== +// init gpio pins for relays +void cControlledRest::init() +{ + ESP_LOGW(TAG, "[%s] initializing gpio pins %d, %d for relays...", name, gpio_up, gpio_down); + // configure 2 gpio pins + gpio_pad_select_gpio(gpio_up); + gpio_set_direction(gpio_up, GPIO_MODE_OUTPUT); + gpio_pad_select_gpio(gpio_down); + gpio_set_direction(gpio_down, GPIO_MODE_OUTPUT); + // both relays off initially + gpio_set_level(gpio_down, 0); + gpio_set_level(gpio_up, 0); + state = REST_OFF; +} + + + +//============================ +//========= setState ========= +//============================ +void cControlledRest::setState(restState_t targetState) +{ + //check if actually changed + if (targetState == state){ + ESP_LOGD(TAG, "[%s] state already at '%s', nothing to do", name, restStateStr[state]); + return; + } + + //apply new state + ESP_LOGI(TAG, "[%s] switching from state '%s' to '%s'", name, restStateStr[state], restStateStr[targetState]); + state = targetState; + timestamp_lastChange = esp_log_timestamp(); //TODO use this to estimate position + switch (state) + { + case REST_UP: + gpio_set_level(gpio_down, 0); + gpio_set_level(gpio_up, 1); + break; + case REST_DOWN: + gpio_set_level(gpio_down, 1); + gpio_set_level(gpio_up, 0); + break; + case REST_OFF: + gpio_set_level(gpio_down, 0); + gpio_set_level(gpio_up, 0); + break; + } +} + + + +//==================================== +//====== controlChairAdjustment ====== +//==================================== +//function that controls the two rests according to joystick data (applies threshold, defines direction) +//TODO: +// - add separate task that controls chair adjustment +// - timeout +// - track position +// - auto-adjust: move to position while driving +// - control via app +// - add delay betweem direction change +void controlChairAdjustment(joystickData_t data, cControlledRest * legRest, cControlledRest * backRest){ + //--- variables --- + float stickThreshold = 0.3; //min coordinate for motor to start + + //--- control rest motors --- + //leg rest (x-axis) + if (data.x > stickThreshold) legRest->setState(REST_UP); + else if (data.x < -stickThreshold) legRest->setState(REST_DOWN); + else legRest->setState(REST_OFF); + + //back rest (y-axis) + if (data.y > stickThreshold) backRest->setState(REST_UP); + else if (data.y < -stickThreshold) backRest->setState(REST_DOWN); + else backRest->setState(REST_OFF); +} diff --git a/common/chairAdjust.hpp b/common/chairAdjust.hpp new file mode 100644 index 0000000..54f9abe --- /dev/null +++ b/common/chairAdjust.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "joystick.hpp" + +typedef enum { + REST_OFF = 0, + REST_DOWN, + REST_UP +} restState_t; + +extern const char* restStateStr[]; + + +//===================================== +//======= cControlledRest class ======= +//===================================== +//class that controls 2 relays powering a motor that moves a rest of the armchair up or down +//2 instances will be created one for back and one for leg rest +class cControlledRest { +public: + cControlledRest(gpio_num_t gpio_up, gpio_num_t gpio_down, const char *name); + void setState(restState_t targetState); + void stop(); + +private: + void init(); + + char name[32]; + gpio_num_t gpio_up; + gpio_num_t gpio_down; + restState_t state; + const uint32_t travelDuration = 5000; + uint32_t timestamp_lastChange; + float currentPosition = 0; +}; + +//==================================== +//====== controlChairAdjustment ====== +//==================================== +//function that controls the two rests according to joystick data (applies threshold, defines direction) +void controlChairAdjustment(joystickData_t data, cControlledRest * legRest, cControlledRest * backRest); \ No newline at end of file diff --git a/common/currentsensor.cpp b/common/currentsensor.cpp index 2569c6d..359a73b 100644 --- a/common/currentsensor.cpp +++ b/common/currentsensor.cpp @@ -3,6 +3,7 @@ extern "C" { #include "esp_log.h" } +#include #include "currentsensor.hpp" //tag for logging @@ -29,10 +30,12 @@ float getVoltage(adc1_channel_t adc, uint32_t samples){ //============================= //======== constructor ======== //============================= -currentSensor::currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent_f){ +currentSensor::currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent_f, float snapToZeroThreshold_f, bool isInverted_f){ //copy config adcChannel = adcChannel_f; ratedCurrent = ratedCurrent_f; + isInverted = isInverted_f; + snapToZeroThreshold = snapToZeroThreshold_f; //init adc adc1_config_width(ADC_WIDTH_BIT_12); //max resolution 4096 adc1_config_channel_atten(adcChannel, ADC_ATTEN_DB_11); //max voltage @@ -58,6 +61,15 @@ float currentSensor::read(void){ current = 0; } + if (fabs(current) < snapToZeroThreshold) + { + ESP_LOGD(TAG, "current=%.3f < threshold=%.3f -> snap to 0", current, snapToZeroThreshold); + current = 0; + } + // invert calculated current if necessary + else if (isInverted) + current = -current; + ESP_LOGI(TAG, "read sensor adc=%d: voltage=%.3fV, centerVoltage=%.3fV => current=%.3fA", (int)adcChannel, voltage, centerVoltage, current); return current; } diff --git a/common/currentsensor.hpp b/common/currentsensor.hpp index f62fa34..d24eb3d 100644 --- a/common/currentsensor.hpp +++ b/common/currentsensor.hpp @@ -7,12 +7,14 @@ class currentSensor{ public: - currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent); + currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent, float snapToZeroThreshold, bool inverted = false); void calibrateZeroAmpere(void); //set current voltage to voltage representing 0A float read(void); //get current ampere private: adc1_channel_t adcChannel; float ratedCurrent; + bool isInverted; + float snapToZeroThreshold; uint32_t measure; float voltage; float current; diff --git a/common/http.cpp b/common/http.cpp index 8b784a4..34b0e33 100644 --- a/common/http.cpp +++ b/common/http.cpp @@ -178,50 +178,39 @@ esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){ //------------------- //----- getData ----- //------------------- -//wait for and return joystick data from queue, if timeout return NULL +//wait for and return joystick data from queue, return last data if nothing received within 500ms, return center data when timeout exceeded joystickData_t httpJoystick::getData(){ //--- get joystick data from queue --- - if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(config.timeoutMs) ) ) { - + if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(500) ) ) { //dont wait longer than 500ms to not block the control loop for too long ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f", dataRead.x, dataRead.y, dataRead.radius, dataRead.angle); + timeLastData = esp_log_timestamp(); } //--- timeout --- - //no new data received within configured timeout + // send error message when last received data did NOT result in CENTER position and timeout exceeded else { - //send error message when last received data did NOT result in CENTER position - if (dataRead.position != joystickPos_t::CENTER) { + if (dataRead.position != joystickPos_t::CENTER && (esp_log_timestamp() - timeLastData) > config.timeoutMs) { //change data to "joystick center" data to stop the motors dataRead = dataCenter; - ESP_LOGE(TAG, "TIMEOUT - no data received for 3s -> set to center"); + ESP_LOGE(TAG, "TIMEOUT - no data received for %dms -> set to center", config.timeoutMs); } } return dataRead; } -//-------------------------------------------- -//--- receiveHttpData for httpJoystickMain --- -//-------------------------------------------- -//function that wraps pointer to member function of httpJoystickMain instance in a "normal" function which the webserver can run on joystick URL - -//declare pointer to receiveHttpData method of httpJoystick class -esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData; - -esp_err_t on_joystick_url(httpd_req_t *req){ - //run pointer to receiveHttpData function of httpJoystickMain instance - return (httpJoystickMain.*pointerToReceiveFunc)(req); -} - - //============================ //===== init http server ===== //============================ -//function that initializes http server and configures available urls -void http_init_server() +//function that initializes http server and configures available url's + +//parameter: provide pointer to function that handle incomming joystick data (for configuring the url) +//TODO add handle functions to future additional endpoints/urls here too +void http_init_server(http_handler_t onJoystickUrl) { + ESP_LOGI(TAG, "initializing HTTP-Server..."); //---- configure webserver ---- httpd_config_t config = HTTPD_DEFAULT_CONFIG(); @@ -236,7 +225,7 @@ void http_init_server() httpd_uri_t joystick_url = { .uri = "/api/joystick", .method = HTTP_POST, - .handler = on_joystick_url, + .handler = onJoystickUrl, }; httpd_register_uri_handler(server, &joystick_url); @@ -265,8 +254,8 @@ void http_init_server() //function that destroys the http server void http_stop_server() { - printf("stopping http\n"); - httpd_stop(server); + ESP_LOGW(TAG, "stopping HTTP-Server"); + httpd_stop(server); } diff --git a/common/http.hpp b/common/http.hpp index 0005c84..7504d51 100644 --- a/common/http.hpp +++ b/common/http.hpp @@ -13,7 +13,18 @@ extern "C" //===== init http server ===== //============================ //function that initializes http server and configures available urls -void http_init_server(); +//parameter: provide pointer to function that handles incomming joystick data (for configuring the url) +//TODO add handle functions to future additional endpoints/urls here too +typedef esp_err_t (*http_handler_t)(httpd_req_t *req); +void http_init_server(http_handler_t onJoystickUrl); + +//example with lambda function to pass method of a class instance: +//esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData; +//esp_err_t on_joystick_url(httpd_req_t *req){ +// //run pointer to receiveHttpData function of httpJoystickMain instance +// return (httpJoystickMain->*pointerToReceiveFunc)(req); +//} +//http_init_server(on_joystick_url); //============================== @@ -27,7 +38,7 @@ void start_mdns_service(); //===== stop http server ===== //============================ //function that destroys the http server -void http_stop_server(); +void http_stop_server(httpd_handle_t * httpServer); //============================== @@ -47,7 +58,7 @@ typedef struct httpJoystick_config_t { class httpJoystick{ public: //--- constructor --- - httpJoystick( httpJoystick_config_t config_f ); + httpJoystick(httpJoystick_config_t config_f); //--- functions --- joystickData_t getData(); //wait for and return joystick data from queue, if timeout return CENTER @@ -59,7 +70,7 @@ class httpJoystick{ httpJoystick_config_t config; QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) ); //struct for receiving data from http function, and storing data of last update - joystickData_t dataRead; + uint32_t timeLastData = 0; const joystickData_t dataCenter = { .position = joystickPos_t::CENTER, .x = 0, @@ -67,11 +78,5 @@ class httpJoystick{ .radius = 0, .angle = 0 }; -}; - - - -//===== global object ===== -//create global instance of httpJoystick -//note: is constructed/configured in config.cpp -extern httpJoystick httpJoystickMain; + joystickData_t dataRead = dataCenter; +}; \ No newline at end of file diff --git a/common/joystick.cpp b/common/joystick.cpp index 93d2097..02a6384 100644 --- a/common/joystick.cpp +++ b/common/joystick.cpp @@ -19,8 +19,9 @@ static const char * TAG_CMD = "joystickCommands"; //-------- constructor -------- //----------------------------- //copy provided struct with all configuration and run init function -evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){ +evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f, nvs_handle_t * nvsHandle_f){ config = config_f; + nvsHandle = nvsHandle_f; init(); } @@ -30,7 +31,7 @@ evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){ //---------- init ------------ //---------------------------- void evaluatedJoystick::init(){ - ESP_LOGI(TAG, "initializing joystick"); + ESP_LOGW(TAG, "initializing ADC's and loading calibration..."); //initialize adc adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096 @@ -41,6 +42,12 @@ void evaluatedJoystick::init(){ adc1_config_channel_atten(config.adc_x, ADC_ATTEN_DB_11); //max voltage adc1_config_channel_atten(config.adc_y, ADC_ATTEN_DB_11); //max voltage + //load stored calibration values (if not found loads defaults from config) + loadCalibration(X_MIN); + loadCalibration(X_MAX); + loadCalibration(Y_MIN); + loadCalibration(Y_MAX); + //define joystick center from current position defineCenter(); //define joystick center from current position } @@ -81,17 +88,17 @@ joystickData_t evaluatedJoystick::getData() { ESP_LOGV(TAG, "getting X coodrdinate..."); uint32_t adcRead; adcRead = readAdc(config.adc_x, config.x_inverted); - float x = scaleCoordinate(readAdc(config.adc_x, config.x_inverted), config.x_min, config.x_max, x_center, config.tolerance_zeroX_per, config.tolerance_end_per); + float x = scaleCoordinate(readAdc(config.adc_x, config.x_inverted), x_min, x_max, x_center, config.tolerance_zeroX_per, config.tolerance_end_per); data.x = x; ESP_LOGD(TAG, "X: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => x=%.3f", - adc1_get_raw(config.adc_x), adcRead, config.x_min, config.x_max, x_center, config.x_inverted, x); + adc1_get_raw(config.adc_x), adcRead, x_min, x_max, x_center, config.x_inverted, x); ESP_LOGV(TAG, "getting Y coodrinate..."); adcRead = readAdc(config.adc_y, config.y_inverted); - float y = scaleCoordinate(adcRead, config.y_min, config.y_max, y_center, config.tolerance_zeroY_per, config.tolerance_end_per); + float y = scaleCoordinate(adcRead, y_min, y_max, y_center, config.tolerance_zeroY_per, config.tolerance_end_per); data.y = y; ESP_LOGD(TAG, "Y: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => y=%.3lf", - adc1_get_raw(config.adc_y), adcRead, config.y_min, config.y_max, y_center, config.y_inverted, y); + adc1_get_raw(config.adc_y), adcRead, y_min, y_max, y_center, config.y_inverted, y); //calculate radius data.radius = sqrt(pow(data.x,2) + pow(data.y,2)); @@ -297,37 +304,43 @@ joystickPos_t joystick_evaluatePosition(float x, float y){ //========= joystick_CommandsDriving ========= //============================================ //function that generates commands for both motors from the joystick data -motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altStickMapping){ +motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){ - //struct with current data of the joystick - //typedef struct joystickData_t { - // joystickPos_t position; - // float x; - // float y; - // float radius; - // float angle; - //} joystickData_t; - - //--- variables --- - motorCommands_t commands; - float dutyMax = 100; //TODO add this to config, make changeable during runtime - - float dutyOffset = 5; //immediately starts with this duty, TODO add this to config - float dutyRange = dutyMax - dutyOffset; - float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0 + //--- interpret config parameters --- + float dutyOffset = config->dutyOffset; // immediately starts with this duty + float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius + // calculate configured boost duty (added when turning) + float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100; + // limit to maximum possible duty + float dutyAvailable = 100 - config->maxDutyStraight; + if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable; - //--- snap ratio to max at angle threshold --- - //(-> more joystick area where inner wheel is off when turning) - /* - //FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn) - float ratioClipThreshold = 0.3; - if (ratio < ratioClipThreshold) ratio = 0; - else if (ratio > 1-ratioClipThreshold) ratio = 1; - //TODO subtract this clip threshold from available joystick range at ratio usage - */ + + //--- calculate paramaters with current data --- + motorCommands_t commands; // store new motor commands + + // -- calculate ratio -- + // get current ratio from stick angle + float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0 + ratioActual = 1 - ratioActual; // invert ratio + // scale and clip ratio according to configured tolerance + // to have some joystick area at max ratio before reaching X-Axis-full-turn-mode + float ratio = ratioActual / (config->ratioSnapToOneThreshold); //0->0 threshold->1 + // limit to 1 when above threshold (inside area max ratio) + if (ratio > 1) ratio = 1; // >threshold -> 1 + + // -- calculate outer tire boost -- + #define BOOST_RATIO_MANIPULATION_SCALE 1.05 // >1 to apply boost slightly faster, this slightly compensates that available boost is most times less than reduction of inner duty, so for small turns the total speed feels more equal + float boostAmountOuter = data.radius*dutyBoost* ratio *BOOST_RATIO_MANIPULATION_SCALE; + // limit to max amount + if (boostAmountOuter > dutyBoost) boostAmountOuter = dutyBoost; + + // -- calculate inner tire reduction -- + float reductionAmountInner = (data.radius * dutyRange + dutyOffset) * ratio; + //--- experimental alternative control mode --- - if (altStickMapping == true){ + if (config->altStickMapping == true){ //swap BOTTOM_LEFT and BOTTOM_RIGHT if (data.position == joystickPos_t::BOTTOM_LEFT){ data.position = joystickPos_t::BOTTOM_RIGHT; @@ -375,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altSt case joystickPos_t::TOP_RIGHT: commands.left.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD; - commands.left.duty = data.radius * dutyRange + dutyOffset; - commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; + commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset; + commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset; break; case joystickPos_t::TOP_LEFT: commands.left.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD; - commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; - commands.right.duty = data.radius * dutyRange + dutyOffset; + commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset; + commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset; break; case joystickPos_t::BOTTOM_LEFT: commands.left.state = motorstate_t::REV; commands.right.state = motorstate_t::REV; - commands.left.duty = data.radius * dutyRange + dutyOffset; - commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; + commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset; + commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset; break; case joystickPos_t::BOTTOM_RIGHT: commands.left.state = motorstate_t::REV; commands.right.state = motorstate_t::REV; - commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; - commands.right.duty = data.radius * dutyRange + dutyOffset; + commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset; + commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset; break; } - ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f", - joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); - ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); - ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); + // log input data + ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f", + joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y); + // log generation details + ESP_LOGI(TAG_CMD, "left=%.2f, right=%.2f -- BoostOuter=%.1f, ReductionInner=%.1f, maxDuty=%.0f, maxBoost=%.0f, dutyOffset=%.0f", + commands.left.duty, commands.right.duty, + boostAmountOuter, reductionAmountInner, + config->maxDutyStraight, dutyBoost, dutyOffset); + // log generated motor commands + ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); + ESP_LOGD(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); return commands; } @@ -418,13 +438,21 @@ uint32_t shake_timestamp_turnedOn = 0; uint32_t shake_timestamp_turnedOff = 0; bool shake_state = false; joystickPos_t lastStickPos = joystickPos_t::CENTER; -//stick position quadrant only with "X_AXIS and Y_AXIS" as hysteresis -joystickPos_t stickQuadrant = joystickPos_t::CENTER; //--- configure shake mode --- TODO: move this to config -uint32_t shake_msOffMax = 80; +uint32_t shake_msOffMax = 60; uint32_t shake_msOnMax = 120; -float dutyShake = 60; +uint32_t shake_minDelay = 20; //min time in ms motor stays on/off +float dutyShakeMax = 30; +float dutyShakeMin = 5; + +inline void invertMotorDirection(motorstate_t *state) +{ + if (*state == motorstate_t::FWD) + *state = motorstate_t::REV; + else + *state = motorstate_t::FWD; +} //function that generates commands for both motors from the joystick data motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ @@ -432,25 +460,29 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ //--- handle pulsing shake variable --- //TODO remove this, make individual per mode? //TODO only run this when not CENTER anyways? - motorCommands_t commands; + static motorCommands_t commands; float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0 + static uint32_t cycleCount = 0; //calculate on/off duration - uint32_t msOn = shake_msOnMax * data.radius; - uint32_t msOff = shake_msOffMax * data.radius; + float msOn = (shake_msOnMax - shake_minDelay) * data.radius + shake_minDelay; + float msOff = (shake_msOffMax - shake_minDelay) * data.radius + shake_minDelay; + float dutyShake = (dutyShakeMax - dutyShakeMin) * ratio + dutyShakeMin; - //evaluate state (on/off) + //evaluate state (motors on/off) if (data.radius > 0 ){ - //currently off + //currently off: if (shake_state == false){ //off long enough if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) { //turn on + cycleCount++; shake_state = true; shake_timestamp_turnedOn = esp_log_timestamp(); + ESP_LOGD(TAG_CMD, "shake: cycleCount=%d, msOn=%f, msOff=%f, radius=%f, shakeDuty=%f", cycleCount, msOn, msOff, data.radius, dutyShake); } } - //currently on + //currently on: else { //on long enough if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) { @@ -475,79 +507,49 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ // float angle; //} joystickData_t; - //--- evaluate stick position --- - //4 quadrants and center only - with X and Y axis as hysteresis - switch (data.position){ - - case joystickPos_t::CENTER: - //immediately set to center at center - stickQuadrant = joystickPos_t::CENTER; - break; - - case joystickPos_t::Y_AXIS: - //when moving from center to axis initially start in a certain quadrant - if (stickQuadrant == joystickPos_t::CENTER) { - if (data.y > 0){ - stickQuadrant = joystickPos_t::TOP_RIGHT; - } else { - stickQuadrant = joystickPos_t::BOTTOM_RIGHT; - } - } - break; - - case joystickPos_t::X_AXIS: - //when moving from center to axis initially start in a certain quadrant - if (stickQuadrant == joystickPos_t::CENTER) { - if (data.x > 0){ - stickQuadrant = joystickPos_t::TOP_RIGHT; - } else { - stickQuadrant = joystickPos_t::TOP_LEFT; - } - } - break; - - case joystickPos_t::TOP_RIGHT: - case joystickPos_t::TOP_LEFT: - case joystickPos_t::BOTTOM_LEFT: - case joystickPos_t::BOTTOM_RIGHT: - //update/change evaluated pos when in one of the 4 quadrants - stickQuadrant = data.position; - //TODO: maybe beep when switching mode? (difficult because beep object has to be passed to function) - break; + // force off when stick pos changes - TODO: is this necessary? + static joystickPos_t stickPosPrev = joystickPos_t::CENTER; + if (data.position != stickPosPrev) { + ESP_LOGW(TAG, "massage: stick quadrant changed, stopping for one cycle"); + shake_state = false; + shake_timestamp_turnedOff = esp_log_timestamp(); } - + stickPosPrev = data.position; // update last position //--- handle different modes (joystick in any of 4 quadrants) --- - switch (stickQuadrant){ + switch (data.position){ + // idle case joystickPos_t::CENTER: - case joystickPos_t::X_AXIS: //never true - case joystickPos_t::Y_AXIS: //never true commands.left.state = motorstate_t::IDLE; commands.right.state = motorstate_t::IDLE; commands.left.duty = 0; commands.right.duty = 0; - ESP_LOGI(TAG_CMD, "generate shake commands: CENTER -> idle"); + ESP_LOGD(TAG_CMD, "generate shake commands: CENTER -> idle"); return commands; break; - //4 different modes + // shake forward/reverse + case joystickPos_t::X_AXIS: + case joystickPos_t::Y_AXIS: case joystickPos_t::TOP_RIGHT: + case joystickPos_t::TOP_LEFT: commands.left.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD; break; - case joystickPos_t::TOP_LEFT: - commands.left.state = motorstate_t::REV; - commands.right.state = motorstate_t::REV; - break; + // shake left right case joystickPos_t::BOTTOM_LEFT: - commands.left.state = motorstate_t::REV; - commands.right.state = motorstate_t::FWD; - break; case joystickPos_t::BOTTOM_RIGHT: commands.left.state = motorstate_t::FWD; commands.right.state = motorstate_t::REV; break; } + // change direction every second on cycle in any mode + //(to not start driving on average) + if (cycleCount % 2 == 0) + { + invertMotorDirection(&commands.left.state); + invertMotorDirection(&commands.right.state); + } //--- turn motors on/off depending on pulsing shake variable --- if (shake_state == true){ @@ -562,11 +564,127 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ commands.right.duty = 0; } - - ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f", - joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); - ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); - ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); + ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f, cycleCount=%d, msOn=%f, msOff=%f", motorstateStr[(int)commands.left.state], commands.left.duty, cycleCount, msOn, msOff); return commands; } + + + + +// corresponding storage key strings to each joystickCalibratenMode variable +const char *calibrationStorageKeys[] = {"stick_x-min", "stick_x-max", "stick_y-min", "stick_y-max", "", ""}; + +//------------------------------- +//------- loadCalibration ------- +//------------------------------- +// loads selected calibration value from nvs or default values from config if no data stored +void evaluatedJoystick::loadCalibration(joystickCalibrationMode_t mode) +{ + // determine desired variables + int *configValue, *usedValue; + switch (mode) + { + case X_MIN: + configValue = &(config.x_min); + usedValue = &x_min; + break; + case X_MAX: + configValue = &(config.x_max); + usedValue = &x_max; + break; + case Y_MIN: + configValue = &(config.y_min); + usedValue = &y_min; + break; + case Y_MAX: + configValue = &(config.y_max); + usedValue = &y_max; + break; + case X_CENTER: + case Y_CENTER: + default: + // center position is not stored in nvs, it gets defined at startup or during calibration + ESP_LOGE(TAG, "loadCalibration: 'center_x' and 'center_y' are not stored in nvs -> not assigning anything"); + // defineCenter(); + return; + } + + // read from nvs + int16_t valueRead; + esp_err_t err = nvs_get_i16(*nvsHandle, calibrationStorageKeys[(int)mode], &valueRead); + switch (err) + { + case ESP_OK: + ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", calibrationStorageKeys[(int)mode], *configValue, valueRead); + *usedValue = (int)valueRead; + break; + case ESP_ERR_NVS_NOT_FOUND: + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, loading default value %d", calibrationStorageKeys[(int)mode], *configValue); + *usedValue = *configValue; + break; + default: + ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); + *usedValue = *configValue; + } +} + + + +//------------------------------- +//------- loadCalibration ------- +//------------------------------- +// loads selected calibration value from nvs or default values from config if no data stored +void evaluatedJoystick::writeCalibration(joystickCalibrationMode_t mode, int newValue) +{ + // determine desired variables + int *configValue, *usedValue; + switch (mode) + { + case X_MIN: + configValue = &(config.x_min); + usedValue = &x_min; + break; + case X_MAX: + configValue = &(config.x_max); + usedValue = &x_max; + break; + case Y_MIN: + configValue = &(config.y_min); + usedValue = &y_min; + break; + case Y_MAX: + configValue = &(config.y_max); + usedValue = &y_max; + break; + case X_CENTER: + x_center = newValue; + ESP_LOGW(TAG, "writeCalibration: 'center_x' or 'center_y' are not stored in nvs -> loading only"); + return; + case Y_CENTER: + y_center = newValue; + ESP_LOGW(TAG, "writeCalibration: 'center_x' or 'center_y' are not stored in nvs -> loading only"); + default: + return; + } + + // check if unchanged + if (*usedValue == newValue) + { + ESP_LOGW(TAG, "writeCalibration: value '%s' unchanged at %d, not writing to nvs", calibrationStorageKeys[(int)mode], newValue); + return; + } + + // update nvs value + ESP_LOGW(TAG, "writeCalibration: updating nvs value '%s' from %d to %d", calibrationStorageKeys[(int)mode], *usedValue, newValue); + esp_err_t err = nvs_set_i16(*nvsHandle, calibrationStorageKeys[(int)mode], newValue); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed writing"); + err = nvs_commit(*nvsHandle); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed committing updates"); + else + ESP_LOGI(TAG, "nvs: successfully committed updates"); + // update variable + *usedValue = newValue; +} \ No newline at end of file diff --git a/common/joystick.hpp b/common/joystick.hpp index ccf5bec..d17dd50 100644 --- a/common/joystick.hpp +++ b/common/joystick.hpp @@ -8,6 +8,9 @@ extern "C" #include "driver/adc.h" #include "esp_log.h" #include "esp_err.h" +#include "nvs_flash.h" +#include "nvs.h" +#include } #include @@ -55,6 +58,7 @@ typedef struct joystick_config_t { enum class joystickPos_t {CENTER, Y_AXIS, X_AXIS, TOP_RIGHT, TOP_LEFT, BOTTOM_LEFT, BOTTOM_RIGHT}; extern const char* joystickPosStr[7]; +typedef enum joystickCalibrationMode_t { X_MIN = 0, X_MAX, Y_MIN, Y_MAX, X_CENTER, Y_CENTER } joystickCalibrationMode_t; //struct with current data of the joystick typedef struct joystickData_t { @@ -65,36 +69,59 @@ typedef struct joystickData_t { float angle; } joystickData_t; - +// struct with parameters provided to joystick_GenerateCommandsDriving() +typedef struct joystickGenerateCommands_config_t +{ + float maxDutyStraight; // max duty applied when driving with ratio=1 (when turning it might increase by Boost) + float maxRelativeBoostPercentOfMaxDuty; // max duty percent added to outer tire when turning (max actual is 100-maxDutyStraight) - set 0 to disable + // note: to be able to reduce the overall driving speed boost has to be limited as well otherwise outer tire when turning would always be 100% no matter of maxDuty + float dutyOffset; // motors immediately start with this duty (duty movement starts) + float ratioSnapToOneThreshold; // have some area around X-Axis where inner tire is completely off - set 1 to disable + bool altStickMapping; // swap reverse direction +} joystickGenerateCommands_config_t; //------------------------------------ //----- evaluatedJoystick class ----- //------------------------------------ -class evaluatedJoystick { - public: - //--- constructor --- - evaluatedJoystick(joystick_config_t config_f); +class evaluatedJoystick +{ +public: + //--- constructor --- + evaluatedJoystick(joystick_config_t config_f, nvs_handle_t * nvsHandle); - //--- functions --- - joystickData_t getData(); //read joystick, calculate values and return the data in a struct - void defineCenter(); //define joystick center from current position + //--- functions --- + joystickData_t getData(); // read joystick, calculate values and return the data in a struct + // get raw adc value (inversion applied) + int getRawX() { return readAdc(config.adc_x, config.x_inverted); } + int getRawY() { return readAdc(config.adc_y, config.y_inverted); } + void defineCenter(); // define joystick center from current position + void writeCalibration(joystickCalibrationMode_t mode, int newValue); // load certain new calibration value and store it in nvs - private: - //--- functions --- - //initialize adc inputs, define center - void init(); - //read adc while making multiple samples with option to invert the result - int readAdc(adc1_channel_t adc_channel, bool inverted = false); +private: + //--- functions --- + // initialize adc inputs, define center + void init(); + // loads selected calibration value from nvs or default values from config if no data stored + void loadCalibration(joystickCalibrationMode_t mode); + // read adc while making multiple samples with option to invert the result + int readAdc(adc1_channel_t adc_channel, bool inverted = false); //--- variables --- + // handle for using the nvs flash (persistent config variables) + nvs_handle_t *nvsHandle; joystick_config_t config; + + int x_min; + int x_max; + int y_min; + int y_max; int x_center; int y_center; joystickData_t data; float x; float y; -}; + }; @@ -103,7 +130,7 @@ class evaluatedJoystick { //============================================ //function that generates commands for both motors from the joystick data //motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick); -motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altStickMapping = false); +motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config); diff --git a/common/motorctl.cpp b/common/motorctl.cpp index 1ff8417..ddbced1 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -5,25 +5,47 @@ //tag for logging static const char * TAG = "motor-control"; -#define TIMEOUT_IDLE_WHEN_NO_COMMAND 8000 +#define TIMEOUT_IDLE_WHEN_NO_COMMAND 15000 // turn motor off when still on and no new command received within that time +#define TIMEOUT_QUEUE_WHEN_AT_TARGET 5000 // time waited for new command when motors at target duty (e.g. IDLE) (no need to handle fading in fast loop) + +//==================================== +//========== motorctl task =========== +//==================================== +//task for handling the motors (ramp, current limit, driver) +void task_motorctl( void * ptrControlledMotor ){ + //get pointer to controlledMotor instance from task parameter + controlledMotor * motor = (controlledMotor *)ptrControlledMotor; + ESP_LOGW(TAG, "Task-motorctl [%s]: starting handle loop...", motor->getName()); + while(1){ + motor->handle(); + vTaskDelay(20 / portTICK_PERIOD_MS); + } +} + + //============================= //======== constructor ======== //============================= //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') -controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control): - cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) { +controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f): + //create current sensor + cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted), + configDefault(config_control){ //copy parameters for controlling the motor config = config_control; + log = config.loggingEnabled; //pointer to update motot dury method motorSetCommand = setCommandFunc; - //copy configured default fading durations to actually used variables - msFadeAccel = config.msFadeAccel; - msFadeDecel = config.msFadeDecel; + //pointer to nvs handle + nvsHandle = nvsHandle_f; + //pointer to other motor object + ppOtherMotor = otherMotor_f; + //pointer to speed sensor + sSensor = speedSensor_f; + //create queue, initialize config values init(); - //TODO: add currentsensor object here - //currentSensor cSensor(config.currentSensor_adc, config.currentSensor_ratedCurrent); } @@ -33,7 +55,19 @@ controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl //============================ void controlledMotor::init(){ commandQueue = xQueueCreate( 1, sizeof( struct motorCommand_t ) ); - //cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle? + if (commandQueue == NULL) + ESP_LOGE(TAG, "Failed to create command-queue"); + else + ESP_LOGI(TAG, "[%s] Initialized command-queue", config.name); + + // load config values from nvs, otherwise use default from config object + loadAccelDuration(); + loadDecelDuration(); + + // turn motor off initially + motorSetCommand({motorstate_t::IDLE, 0.00}); + + //cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle? } @@ -78,83 +112,271 @@ void controlledMotor::handle(){ //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor - //--- receive commands from queue --- - if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) ) + //--- RECEIVE DATA FROM QUEUE --- + if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already { - ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty); + if(log) ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty); state = commandReceive.state; dutyTarget = commandReceive.duty; receiveTimeout = false; timestamp_commandReceived = esp_log_timestamp(); + } + + + + + +// ----- EXPERIMENTAL, DIFFERENT MODES ----- +// define target duty differently depending on current contro-mode +//declare variables used inside switch +float ampereNow, ampereTarget, ampereDiff; +float speedDiff; + switch (mode) + { + case motorControlMode_t::DUTY: // regulate to desired duty (as originally) //--- convert duty --- - //define target duty (-100 to 100) from provided duty and motorstate - //this value is more suitable for the fading algorithm - switch(commandReceive.state){ - case motorstate_t::BRAKE: - //update state - state = motorstate_t::BRAKE; - //dutyTarget = 0; - dutyTarget = fabs(commandReceive.duty); - break; - case motorstate_t::IDLE: - dutyTarget = 0; - break; - case motorstate_t::FWD: - dutyTarget = fabs(commandReceive.duty); - break; - case motorstate_t::REV: - dutyTarget = - fabs(commandReceive.duty); - break; + // define target duty (-100 to 100) from provided duty and motorstate + // this value is more suitable for t + // todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm + switch (commandReceive.state) + { + case motorstate_t::BRAKE: + // update state + state = motorstate_t::BRAKE; + // dutyTarget = 0; + dutyTarget = fabs(commandReceive.duty); + break; + case motorstate_t::IDLE: + dutyTarget = 0; + break; + case motorstate_t::FWD: + dutyTarget = fabs(commandReceive.duty); + break; + case motorstate_t::REV: + dutyTarget = -fabs(commandReceive.duty); + break; } - } + break; - //--- timeout, no data --- - //turn motors off if no data received for long time (e.g. no uart data / control offline) - //TODO no timeout when braking? - if ((esp_log_timestamp() - timestamp_commandReceived) > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout){ - receiveTimeout = true; - state = motorstate_t::IDLE; - dutyTarget = 0; - ESP_LOGE(TAG, "TIMEOUT, no target data received for more than %ds -> switch to IDLE", TIMEOUT_IDLE_WHEN_NO_COMMAND/1000); - } +#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 1 //difference from target where no change is made yet +#define CURRENT_CONTROL_MIN_AMPERE 0.7 //current where motor is turned off +//TODO define different, fixed fading configuration in current mode, fade down can be significantly less (500/500ms fade up worked fine) + case motorControlMode_t::CURRENT: // regulate to desired current flow + ampereNow = cSensor.read(); + ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config + if (commandReceive.state == motorstate_t::REV) ampereTarget = - ampereTarget; //target is negative when driving reverse + ampereDiff = ampereTarget - ampereNow; + if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", config.name, ampereNow, ampereTarget, ampereDiff); // todo handle brake - //--- calculate increment --- - //calculate increment for fading UP with passed time since last run and configured fade time - int64_t usPassed = esp_timer_get_time() - timestampLastRunUs; - if (msFadeAccel > 0){ - dutyIncrementAccel = ( usPassed / ((float)msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment + //--- when IDLE to keep the current at target zero motor needs to be on for some duty (to compensate generator current) + if (commandReceive.duty == 0 && fabs(ampereNow) < CURRENT_CONTROL_MIN_AMPERE){ //stop motors completely when current is very low already + dutyTarget = 0; + } + else if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF || commandReceive.duty == 0) //#### BOOST BY 1 A + { + if (ampereDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase current + { + dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times + } + else if (ampereDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase current (more negative) + { + dutyTarget = -100; + } + else // fwd too much, rev too much -> decrease + { + dutyTarget = 0; + } + if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: set target to %.0f%%", config.name, dutyTarget); + } + else + { + dutyTarget = dutyNow; // target current reached + if(log) ESP_LOGD("TESTING", "[%s] CURRENT-CONTROL: target current %.3f reached", config.name, dutyTarget); + } + break; + +#define SPEED_CONTROL_MAX_SPEED_KMH 10 +#define SPEED_CONTROL_ALLOWED_KMH_DIFF 0.6 +#define SPEED_CONTROL_MIN_SPEED 0.7 //" start from standstill" always accelerate to this speed, ignoring speedsensor data + case motorControlMode_t::SPEED: // regulate to desired speed + speedNow = sSensor->getKmph(); + + //caculate target speed from input + speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config + // target speed negative when driving reverse + if (commandReceive.state == motorstate_t::REV) + speedTarget = -speedTarget; + if (sSensor->getTimeLastUpdate() != timestamp_speedLastUpdate ){ //only modify duty when new speed data available + timestamp_speedLastUpdate = sSensor->getTimeLastUpdate(); //TODO get time only once + speedDiff = speedTarget - speedNow; } else { - dutyIncrementAccel = 100; + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: no new speed data, not changing duty", config.name); + speedDiff = 0; + } + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff); + + //stop when target is 0 + if (commandReceive.duty == 0) { //TODO add IDLE, BRAKE state + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: OFF, target is 0... current-speed=%.2f, diff=%.3f", config.name, speedNow, speedDiff); + dutyTarget = 0; + } + else if (fabs(speedNow) < SPEED_CONTROL_MIN_SPEED){ //start from standstill or too slow (not enough speedsensor data) + if (log) + ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: starting from standstill -> increase duty... target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff); + if (commandReceive.state == motorstate_t::FWD) + dutyTarget = 100; + else if (commandReceive.state == motorstate_t::REV) + dutyTarget = -100; + } + else if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF) //speed too fast/slow + { + if (speedDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase speed + { + // TODO retain max duty here + dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (fwd), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget); + } + else if (speedDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase speed (more negative) + { + dutyTarget = -100; + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (rev), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget); + } + else // fwd too much, rev too much -> decrease + { + dutyTarget = 0; + if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to high, diff=%.2f, decreasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget); + } + } + else + { + dutyTarget = dutyNow; // target speed reached + if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget); + } + + break; } - //calculate increment for fading DOWN with passed time since last run and configured fade time - if (msFadeDecel > 0){ - dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100; - } else { - dutyIncrementDecel = 100; + + + +//--- TIMEOUT NO DATA --- +// turn motors off if no data received for a long time (e.g. no uart data or control task offline) +if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout) +{ + if(log) ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget); + receiveTimeout = true; + // set target and last command to IDLE + state = motorstate_t::IDLE; + commandReceive.state = motorstate_t::IDLE; + dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"? + commandReceive.duty = 0; +} + + + //--- CALCULATE DUTY-DIFF --- + dutyDelta = dutyTarget - dutyNow; + //positive: need to increase by that value + //negative: need to decrease + + + //--- DETECT ALREADY AT TARGET --- + // when already at exact target duty there is no need to run very fast to handle fading + //-> slow down loop by waiting significantly longer for new commands to arrive + if (mode != motorControlMode_t::CURRENT //dont slow down when in CURRENT mode at all + && ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled && mode != motorControlMode_t::SPEED) //when neither of current-limit, tractioncontrol or speed-mode is enabled slow down when target reached + || (dutyTarget == 0 && dutyNow == 0))) //otherwise only slow down when when actually off + { + //increase queue timeout when duty is the same (once) + if (timeoutWaitForCommand == 0) + { // TODO verify if state matches too? + if(log) ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget); + timeoutWaitForCommand = TIMEOUT_QUEUE_WHEN_AT_TARGET; // wait in queue very long, for new command to arrive + } + vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed } + //reset timeout when duty differs again (once) + else if (timeoutWaitForCommand != 0) + { + timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast + if(log) ESP_LOGI(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget); + // adjust lastRun timestamp to not mess up fading, due to much time passed but with no actual duty change + timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay + } + //TODO skip rest of the handle function below using return? Some regular driver updates sound useful though //--- BRAKE --- //brake immediately, update state, duty and exit this cycle of handle function if (state == motorstate_t::BRAKE){ - ESP_LOGD(TAG, "braking - skip fading"); + if(log) ESP_LOGD(TAG, "braking - skip fading"); motorSetCommand({motorstate_t::BRAKE, dutyTarget}); - ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow); + if(log) ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); //dutyNow = 0; return; //no need to run the fade algorithm } - //--- calculate difference --- - dutyDelta = dutyTarget - dutyNow; - //positive: need to increase by that value - //negative: need to decrease - - //----- FADING ----- - //fade duty to target (up and down) + //calculate passed time since last run + int64_t usPassed = esp_timer_get_time() - timestampLastRunUs; + + //--- calculate increment (acceleration) --- + //calculate increment for fading UP with passed time since last run and configured fade time + //- traction control - + if (tcs_isExceeded) // disable acceleration when slippage is currently detected + dutyIncrementAccel = 0; + //- recent braking - + //FIXME reset timeout when duty less + else if (isBraking && (esp_log_timestamp() - timestampBrakeStart) < config.brakePauseBeforeResume) // prevent immediate direction change when currently braking with timeout (eventually currently sliding) + { + if (log) ESP_LOGI(TAG, "pause after brake... -> accel = 0"); + dutyIncrementAccel = 0; + } + //- normal accel - + else if (config.msFadeAccel > 0) + dutyIncrementAccel = (usPassed / ((float)config.msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment + //- sport mode - + else //no accel limit (immediately set to 100) + dutyIncrementAccel = 100; + + //--- calculate increment (deceleration) --- + //- sport mode - + if (config.msFadeDecel == 0){ //no decel limit (immediately reduce to 0) + dutyIncrementDecel = 100; + } + //- brake - + //detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast) + #define NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY 10 //TODO test/adjust this - dont brake when slow already (avoids starting full dead time) + else if (commandReceive.state != state && // direction differs + fabs(dutyNow) > NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY && // not very slow already + fabs(dutyTarget) > brakeStartThreshold) // joystick above threshold + { + // set braking state and track start time (both for disabling acceleration for some time) + if (!isBraking) { + if (log) ESP_LOGW(TAG, "started braking..."); + timestampBrakeStart = esp_log_timestamp(); + isBraking = true; + } + // use brake deceleration instead of normal deceleration + dutyIncrementDecel = (usPassed / ((float)config.brakeDecel * 1000)) * 100; + if(log) ESP_LOGI(TAG, "braking (target duty >%.0f%% in other direction) -> using deceleration %dms", brakeStartThreshold, config.brakeDecel); + } + //- normal deceleration - + else { + // normal deceleration according to configured time + dutyIncrementDecel = (usPassed / ((float)config.msFadeDecel * 1000)) * 100; + } + + // reset braking state when start condition is no longer met (stick below threshold again) + if (isBraking && + (fabs(dutyTarget) < brakeStartThreshold || commandReceive.state == state)) + { + ESP_LOGW(TAG, "brake condition no longer met"); + isBraking = false; + } + + //--- fade duty to target (up and down) --- //TODO: this needs optimization (can be more clear and/or simpler) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) if (dutyNow < 0) { //reverse, decelerating @@ -174,25 +396,103 @@ void controlledMotor::handle(){ } - //----- CURRENT LIMIT ----- + //----- CURRENT LIMIT ----- currentNow = cSensor.read(); if ((config.currentLimitEnabled) && (dutyDelta != 0)){ if (fabs(currentNow) > config.currentMax){ float dutyOld = dutyNow; //adaptive decrement: //Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values - dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)msFadeDecel * 1500) ) * 100; + dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)config.msFadeDecel * 1500) ) * 100; float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0 if (dutyNow < -currentLimitDecrement) { dutyNow += currentLimitDecrement; } else if (dutyNow > currentLimitDecrement) { dutyNow -= currentLimitDecrement; } - ESP_LOGW(TAG, "current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", currentNow, config.currentMax, dutyOld, dutyNow); + if(log) ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow); } } + + //----- TRACTION CONTROL ----- + //reduce duty when turning faster than expected + //TODO only run this when speed sensors actually updated + //handle tcs when enabled and new speed sensor data is available TODO: currently assumes here that speed sensor data of other motor updated as well + #define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down + #define TCS_NO_SPEED_DATA_TIMEOUT_US 200*1000 + #define TCS_MIN_SPEED_KMH 1 //must be at least that fast for TCS to be enabled + //TODO rework this: clearer structure (less nested if statements) + if (config.tractionControlSystemEnabled && mode == motorControlMode_t::SPEED && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate && (esp_timer_get_time() - tcs_timestampLastRun < TCS_NO_SPEED_DATA_TIMEOUT_US)){ + //update last speed update received + tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate + + //calculate time passed since last run + uint32_t tcs_usPassed = esp_timer_get_time() - tcs_timestampLastRun; // passed time since last time handled + tcs_timestampLastRun = esp_timer_get_time(); + + //get motor stats + float speedNowThis = sSensor->getKmph(); + float speedNowOther = (*ppOtherMotor)->getCurrentSpeed(); + float speedTargetThis = speedTarget; + float speedTargetOther = (*ppOtherMotor)->getTargetSpeed(); + float dutyTargetOther = (*ppOtherMotor)->getTargetDuty(); + float dutyTargetThis = dutyTarget; + float dutyNowOther = (*ppOtherMotor)->getDuty(); + float dutyNowThis = dutyNow; + + + //calculate expected ratio + float ratioSpeedTarget = speedTargetThis / speedTargetOther; + //calculate current ratio of actual measured rotational speed + float ratioSpeedNow = speedNowThis / speedNowOther; + //calculate current duty ration (logging only) + float ratioDutyNow = dutyNowThis / dutyNowOther; + + //calculate unexpected difference + float ratioDiff = ratioSpeedNow - ratioSpeedTarget; + if(log) ESP_LOGD("TESTING", "[%s] TCS: speedThis=%.3f, speedOther=%.3f, ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, speedNowThis, speedNowOther, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff); + + //-- handle rotating faster than expected -- + //TODO also increase duty when other motor is slipping? (diff negative) + if (speedNowThis < TCS_MIN_SPEED_KMH) { //disable / turn off TCS when currently too slow (danger of deadlock) + tcs_isExceeded = false; + tcs_usExceeded = 0; + } + else if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF ) // motor turns too fast compared to expected target ratio + { + if (!tcs_isExceeded) // just started being too fast + { + tcs_timestampBeginExceeded = esp_timer_get_time(); + tcs_isExceeded = true; //also blocks further acceleration (fade) + if(log) ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF); + } + else + { // too fast for more than 2 cycles already + tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already + if(log) ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff); + // calculate amount duty gets decreased + float dutyDecrement = (tcs_usPassed / ((float)config.msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded + // decrease duty + if(log) ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement); + fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0 + } + } + else + { // not exceeded + tcs_isExceeded = false; + tcs_usExceeded = 0; + } + } + else // TCS mode not active or timed out + { // not exceeded + tcs_isExceeded = false; + tcs_usExceeded = 0; + } + + + //--- define new motorstate --- (-100 to 100 => direction) state=getStateFromDuty(dutyNow); @@ -202,25 +502,27 @@ void controlledMotor::handle(){ //FWD -> IDLE -> FWD continue without waiting //FWD -> IDLE -> REV wait for dead-time in IDLE //TODO check when changed only? - if ( //not enough time between last direction state - ( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) - || (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs)) - ){ - ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); - if (!deadTimeWaiting){ //log start - deadTimeWaiting = true; - ESP_LOGW(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); - } - //force IDLE state during wait - state = motorstate_t::IDLE; - dutyNow = 0; - } else { - if (deadTimeWaiting){ //log end - deadTimeWaiting = false; - ESP_LOGW(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); - } - ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow); - } + if (config.deadTimeMs > 0) { //deadTime is enabled + if ( //not enough time between last direction state + ( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) + || (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs)) + ){ + if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); + if (!deadTimeWaiting){ //log start + deadTimeWaiting = true; + if(log) ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); + } + //force IDLE state during wait + state = motorstate_t::IDLE; + dutyNow = 0; + } else { + if (deadTimeWaiting){ //log end + deadTimeWaiting = false; + if(log) ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); + } + if(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow); + } + } //--- save current actual motorstate and timestamp --- @@ -232,7 +534,7 @@ void controlledMotor::handle(){ //--- apply new target to motor --- motorSetCommand({state, (float)fabs(dutyNow)}); - ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow); + if(log) ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); //note: BRAKE state is handled earlier @@ -247,17 +549,18 @@ void controlledMotor::handle(){ //=============================== //function to set the target mode and duty of a motor //puts the provided command in a queue for the handle function running in another task -void controlledMotor::setTarget(motorstate_t state_f, float duty_f){ - commandSend = { - .state = state_f, - .duty = duty_f - }; - - ESP_LOGD(TAG, "Inserted command to queue: state=%s, duty=%.2f", motorstateStr[(int)commandSend.state], commandSend.duty); +void controlledMotor::setTarget(motorCommand_t commandSend){ + if(log) ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty); //send command to queue (overwrite if an old command is still in the queue and not processed) xQueueOverwrite( commandQueue, ( void * )&commandSend); //xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 ); + if(log) ESP_LOGD(TAG, "finished inserting new command"); +} +// accept target state and duty as separate agrguments: +void controlledMotor::setTarget(motorstate_t state_f, float duty_f){ + // create motorCommand struct from the separate parameters, and run the method to insert that new command + setTarget({state_f, duty_f}); } @@ -277,20 +580,63 @@ motorCommand_t controlledMotor::getStatus(){ +//=============================== +//=========== getFade =========== +//=============================== +//return currently configured accel / decel time +uint32_t controlledMotor::getFade(fadeType_t fadeType){ + switch(fadeType){ + case fadeType_t::ACCEL: + return config.msFadeAccel; + break; + case fadeType_t::DECEL: + return config.msFadeDecel; + break; + } + return 0; +} + +//============================== +//======= getFadeDefault ======= +//============================== +//return default accel / decel time (from config) +uint32_t controlledMotor::getFadeDefault(fadeType_t fadeType){ + switch(fadeType){ + case fadeType_t::ACCEL: + return configDefault.msFadeAccel; + break; + case fadeType_t::DECEL: + return configDefault.msFadeDecel; + break; + } + return 0; +} + + + //=============================== //=========== setFade =========== //=============================== //function for editing or enabling the fading/ramp of the motor control //set/update fading duration/amount -void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew){ +void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew, bool writeToNvs){ //TODO: mutex for msFade variable also used in handle function switch(fadeType){ case fadeType_t::ACCEL: - msFadeAccel = msFadeNew; + ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, config.msFadeAccel, msFadeNew); + if (writeToNvs) + writeAccelDuration(msFadeNew); + else + config.msFadeAccel = msFadeNew; break; case fadeType_t::DECEL: - msFadeDecel = msFadeNew; + ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, config.msFadeDecel, msFadeNew); + // write new value to nvs and update the variable + if (writeToNvs) + writeDecelDuration(msFadeNew); + else + config.msFadeDecel = msFadeNew; break; } } @@ -324,16 +670,16 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){ bool enabled = false; switch(fadeType){ case fadeType_t::ACCEL: - if (msFadeAccel == 0){ - msFadeNew = config.msFadeAccel; + if (config.msFadeAccel == 0){ + msFadeNew = configDefault.msFadeAccel; enabled = true; } else { msFadeNew = 0; } break; case fadeType_t::DECEL: - if (msFadeDecel == 0){ - msFadeNew = config.msFadeAccel; + if (config.msFadeDecel == 0){ + msFadeNew = configDefault.msFadeAccel; enabled = true; } else { msFadeNew = 0; @@ -347,3 +693,114 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){ return enabled; } + + + +//----------------------------- +//----- loadAccelDuration ----- +//----------------------------- +// load stored value from nvs if not successfull uses config default value +void controlledMotor::loadAccelDuration(void) +{ + // read from nvs + uint32_t valueNew; + char key[15]; + snprintf(key, 15, "m-%s-accel", config.name); + esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew); + switch (err) + { + case ESP_OK: + ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, configDefault.msFadeAccel, valueNew); + config.msFadeAccel = valueNew; + break; + case ESP_ERR_NVS_NOT_FOUND: + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeAccel); + break; + default: + ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); + } +} + +//----------------------------- +//----- loadDecelDuration ----- +//----------------------------- +void controlledMotor::loadDecelDuration(void) +{ + // read from nvs + uint32_t valueNew; + char key[15]; + snprintf(key, 15, "m-%s-decel", config.name); + esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew); + switch (err) + { + case ESP_OK: + ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeDecel, valueNew); + config.msFadeDecel = valueNew; + break; + case ESP_ERR_NVS_NOT_FOUND: + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeDecel); + break; + default: + ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); + } +} + + + + +//------------------------------ +//----- writeAccelDuration ----- +//------------------------------ +// write provided value to nvs to be persistent and update the local config +void controlledMotor::writeAccelDuration(uint32_t newValue) +{ + // check if unchanged + if(config.msFadeAccel == newValue){ + ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue); + return; + } + // generate nvs storage key + char key[15]; + snprintf(key, 15, "m-%s-accel", config.name); + // update nvs value + ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeAccel, newValue); + esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed writing"); + err = nvs_commit(*nvsHandle); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed committing updates"); + else + ESP_LOGI(TAG, "nvs: successfully committed updates"); + // update variable + config.msFadeAccel = newValue; +} + +//------------------------------ +//----- writeDecelDuration ----- +//------------------------------ +// write provided value to nvs to be persistent and update the local config +// TODO: reduce duplicate code +void controlledMotor::writeDecelDuration(uint32_t newValue) +{ + // check if unchanged + if(config.msFadeDecel == newValue){ + ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue); + return; + } + // generate nvs storage key + char key[15]; + snprintf(key, 15, "m-%s-decel", config.name); + // update nvs value + ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeDecel, newValue); + esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed writing"); + err = nvs_commit(*nvsHandle); + if (err != ESP_OK) + ESP_LOGE(TAG, "nvs: failed committing updates"); + else + ESP_LOGI(TAG, "nvs: successfully committed updates"); + // update variable + config.msFadeDecel = newValue; +} \ No newline at end of file diff --git a/common/motorctl.hpp b/common/motorctl.hpp index bea2060..55d13e8 100644 --- a/common/motorctl.hpp +++ b/common/motorctl.hpp @@ -7,10 +7,13 @@ extern "C" #include "freertos/queue.h" #include "esp_log.h" #include "esp_timer.h" +#include "nvs_flash.h" +#include "nvs.h" } #include "motordrivers.hpp" #include "currentsensor.hpp" +#include "speedsensor.hpp" //======================================= @@ -21,6 +24,7 @@ extern "C" typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); +enum class motorControlMode_t {DUTY, CURRENT, SPEED}; //=================================== //====== controlledMotor class ====== @@ -28,49 +32,88 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); class controlledMotor { public: //--- functions --- - controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor + //TODO move speedsensor object creation in this class to (pass through / wrap methods) + controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle, speedSensor * speedSensor, controlledMotor ** otherMotor); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task) void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function + void setTarget(motorCommand_t command); motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty) + float getDuty() {return dutyNow;}; + float getTargetDuty() {return dutyTarget;}; + float getTargetSpeed() {return speedTarget;}; + float getCurrentSpeed() {return sSensor->getKmph();}; + void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;}; + void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;}; + bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;}; + void setControlMode(motorControlMode_t newMode) {mode = newMode;}; + void setBrakeStartThresholdDuty(float duty) {brakeStartThreshold = duty;}; + void setBrakeDecel(uint32_t msFadeBrake) {config.brakeDecel = msFadeBrake;}; + uint32_t getBrakeDecel() {return config.brakeDecel;}; //todo store and load from nvs + uint32_t getBrakeDecelDefault() {return configDefault.brakeDecel;}; + uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time + uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config void setFade(fadeType_t fadeType, bool enabled); //enable/disable acceleration or deceleration fading - void setFade(fadeType_t fadeType, uint32_t msFadeNew); //set acceleration or deceleration fade time + void setFade(fadeType_t fadeType, uint32_t msFadeNew, bool writeToNvs = true); //set acceleration or deceleration fade time and write it to nvs by default bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off + + float getCurrentA() {return cSensor.read();}; //read current-sensor of this motor (Ampere) + char * getName() const {return config.name;}; //TODO set current limit method private: //--- functions --- - void init(); //creates currentsensor objects, motordriver objects and queue + void init(); // creates command-queue and initializes config values + void loadAccelDuration(void); // load stored value for msFadeAccel from nvs + void loadDecelDuration(void); + void writeAccelDuration(uint32_t newValue); // write value to nvs and update local variable + void writeDecelDuration(uint32_t newValue); //--- objects --- //queue for sending commands to the separate task running the handle() function very fast QueueHandle_t commandQueue = NULL; //current sensor currentSensor cSensor; + //speed sensor + speedSensor * sSensor; + //other motor (needed for traction control) + controlledMotor ** ppOtherMotor; //ptr to ptr of controlledMotor (because not created at initialization yet) + //function pointer that sets motor duty (driver) motorSetCommandFunc_t motorSetCommand; //--- variables --- + //TODO add name for logging? //struct for storing control specific parameters motorctl_config_t config; + const motorctl_config_t configDefault; //backup default configuration (unchanged) + bool log = false; motorstate_t state = motorstate_t::IDLE; + motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode + //handle for using the nvs flash (persistent config variables) + nvs_handle_t * nvsHandle; float currentMax; float currentNow; - float dutyTarget; - float dutyNow; + //speed mode + float speedTarget = 0; + float speedNow = 0; + uint32_t timestamp_speedLastUpdate = 0; + + + float dutyTarget = 0; + float dutyNow = 0; + float dutyIncrementAccel; float dutyIncrementDecel; float dutyDelta; - - uint32_t msFadeAccel; - uint32_t msFadeDecel; + uint32_t timeoutWaitForCommand = 0; uint32_t ramp; - int64_t timestampLastRunUs; + int64_t timestampLastRunUs = 0; bool deadTimeWaiting = false; uint32_t timestampsModeLastActive[4] = {}; @@ -81,4 +124,24 @@ class controlledMotor { uint32_t timestamp_commandReceived = 0; bool receiveTimeout = false; + + //traction control system + uint32_t tcs_timestampLastSpeedUpdate = 0; //track speedsensor update + int64_t tcs_timestampBeginExceeded = 0; //track start of event + uint32_t tcs_usExceeded = 0; //sum up time + bool tcs_isExceeded = false; //is currently too fast + int64_t tcs_timestampLastRun = 0; + + //brake (decel boost) + uint32_t timestampBrakeStart = 0; + bool isBraking = false; + float brakeStartThreshold = 60; }; + +//==================================== +//========== motorctl task =========== +//==================================== +// note: pointer to a 'controlledMotor' object has to be provided as task-parameter +// runs handle method of certain motor repeatedly: +// receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr) +void task_motorctl( void * controlledMotor ); diff --git a/common/speedsensor.cpp b/common/speedsensor.cpp index 22c7f2e..eadb0ad 100644 --- a/common/speedsensor.cpp +++ b/common/speedsensor.cpp @@ -7,6 +7,9 @@ static const char* TAG = "speedSensor"; +//initialize ISR only once (for multiple instances) +bool speedSensor::isrIsInitialized = false; + uint32_t min(uint32_t a, uint32_t b){ if (a>b) return b; @@ -16,63 +19,82 @@ uint32_t min(uint32_t a, uint32_t b){ //========================================= -//========== ISR onEncoderChange ========== +//========== ISR onEncoderRising ========== //========================================= -//handle gpio edge event +//handle gpio rising edge event //determines direction and rotational speed with a speedSensor object -void IRAM_ATTR onEncoderChange(void* arg) { - speedSensor* sensor = (speedSensor*)arg; +void IRAM_ATTR onEncoderRising(void *arg) +{ + speedSensor *sensor = (speedSensor *)arg; int currentState = gpio_get_level(sensor->config.gpioPin); - //detect rising edge LOW->HIGH (reached end of gap in encoder disk) - if (currentState == 1 && sensor->prevState == 0) { - //time since last edge in us - uint32_t currentTime = esp_timer_get_time(); - uint32_t timeElapsed = currentTime - sensor->lastEdgeTime; - sensor->lastEdgeTime = currentTime; //update last edge time + // time since last edge in us + uint32_t currentTime = esp_timer_get_time(); + uint32_t timeElapsed = currentTime - sensor->lastEdgeTime; + sensor->lastEdgeTime = currentTime; // update last edge time - //store duration of last pulse - sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; - sensor->pulseCounter++; + // store duration of last pulse + sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; + sensor->pulseCounter++; - //check if 3rd pulse has occoured - if (sensor->pulseCounter >= 3) { - sensor->pulseCounter = 0; //reset counter + // check if 3rd pulse has occoured (one sequence recorded) + if (sensor->pulseCounter >= 3) + { + sensor->pulseCounter = 0; // reset count - //simplify variable names - uint32_t pulse1 = sensor->pulseDurations[0]; - uint32_t pulse2 = sensor->pulseDurations[1]; - uint32_t pulse3 = sensor->pulseDurations[2]; + // simplify variable names + uint32_t pulse1 = sensor->pulseDurations[0]; + uint32_t pulse2 = sensor->pulseDurations[1]; + uint32_t pulse3 = sensor->pulseDurations[2]; - //find shortest pulse - uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3)); + // save all recored pulses of this sequence (for logging only) + sensor->pulse1 = pulse1; + sensor->pulse2 = pulse2; + sensor->pulse3 = pulse3; - //Determine direction based on pulse order - int directionNew = 0; - if (shortestPulse == pulse1) { //short-medium-long... - directionNew = 1; //fwd - } else if (shortestPulse == pulse3) { //long-medium-short... - directionNew = -1; //rev - } else if (shortestPulse == pulse2) { - if (pulse1 < pulse3){ //medium short long-medium-short long... - directionNew = -1; //rev - } else { //long short-medium-long short-medium-long... - directionNew = 1; //fwd - } - } + // find shortest pulse + sensor->shortestPulse = min(pulse1, min(pulse2, pulse3)); - //save and invert direction if necessay - //TODO mutex? - if (sensor->config.directionInverted) sensor->direction = -directionNew; - else sensor->direction = directionNew; - - //calculate rotational speed - uint64_t pulseSum = pulse1 + pulse2 + pulse3; - sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); + // ignore this pulse sequence if one pulse is too short (possible noise) + if (sensor->shortestPulse < sensor->config.minPulseDurationUs) + { + sensor->debug_countIgnoredSequencesTooShort++; + return; } + + //--- Determine direction based on pulse order --- + int direction = 0; + if (sensor->shortestPulse == pulse1) // short... + { + if (pulse2 < pulse3) // short-medium-long --> + direction = 1; + else // short-long-medium <-- + direction = -1; + } + else if (sensor->shortestPulse == pulse3) //...short + { + if (pulse1 > pulse2) // long-medium-short <-- + direction = -1; + else // medium-long-short --> + direction = 1; + } + else if (sensor->shortestPulse == pulse2) //...short... + { + if (pulse1 < pulse3) // medium-short-long + direction = -1; + else // long-short-medium + direction = 1; + } + + // save and invert direction if necessay + if (sensor->config.directionInverted) + direction = -direction; + + // calculate rotational speed + uint64_t pulseSum = pulse1 + pulse2 + pulse3; + sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); + sensor->timeLastUpdate = currentTime; } - //store current pin state for next edge detection - sensor->prevState = currentState; } @@ -84,11 +106,8 @@ void IRAM_ATTR onEncoderChange(void* arg) { speedSensor::speedSensor(speedSensor_config_t config_f){ //copy config config = config_f; - //note: currently gets initialized at first method call - //this prevents crash due to too early initialization at boot - //TODO: create global objects later after boot //init gpio and ISR - //init(); + init(); } @@ -102,15 +121,16 @@ void speedSensor::init() { gpio_pad_select_gpio(config.gpioPin); gpio_set_direction(config.gpioPin, GPIO_MODE_INPUT); gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY); - ESP_LOGW(TAG, "%s, configured gpio-pin %d", config.logName, (int)config.gpioPin); //configure interrupt - gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE); - gpio_install_isr_service(0); - gpio_isr_handler_add(config.gpioPin, onEncoderChange, this); - ESP_LOGW(TAG, "%s, configured interrupt", config.logName); - - isInitialized = true; + gpio_set_intr_type(config.gpioPin, GPIO_INTR_POSEDGE); + if (!isrIsInitialized) { + gpio_install_isr_service(0); + isrIsInitialized = true; + ESP_LOGW(TAG, "Initialized ISR service"); + } + gpio_isr_handler_add(config.gpioPin, onEncoderRising, this); + ESP_LOGW(TAG, "[%s], configured gpio-pin %d and interrupt routine", config.logName, (int)config.gpioPin); } @@ -121,41 +141,38 @@ void speedSensor::init() { //========================== //get rotational speed in revolutions per minute float speedSensor::getRpm(){ - //check if initialized - if (!isInitialized) init(); uint32_t timeElapsed = esp_timer_get_time() - lastEdgeTime; //timeout (standstill) //TODO variable timeout considering config.degreePerGroup if ((currentRpm != 0) && (esp_timer_get_time() - lastEdgeTime) > TIMEOUT_NO_ROTATION*1000){ - ESP_LOGW(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0", + ESP_LOGI(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0", config.logName, TIMEOUT_NO_ROTATION, timeElapsed/1000); currentRpm = 0; } //debug output (also log variables when this function is called) - ESP_LOGI(TAG, "%s - getRpm: returning stored rpm=%.3f", config.logName, currentRpm); - ESP_LOGV(TAG, "%s - rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d lastEdgetime=%d", - config.logName, - currentRpm, - direction, - pulseCounter, - (int)pulseDurations[0]/1000, - (int)pulseDurations[1]/1000, - (int)pulseDurations[2]/1000, - (int)lastEdgeTime); - + ESP_LOGD(TAG, "[%s] getRpm: returning stored rpm=%.3f", config.logName, currentRpm); + ESP_LOGV(TAG, "[%s] rpm=%f, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, totalTooShortCount=%d", + config.logName, + currentRpm, + pulseCounter, + pulse1 / 1000, + pulse2 / 1000, + pulse3 / 1000, + shortestPulse / 1000, + debug_countIgnoredSequencesTooShort); //return currently stored rpm return currentRpm; } -//========================== +//=========================== //========= getKmph ========= -//========================== +//=========================== //get speed in kilometers per hour float speedSensor::getKmph(){ float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000; - ESP_LOGI(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed); + ESP_LOGD(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed); return currentSpeed; } @@ -165,7 +182,7 @@ float speedSensor::getKmph(){ //========================== //get speed in meters per second float speedSensor::getMps(){ - float currentSpeed = getRpm() * config.tireCircumferenceMeter; - ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); + float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60; + ESP_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); return currentSpeed; } diff --git a/common/speedsensor.hpp b/common/speedsensor.hpp index c6e93bf..7b3bb6b 100644 --- a/common/speedsensor.hpp +++ b/common/speedsensor.hpp @@ -12,8 +12,9 @@ extern "C" { typedef struct { gpio_num_t gpioPin; float degreePerGroup; //360 / [count of short,medium,long groups on encoder disk] + uint32_t minPulseDurationUs; //smallest possible pulse duration (time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection float tireCircumferenceMeter; - //positive direction is pulse order "short, medium, long" + //default positive direction is pulse order "short, medium, long" bool directionInverted; char* logName; } speedSensor_config_t; @@ -24,30 +25,31 @@ class speedSensor { public: //constructor speedSensor(speedSensor_config_t config); - //initializes gpio pin and configures interrupt - void init(); + // initializes gpio pin, configures and starts interrupt + void init(); //negative values = reverse direction //positive values = forward direction float getKmph(); //kilometers per hour float getMps(); //meters per second float getRpm(); //rotations per minute + uint32_t getTimeLastUpdate() {return timeLastUpdate;}; - //1=forward, -1=reverse - int direction; - - //variables for handling the encoder + //variables for handling the encoder (public because ISR needs access) speedSensor_config_t config; - int prevState = 0; - uint64_t pulseDurations[3] = {}; - uint64_t lastEdgeTime = 0; + uint32_t pulseDurations[3] = {}; + uint32_t pulse1, pulse2, pulse3; + uint32_t shortestPulse = 0; + uint32_t shortestPulsePrev = 0; + uint32_t lastEdgeTime = 0; uint8_t pulseCounter = 0; int debugCount = 0; + uint32_t debug_countIgnoredSequencesTooShort = 0; double currentRpm = 0; - bool isInitialized = false; + uint32_t timeLastUpdate = 0; private: - + static bool isrIsInitialized; // default false due to static }; diff --git a/common/types.hpp b/common/types.hpp index 6cf868e..23f5d04 100644 --- a/common/types.hpp +++ b/common/types.hpp @@ -12,6 +12,7 @@ extern "C" //====== struct/type declarations ====== //======================================= //global structs and types that need to be available for all boards +//this file is necessary to prevent dependency loop between motordrivers.hpp and motorctl.hpp since //=============================== @@ -40,13 +41,20 @@ typedef struct motorCommands_t { //struct with all config parameters for a motor regarding ramp and current limit typedef struct motorctl_config_t { + char * name; //name for unique nvs storage-key prefix and logging + bool loggingEnabled; //enable/disable ALL log output (mostly better to debug only one instance) uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%) uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%) bool currentLimitEnabled; + bool tractionControlSystemEnabled; adc1_channel_t currentSensor_adc; float currentSensor_ratedCurrent; float currentMax; + bool currentInverted; + float currentSnapToZeroThreshold; uint32_t deadTimeMs; //time motor stays in IDLE before direction change + uint32_t brakePauseBeforeResume; + uint32_t brakeDecel; } motorctl_config_t; //enum fade type (acceleration, deceleration) diff --git a/common/wifi.c b/common/wifi.c index 27eb3db..557abfc 100644 --- a/common/wifi.c +++ b/common/wifi.c @@ -21,26 +21,45 @@ static const char *TAG = "wifi"; static esp_event_handler_instance_t instance_any_id; -//============================================ -//============ init nvs and netif ============ -//============================================ -//initialize nvs-flash and netif (needed for both AP and CLIENT) +//########################################## +//############ common functions ############ +//########################################## + +//============================ +//========= init nvs ========= +//============================ +//initialize nvs-flash (needed for both AP and CLIENT) //has to be run once at startup -void wifi_initNvs_initNetif(){ +void wifi_initNvs(){ //Initialize NVS (needed for wifi) - esp_err_t ret = nvs_flash_init(); - if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { - ESP_ERROR_CHECK(nvs_flash_erase()); - ret = nvs_flash_init(); - } + esp_err_t err = nvs_flash_init(); + if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) + { + ESP_LOGE(TAG, "NVS truncated -> deleting flash"); + // Retry nvs_flash_init + ESP_ERROR_CHECK(nvs_flash_erase()); + err = nvs_flash_init(); + } + ESP_ERROR_CHECK(err); +} + + +//============================== +//========= init netif ========= +//============================== +//initialize netif (needed for both AP and CLIENT) +//has to be run once at startup +void wifi_initNetif(){ ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_event_loop_create_default()); } -//=========================================== -//============ init access point ============ -//=========================================== + + +//############################################ +//############### access point ############### +//############################################ //-------------------------------------------- //------ configuration / declarations -------- @@ -66,10 +85,12 @@ static void wifi_event_handler_ap(void* arg, esp_event_base_t event_base, } } -//----------------------- -//------ init ap -------- -//----------------------- -void wifi_init_ap(void) + + +//======================== +//====== start AP ======== +//======================== +void wifi_start_ap(void) { ap = esp_netif_create_default_wifi_ap(); @@ -107,9 +128,9 @@ void wifi_init_ap(void) //============================= -//========= deinit AP ========= +//========== stop AP ========== //============================= -void wifi_deinit_ap(void) +void wifi_stop_ap(void) { ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); @@ -123,9 +144,9 @@ void wifi_deinit_ap(void) -//=========================================== -//=============== init client =============== -//=========================================== +//########################################## +//################# client ################# +//########################################## //-------------------------------------------- //------ configuration / declarations -------- @@ -168,10 +189,13 @@ static void event_handler(void* arg, esp_event_base_t event_base, xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); } } -//--------------------------- -//------ init client -------- -//--------------------------- -void wifi_init_client(void) + + + +//=========================== +//====== init client ======== +//=========================== +void wifi_start_client(void) { s_wifi_event_group = xEventGroupCreate(); sta = esp_netif_create_default_wifi_sta(); @@ -249,10 +273,10 @@ void wifi_init_client(void) -//================================= -//========= deinit client ========= -//================================= -void wifi_deinit_client(void) +//=============================== +//========= stop client ========= +//=============================== +void wifi_stop_client(void) { /* The event will not be processed after unregister */ ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip)); diff --git a/common/wifi.h b/common/wifi.h index 39eecee..d65a3bf 100644 --- a/common/wifi.h +++ b/common/wifi.h @@ -3,20 +3,20 @@ //TODO: currently wifi names and passwords are configured in wifi.c -> move this to config? //initialize nvs-flash and netif (needed for both AP and CLIENT) -//has to be run once at startup -//Note: this cant be put in wifi_init functions because this may not be in deinit functions -void wifi_initNvs_initNetif(); +//both functions have to be run once at startup +void wifi_initNvs(); +void wifi_initNetif(); -//function to start an access point -void wifi_init_ap(void); -//function to disable/deinit access point -void wifi_deinit_ap(void); +//function to start an access point (config in wifi.c) +void wifi_start_ap(void); +//function to disable/stop access point +void wifi_stop_ap(void); -//function to connect to existing wifi network -void wifi_init_client(void); +//function to connect to existing wifi network (config in wifi.c) +void wifi_start_client(void); //function to disable/deinit client -void wifi_deinit_client(void); +void wifi_stop_client(void); diff --git a/components/encoder/.eil.yml b/components/encoder/.eil.yml new file mode 100644 index 0000000..d741eb2 --- /dev/null +++ b/components/encoder/.eil.yml @@ -0,0 +1,21 @@ +name: encoder +description: HW timer-based driver for incremental rotary encoders +version: 1.0.0 +groups: + - input +code_owners: + - UncleRus +depends: + - driver + - freertos + - log +thread_safe: yes +targets: + - esp32 + - esp8266 + - esp32s2 + - esp32c3 +license: BSD-3 +copyrights: + - name: UncleRus + year: 2019 diff --git a/components/encoder/CMakeLists.txt b/components/encoder/CMakeLists.txt new file mode 100644 index 0000000..0dc9a93 --- /dev/null +++ b/components/encoder/CMakeLists.txt @@ -0,0 +1,13 @@ +if(${IDF_TARGET} STREQUAL esp8266) + set(req esp8266 freertos log esp_timer) +elseif(${IDF_VERSION_MAJOR} STREQUAL 4 AND ${IDF_VERSION_MINOR} STREQUAL 1 AND ${IDF_VERSION_PATCH} STREQUAL 3) + set(req driver freertos log) +else() + set(req driver freertos log esp_timer) +endif() + +idf_component_register( + SRCS encoder.c + INCLUDE_DIRS . + REQUIRES ${req} +) diff --git a/components/encoder/Kconfig b/components/encoder/Kconfig new file mode 100644 index 0000000..5ef2a56 --- /dev/null +++ b/components/encoder/Kconfig @@ -0,0 +1,27 @@ +menu "Rotary encoders" + + config RE_MAX + int "Maximum number of rotary encoders" + default 1 + + config RE_INTERVAL_US + int "Polling interval, us" + default 1000 + + config RE_BTN_DEAD_TIME_US + int "Button dead time, us" + default 10000 + + choice RE_BTN_PRESSED_LEVEL + prompt "Logical level on pressed button" + config RE_BTN_PRESSED_LEVEL_0 + bool "0" + config RE_BTN_PRESSED_LEVEL_1 + bool "1" + endchoice + + config RE_BTN_LONG_PRESS_TIME_US + int "Long press timeout, us" + default 500000 + +endmenu diff --git a/components/encoder/LICENSE b/components/encoder/LICENSE new file mode 100644 index 0000000..b280b32 --- /dev/null +++ b/components/encoder/LICENSE @@ -0,0 +1,26 @@ +Copyright 2019 Ruslan V. Uss + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of itscontributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/components/encoder/component.mk b/components/encoder/component.mk new file mode 100644 index 0000000..a03513e --- /dev/null +++ b/components/encoder/component.mk @@ -0,0 +1,7 @@ +COMPONENT_ADD_INCLUDEDIRS = . + +ifdef CONFIG_IDF_TARGET_ESP8266 +COMPONENT_DEPENDS = esp8266 freertos log +else +COMPONENT_DEPENDS = driver freertos log +endif diff --git a/components/encoder/encoder.c b/components/encoder/encoder.c new file mode 100644 index 0000000..06b64e8 --- /dev/null +++ b/components/encoder/encoder.c @@ -0,0 +1,250 @@ +/* + * Copyright (c) 2019 Ruslan V. Uss + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of itscontributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @file encoder.c + * + * ESP-IDF HW timer-based driver for rotary encoders + * + * Copyright (c) 2019 Ruslan V. Uss + * + * BSD Licensed as described in the file LICENSE + */ +#include "encoder.h" +#include +#include +#include +#include + +#define MUTEX_TIMEOUT 10 + +#ifdef CONFIG_RE_BTN_PRESSED_LEVEL_0 +#define BTN_PRESSED_LEVEL 0 +#else +#define BTN_PRESSED_LEVEL 1 +#endif + +static const char *TAG = "encoder"; +static rotary_encoder_t *encs[CONFIG_RE_MAX] = { 0 }; +static const int8_t valid_states[] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 }; +static SemaphoreHandle_t mutex; +static QueueHandle_t _queue; + +#define GPIO_BIT(x) ((x) < 32 ? BIT(x) : ((uint64_t)(((uint64_t)1)<<(x)))) +#define CHECK(x) do { esp_err_t __; if ((__ = x) != ESP_OK) return __; } while (0) +#define CHECK_ARG(VAL) do { if (!(VAL)) return ESP_ERR_INVALID_ARG; } while (0) + +inline static void read_encoder(rotary_encoder_t *re) +{ + rotary_encoder_event_t ev = { + .sender = re + }; + + if (re->pin_btn < GPIO_NUM_MAX) + do + { + if (re->btn_state == RE_BTN_PRESSED && re->btn_pressed_time_us < CONFIG_RE_BTN_DEAD_TIME_US) + { + // Dead time + re->btn_pressed_time_us += CONFIG_RE_INTERVAL_US; + break; + } + + // read button state + if (gpio_get_level(re->pin_btn) == BTN_PRESSED_LEVEL) + { + if (re->btn_state == RE_BTN_RELEASED) + { + // first press + re->btn_state = RE_BTN_PRESSED; + re->btn_pressed_time_us = 0; + ev.type = RE_ET_BTN_PRESSED; + xQueueSendToBack(_queue, &ev, 0); + break; + } + + re->btn_pressed_time_us += CONFIG_RE_INTERVAL_US; + + if (re->btn_state == RE_BTN_PRESSED && re->btn_pressed_time_us >= CONFIG_RE_BTN_LONG_PRESS_TIME_US) + { + // Long press + re->btn_state = RE_BTN_LONG_PRESSED; + ev.type = RE_ET_BTN_LONG_PRESSED; + xQueueSendToBack(_queue, &ev, 0); + } + } + else if (re->btn_state != RE_BTN_RELEASED) + { + bool clicked = re->btn_state == RE_BTN_PRESSED; + // released + re->btn_state = RE_BTN_RELEASED; + ev.type = RE_ET_BTN_RELEASED; + xQueueSendToBack(_queue, &ev, 0); + if (clicked) + { + ev.type = RE_ET_BTN_CLICKED; + xQueueSendToBack(_queue, &ev, 0); + } + } + } while(0); + + re->code <<= 2; + re->code |= gpio_get_level(re->pin_a); + re->code |= gpio_get_level(re->pin_b) << 1; + re->code &= 0xf; + + if (!valid_states[re->code]) + return; + + int8_t inc = 0; + + re->store = (re->store << 4) | re->code; + + if (re->store == 0xe817) inc = 1; + if (re->store == 0xd42b) inc = -1; + + if (inc) + { + ev.type = RE_ET_CHANGED; + ev.diff = inc; + xQueueSendToBack(_queue, &ev, 0); + } +} + +static void timer_handler(void *arg) +{ + if (!xSemaphoreTake(mutex, 0)) + return; + + for (size_t i = 0; i < CONFIG_RE_MAX; i++) + if (encs[i]) + read_encoder(encs[i]); + + xSemaphoreGive(mutex); +} + +static const esp_timer_create_args_t timer_args = { + .name = "__encoder__", + .arg = NULL, + .callback = timer_handler, + .dispatch_method = ESP_TIMER_TASK +}; + +static esp_timer_handle_t timer; + +esp_err_t rotary_encoder_init(QueueHandle_t queue) +{ + CHECK_ARG(queue); + _queue = queue; + + mutex = xSemaphoreCreateMutex(); + if (!mutex) + { + ESP_LOGE(TAG, "Failed to create mutex"); + return ESP_ERR_NO_MEM; + } + + CHECK(esp_timer_create(&timer_args, &timer)); + CHECK(esp_timer_start_periodic(timer, CONFIG_RE_INTERVAL_US)); + + ESP_LOGI(TAG, "Initialization complete, timer interval: %dms", CONFIG_RE_INTERVAL_US / 1000); + return ESP_OK; +} + +esp_err_t rotary_encoder_add(rotary_encoder_t *re) +{ + CHECK_ARG(re); + if (!xSemaphoreTake(mutex, MUTEX_TIMEOUT)) + { + ESP_LOGE(TAG, "Failed to take mutex"); + return ESP_ERR_INVALID_STATE; + } + + bool ok = false; + for (size_t i = 0; i < CONFIG_RE_MAX; i++) + if (!encs[i]) + { + re->index = i; + encs[i] = re; + ok = true; + break; + } + if (!ok) + { + ESP_LOGE(TAG, "Too many encoders"); + xSemaphoreGive(mutex); + return ESP_ERR_NO_MEM; + } + + // setup GPIO + gpio_config_t io_conf; + memset(&io_conf, 0, sizeof(gpio_config_t)); + io_conf.mode = GPIO_MODE_INPUT; + if (BTN_PRESSED_LEVEL == 0) { + io_conf.pull_up_en = GPIO_PULLUP_ENABLE; + io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE; + } else { + io_conf.pull_up_en = GPIO_PULLUP_DISABLE; + io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE; + } + io_conf.intr_type = GPIO_INTR_DISABLE; + io_conf.pin_bit_mask = GPIO_BIT(re->pin_a) | GPIO_BIT(re->pin_b); + if (re->pin_btn < GPIO_NUM_MAX) + io_conf.pin_bit_mask |= GPIO_BIT(re->pin_btn); + CHECK(gpio_config(&io_conf)); + + re->btn_state = RE_BTN_RELEASED; + re->btn_pressed_time_us = 0; + + xSemaphoreGive(mutex); + + ESP_LOGI(TAG, "Added rotary encoder %d, A: %d, B: %d, BTN: %d", re->index, re->pin_a, re->pin_b, re->pin_btn); + return ESP_OK; +} + +esp_err_t rotary_encoder_remove(rotary_encoder_t *re) +{ + CHECK_ARG(re); + if (!xSemaphoreTake(mutex, MUTEX_TIMEOUT)) + { + ESP_LOGE(TAG, "Failed to take mutex"); + return ESP_ERR_INVALID_STATE; + } + + for (size_t i = 0; i < CONFIG_RE_MAX; i++) + if (encs[i] == re) + { + encs[i] = NULL; + ESP_LOGI(TAG, "Removed rotary encoder %d", i); + xSemaphoreGive(mutex); + return ESP_OK; + } + + ESP_LOGE(TAG, "Unknown encoder"); + xSemaphoreGive(mutex); + return ESP_ERR_NOT_FOUND; +} diff --git a/components/encoder/encoder.h b/components/encoder/encoder.h new file mode 100644 index 0000000..fc672c8 --- /dev/null +++ b/components/encoder/encoder.h @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2019 Ruslan V. Uss + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of itscontributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @file encoder.h + * @defgroup encoder encoder + * @{ + * + * ESP-IDF HW timer-based driver for rotary encoders + * + * Copyright (c) 2019 Ruslan V. Uss + * + * BSD Licensed as described in the file LICENSE + */ +#ifndef __ENCODER_H__ +#define __ENCODER_H__ + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +/** + * Button state + */ +typedef enum { + RE_BTN_RELEASED = 0, //!< Button currently released + RE_BTN_PRESSED = 1, //!< Button currently pressed + RE_BTN_LONG_PRESSED = 2 //!< Button currently long pressed +} rotary_encoder_btn_state_t; + +/** + * Rotary encoder descriptor + */ +typedef struct +{ + gpio_num_t pin_a, pin_b, pin_btn; //!< Encoder pins. pin_btn can be >= GPIO_NUM_MAX if no button used + uint8_t code; + uint16_t store; + size_t index; + uint64_t btn_pressed_time_us; + rotary_encoder_btn_state_t btn_state; +} rotary_encoder_t; + +/** + * Event type + */ +typedef enum { + RE_ET_CHANGED = 0, //!< Encoder turned + RE_ET_BTN_RELEASED, //!< Button released + RE_ET_BTN_PRESSED, //!< Button pressed + RE_ET_BTN_LONG_PRESSED, //!< Button long pressed (press time (us) > RE_BTN_LONG_PRESS_TIME_US) + RE_ET_BTN_CLICKED //!< Button was clicked +} rotary_encoder_event_type_t; + +/** + * Event + */ +typedef struct +{ + rotary_encoder_event_type_t type; //!< Event type + rotary_encoder_t *sender; //!< Pointer to descriptor + int32_t diff; //!< Difference between new and old positions (only if type == RE_ET_CHANGED) +} rotary_encoder_event_t; + +/** + * @brief Initialize library + * + * @param queue Event queue to send encoder events + * @return `ESP_OK` on success + */ +esp_err_t rotary_encoder_init(QueueHandle_t queue); + +/** + * @brief Add new rotary encoder + * + * @param re Encoder descriptor + * @return `ESP_OK` on success + */ +esp_err_t rotary_encoder_add(rotary_encoder_t *re); + +/** + * @brief Remove previously added rotary encoder + * + * @param re Encoder descriptor + * @return `ESP_OK` on success + */ +esp_err_t rotary_encoder_remove(rotary_encoder_t *re); + +#ifdef __cplusplus +} +#endif + +/**@}*/ + +#endif /* __ENCODER_H__ */ diff --git a/components/ssd1306/ssd1306.c b/components/ssd1306/ssd1306.c index f5d97f3..5b2a28c 100644 --- a/components/ssd1306/ssd1306.c +++ b/components/ssd1306/ssd1306.c @@ -17,8 +17,10 @@ typedef union out_column_t { uint8_t u8[4]; } PACK8 out_column_t; -void ssd1306_init(SSD1306_t * dev, int width, int height) +//void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX) //original +void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX) { + dev->_offsetX = offsetX; if (dev->_address == SPIAddress) { spi_init(dev, width, height); } else { diff --git a/components/ssd1306/ssd1306.h b/components/ssd1306/ssd1306.h index 120315d..893694b 100644 --- a/components/ssd1306/ssd1306.h +++ b/components/ssd1306/ssd1306.h @@ -98,6 +98,7 @@ typedef struct { int _scDirection; PAGE_t _page[8]; bool _flip; + int _offsetX; //added offset here instead of using macro variable } SSD1306_t; #ifdef __cplusplus @@ -105,7 +106,7 @@ extern "C" { #endif -void ssd1306_init(SSD1306_t * dev, int width, int height); +void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX); int ssd1306_get_width(SSD1306_t * dev); int ssd1306_get_height(SSD1306_t * dev); int ssd1306_get_pages(SSD1306_t * dev); @@ -128,6 +129,7 @@ void _ssd1306_pixel(SSD1306_t * dev, int xpos, int ypos, bool invert); void _ssd1306_line(SSD1306_t * dev, int x1, int y1, int x2, int y2, bool invert); void ssd1306_invert(uint8_t *buf, size_t blen); void ssd1306_flip(uint8_t *buf, size_t blen); +void ssd1306_setOffset(SSD1306_t * dev, int offset); uint8_t ssd1306_copy_bit(uint8_t src, int srcBits, uint8_t dst, int dstBits); uint8_t ssd1306_rotate_byte(uint8_t ch1); void ssd1306_fadeout(SSD1306_t * dev); diff --git a/components/ssd1306/ssd1306_i2c.c b/components/ssd1306/ssd1306_i2c.c index fe2d821..cb092f3 100644 --- a/components/ssd1306/ssd1306_i2c.c +++ b/components/ssd1306/ssd1306_i2c.c @@ -112,7 +112,8 @@ void i2c_display_image(SSD1306_t * dev, int page, int seg, uint8_t * images, int if (page >= dev->_pages) return; if (seg >= dev->_width) return; - int _seg = seg + CONFIG_OFFSETX; + //int _seg = seg + CONFIG_OFFSETX; //original + int _seg = seg + dev->_offsetX; uint8_t columLow = _seg & 0x0F; uint8_t columHigh = (_seg >> 4) & 0x0F; diff --git a/components/ssd1306/ssd1306_spi.c b/components/ssd1306/ssd1306_spi.c index fce2b7f..e6cc5ee 100644 --- a/components/ssd1306/ssd1306_spi.c +++ b/components/ssd1306/ssd1306_spi.c @@ -158,7 +158,8 @@ void spi_display_image(SSD1306_t * dev, int page, int seg, uint8_t * images, int if (page >= dev->_pages) return; if (seg >= dev->_width) return; - int _seg = seg + CONFIG_OFFSETX; + //int _seg = seg + CONFIG_OFFSETX; //original + int _seg = seg + dev->_offsetX; uint8_t columLow = _seg & 0x0F; uint8_t columHigh = (_seg >> 4) & 0x0F; diff --git a/connection-plan.drawio.pdf b/connection-plan.drawio.pdf index 8264a57..4fe79a7 100644 Binary files a/connection-plan.drawio.pdf and b/connection-plan.drawio.pdf differ diff --git a/doc/2023.09.09_armchair-frame.jpg b/doc/2023.09.09_armchair-frame.jpg new file mode 100644 index 0000000..97bb7cc Binary files /dev/null and b/doc/2023.09.09_armchair-frame.jpg differ diff --git a/doc/MLX90333-Datasheet_IC-Stick-small.PDF b/doc/MLX90333-Datasheet_IC-Stick-small.PDF new file mode 100644 index 0000000..f8968f1 Binary files /dev/null and b/doc/MLX90333-Datasheet_IC-Stick-small.PDF differ diff --git a/doc/MLX91204-Datasheet_IC-Stick-large.pdf b/doc/MLX91204-Datasheet_IC-Stick-large.pdf new file mode 100644 index 0000000..1333194 Binary files /dev/null and b/doc/MLX91204-Datasheet_IC-Stick-large.pdf differ diff --git a/doc/schematic_custom-pcb.pdf b/doc/schematic_custom-pcb.pdf new file mode 100644 index 0000000..398c4d5 Binary files /dev/null and b/doc/schematic_custom-pcb.pdf differ diff --git a/function-diagram.drawio.pdf b/function-diagram.drawio.pdf index 9d55d53..86fb8ab 100644 Binary files a/function-diagram.drawio.pdf and b/function-diagram.drawio.pdf differ