j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'dev' - brake, mode-sel menu, massage, status-screen scroll

Browse Source 
Armchair works reliably with the mentioned new features and fixed
massage mode
This commit is contained in:
jonny_l480 2024-06-03 15:19:33 +02:00
commit f755d3775b
62 changed files with 5216 additions and 1478 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored
View File

@ -10,9 +10,15 @@ dependencies.lock
**/.cache **/.cache
# VS-code
settings.json
# drawio # drawio
*.dtmp *.dtmp
*.bkp *.bkp
# diagrams are mostly temporary (pdf files are tracked)
*.drawio
# React # React

174
README.md
View File

@ -1,7 +1,61 @@
# Overview
Firmware for a homemade automated electric armchair. Firmware for a homemade automated electric armchair.
More details about this project: Extensive details about this project can be found on the website:
V1: https://pfusch.zone/electric-armchair - ~~V1: [Electric Armchair V1](https://pfusch.zone/electric-armchair)~~
V2: https://pfusch.zone/electric-armchair-v2 - 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.
<img src="doc/2023.09.09_armchair-frame.jpg" alt="Photo machine" style="width:60%;">
*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 git clone git@github.com:Jonny999999/armchair_fw
``` ```
### Instal node packages ### 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 cd react-app
npm install npm install
@ -28,11 +82,12 @@ npm install
# Building the Project # Building the Project
## react-webapp ## React-webapp
For the webapp to work on the esp32 it has to be built. When flashing to the ESP32, the files in the `react-app/build/` folder are written to a SPIFFS partition.
When flashing, the folder react-app/build is flashed to siffs (which is used as webroot) onto the esp32. These files are then served via HTTP in the Wi-Fi network "armchair" created by the ESP32.
The following command builds the react webapp and creates this folder In HTTP control mode, you can control the armchair using a joystick on the provided website.
TODO: add this to flash target with cmake?
Initially, or when changing the React code, you need to manually build the React app:
```bash ```bash
cd react-app cd react-app
#compile #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 Note: Use `npm start` for starting the webapp locally for testing
## esp project
## Firmware
### Set up environment ### Set up environment
```bash ```bash
source /opt/esp-idf/export.sh source /opt/esp-idf/export.sh
@ -65,84 +121,52 @@ idf.py flash
``` ```
- once "connecting...' was successfully, BOOT button can be released - once "connecting...' was successfully, BOOT button can be released
### Monitor ### Monitor
- connect FTDI programmer to board (VCC to VCC; TX to RX; RX to TX) To view log output for debugging, follow the same steps as in the Upload section, but run:
- press REST and BOOT button
- release RESET button (keep pressing boot)
- run monitor command:
```bash ```bash
idf.py monitor 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 ## Encoder Functions
- More sensors:
- Accelerometer
- Lidar sensor
- GPS receiver
- Anti slip regulation
- Self driving algorithm
- Lights
- Improved webinterface
- App
**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 **When in MENU_SETTINGS mode** (5x click), the encoder controls the settings menu: (similar in MENU_MODE_SELECT)
**Add switch functions**
- set loglevel
- define max-speed
| 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 **Usage:**
## Switch functions - Switch to HTTP mode (4 button presses).
**Currently implemented** - Connect to WiFi `armchar`, no password.
| Count | Type | Action | Description | - Access http://192.168.4.1 (note: **http** NOT https, some browsers automatically add https!).
| --- | --- | --- | --- |
| 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

1
board_control/main/CMakeLists.txt
View File

@ -6,6 +6,7 @@ idf_component_register(
"button.cpp" "button.cpp"
"auto.cpp" "auto.cpp"
"uart.cpp" "uart.cpp"
"encoder.cpp"
INCLUDE_DIRS INCLUDE_DIRS
"." "."
) )

81
board_control/main/encoder.cpp Normal file
View File

@ -0,0 +1,81 @@
#include "encoder.h"
extern "C"
{
#include <stdio.h>
#include <esp_system.h>
#include <esp_event.h>
#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;
}
}
}
}

18
board_control/main/encoder.hpp Normal file
View File

@ -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);

29
board_control/main/main.cpp
View File

@ -18,6 +18,7 @@ extern "C"
#include "uart.hpp" #include "uart.hpp"
#include "encoder.hpp"
//========================= //=========================
@ -28,6 +29,13 @@ extern "C"
//#define UART_TEST_ONLY //#define UART_TEST_ONLY
//=========================
//====== encoder TEST =====
//=========================
//only start encoder task
#define ENCODER_TEST_ONLY
//tag for logging //tag for logging
static const char * TAG = "main"; static const char * TAG = "main";
@ -157,7 +165,7 @@ void setLoglevels(void){
//=========== app_main ============ //=========== app_main ============
//================================= //=================================
extern "C" void app_main(void) { extern "C" void app_main(void) {
#ifndef UART_TEST_ONLY #if !defined(ENCODER_TEST_ONLY) && !defined(UART_TEST_ONLY)
//enable 5V volate regulator //enable 5V volate regulator
gpio_pad_select_gpio(GPIO_NUM_17); gpio_pad_select_gpio(GPIO_NUM_17);
gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT);
@ -214,24 +222,35 @@ extern "C" void app_main(void) {
// vTaskDelay(2000 / portTICK_PERIOD_MS); // vTaskDelay(2000 / portTICK_PERIOD_MS);
// ESP_LOGI(TAG, "initializing http server"); // ESP_LOGI(TAG, "initializing http server");
// http_init_server(); // http_init_server();
#endif #endif
//------------------------------------------- //-------------------------------------------
//--- create tasks for uart communication --- //--- create tasks for uart communication ---
//------------------------------------------- //-------------------------------------------
#ifndef ENCODER_TEST_ONLY
uart_init(); uart_init();
xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL); xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL);
xTaskCreate(task_uartSend, "task_uartSend", 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 --- //--- main loop ---
//does nothing except for testing things //does nothing except for testing things
//--- testing force http mode after startup --- //--- testing force http mode after startup ---
vTaskDelay(5000 / portTICK_PERIOD_MS); vTaskDelay(5000 / portTICK_PERIOD_MS);
control.changeMode(controlMode_t::HTTP); //control.changeMode(controlMode_t::HTTP);
while(1){ while(1){
vTaskDelay(1000 / portTICK_PERIOD_MS); vTaskDelay(1000 / portTICK_PERIOD_MS);
//--------------------------------- //---------------------------------

3
board_single/CMakeLists.txt
View File

@ -7,3 +7,6 @@ cmake_minimum_required(VERSION 3.5)
include($ENV{IDF_PATH}/tools/cmake/project.cmake) include($ENV{IDF_PATH}/tools/cmake/project.cmake)
set(EXTRA_COMPONENT_DIRS "../components ../common") set(EXTRA_COMPONENT_DIRS "../components ../common")
project(armchair-singleBoard) project(armchair-singleBoard)
# colored build output (errors, warnings...)
idf_build_set_property(COMPILE_OPTIONS "-fdiagnostics-color=always" APPEND)

3
board_single/main/CMakeLists.txt
View File

@ -1,12 +1,13 @@
idf_component_register( idf_component_register(
SRCS SRCS
"main.cpp" "main.cpp"
"config.cpp"
"control.cpp" "control.cpp"
"button.cpp" "button.cpp"
"fan.cpp" "fan.cpp"
"auto.cpp" "auto.cpp"
"display.cpp" "display.cpp"
"menu.cpp"
"encoder.cpp"
INCLUDE_DIRS INCLUDE_DIRS
"." "."
) )

26
board_single/main/auto.cpp
View File

@ -1,5 +1,4 @@
#include "auto.hpp" #include "auto.hpp"
#include "config.hpp"
//tag for logging //tag for logging
static const char * TAG = "automatedArmchair"; static const char * TAG = "automatedArmchair";
@ -8,9 +7,12 @@ static const char * TAG = "automatedArmchair";
//============================= //=============================
//======== constructor ======== //======== constructor ========
//============================= //=============================
automatedArmchair::automatedArmchair(void) { automatedArmchair_c::automatedArmchair_c(controlledMotor *motorLeft_f, controlledMotor *motorRight_f)
//create command queue {
commandQueue = xQueueCreate( 32, sizeof( commandSimple_t ) ); //TODO add max size to config? 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 ====== //====== generateCommands ======
//============================== //==============================
motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruction) { motorCommands_t automatedArmchair_c::generateCommands(auto_instruction_t * instruction) {
//reset instruction //reset instruction
*instruction = auto_instruction_t::NONE; *instruction = auto_instruction_t::NONE;
//check if previous command is finished //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 //copy instruction to be provided to control task
*instruction = cmdCurrent.instruction; *instruction = cmdCurrent.instruction;
//set acceleration / fading parameters according to command //set acceleration / fading parameters according to command
motorLeft.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); motorLeft->setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel);
motorRight.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); motorRight->setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel);
motorLeft.setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); motorLeft->setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel);
motorRight.setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel); motorRight->setFade(fadeType_t::ACCEL, cmdCurrent.fadeAccel);
//calculate timestamp the command is finished //calculate timestamp the command is finished
timestampCmdFinished = esp_log_timestamp() + cmdCurrent.msDuration; timestampCmdFinished = esp_log_timestamp() + cmdCurrent.msDuration;
//copy the new commands //copy the new commands
@ -55,7 +57,7 @@ motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruc
//======== addCommand ======== //======== addCommand ========
//============================ //============================
//function that adds a basic command to the queue //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 //add command to queue
if ( xQueueSend( commandQueue, ( void * )&command, ( TickType_t ) 0 ) ){ if ( xQueueSend( commandQueue, ( void * )&command, ( TickType_t ) 0 ) ){
ESP_LOGI(TAG, "Successfully inserted command to queue"); 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++) { for (int i = 0; i < count; i++) {
ESP_LOGI(TAG, "Reading command no. %d from provided array", i); ESP_LOGI(TAG, "Reading command no. %d from provided array", i);
addCommand(commands[i]); addCommand(commands[i]);
@ -77,7 +79,7 @@ void automatedArmchair::addCommands(commandSimple_t commands[], size_t count) {
//=============================== //===============================
//function that deletes all pending/queued commands //function that deletes all pending/queued commands
//e.g. when switching modes //e.g. when switching modes
motorCommands_t automatedArmchair::clearCommands() { motorCommands_t automatedArmchair_c::clearCommands() {
//clear command queue //clear command queue
xQueueReset( commandQueue ); xQueueReset( commandQueue );
ESP_LOGW(TAG, "command queue was successfully emptied"); ESP_LOGW(TAG, "command queue was successfully emptied");

55
board_single/main/auto.hpp
View File

@ -33,13 +33,13 @@ typedef struct commandSimple_t{
//------------------------------------ //------------------------------------
//----- automatedArmchair class ----- //----- automatedArmchair_c class -----
//------------------------------------ //------------------------------------
class automatedArmchair { class automatedArmchair_c {
public: public:
//--- methods --- //--- methods ---
//constructor //constructor
automatedArmchair(void); automatedArmchair_c(controlledMotor * motorLeft, controlledMotor * motorRight);
//function to generate motor commands //function to generate motor commands
//can be also seen as handle function //can be also seen as handle function
//TODO: go with other approach: separate task for handling auto mode //TODO: go with other approach: separate task for handling auto mode
@ -62,6 +62,8 @@ class automatedArmchair {
private: private:
//--- methods --- //--- methods ---
//--- objects --- //--- objects ---
controlledMotor * motorLeft;
controlledMotor * motorRight;
//TODO: add buzzer here //TODO: add buzzer here
//--- variables --- //--- variables ---
//queue for storing pending commands //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;
}
*/

322
board_single/main/button.cpp
View File

@ -8,139 +8,145 @@ extern "C"
} }
#include "button.hpp" #include "button.hpp"
#include "encoder.hpp"
#include "display.hpp"
// tag for logging
static const char *TAG = "button";
//======================================
//tag for logging //============ button task =============
static const char * TAG = "button"; //======================================
// 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 -------- //-------- constructor --------
//----------------------------- //-----------------------------
buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, evaluatedJoystick * joystick_f, controlledArmchair * control_f, buzzer_t * buzzer_f, controlledMotor * motorLeft_f, controlledMotor * motorRight_f){ buttonCommands::buttonCommands(
//copy object pointers controlledArmchair *control_f,
button = button_f; evaluatedJoystick *joystick_f,
joystick = joystick_f; QueueHandle_t encoderQueue_f,
controlledMotor *motorLeft_f,
controlledMotor *motorRight_f,
buzzer_t *buzzer_f)
{
// copy object pointers
control = control_f; control = control_f;
buzzer = buzzer_f; joystick = joystick_f;
encoderQueue = encoderQueue_f;
motorLeft = motorLeft_f; motorLeft = motorLeft_f;
motorRight = motorRight_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 ----------- //--------- action -----------
//---------------------------- //----------------------------
//function that runs commands depending on a count value //function that runs commands depending on a count value
void buttonCommands::action (uint8_t count, bool lastPressLong){ void buttonCommands::action (uint8_t count, bool lastPressLong){
//--- variable declarations --- //--- variables ---
bool decelEnabled; //for different beeping when toggling 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 --- //--- get joystick position ---
//in case joystick is used for additional cases:
//joystickData_t stickData = joystick->getData(); //joystickData_t stickData = joystick->getData();
//--- actions based on count --- //--- run actions based on count ---
switch (count){ switch (count)
//no such command {
default: // ## no command ##
ESP_LOGE(TAG, "no command for count=%d defined", count); default:
buzzer->beep(3, 400, 100); ESP_LOGE(TAG, "no command for count=%d and long=%d defined", count, lastPressLong);
break; buzzer->beep(3, 200, 100);
break;
case 1: case 1:
//restart contoller when 1x long pressed // ## switch to MENU_SETTINGS state ##
if (lastPressLong){ if (lastPressLong)
ESP_LOGW(TAG, "RESTART"); {
buzzer->beep(1,1000,1); ESP_LOGW(TAG, "1x long press -> clear encoder queue and change to mode 'menu mode select'");
vTaskDelay(500 / portTICK_PERIOD_MS); buzzer->beep(5, 50, 30);
//esp_restart(); // clear encoder event queue (prevent menu from exiting immediately due to long press event just happend)
//-> define joystick center or toggle freeze input (executed in control task) vTaskDelay(200 / portTICK_PERIOD_MS);
control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed //TODO move encoder queue clear to changeMode() method?
return; rotary_encoder_event_t ev;
} while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS);
//note: disabled joystick calibration due to accidential trigger control->changeMode(controlMode_t::MENU_MODE_SELECT);
// }
// ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); // ## toggle joystick freeze ##
// //-> define joystick center or toggle freeze input (executed in control task) else if (control->getCurrentMode() == controlMode_t::MASSAGE)
// control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed {
break; control->toggleFreezeInputMassage();
case 2: }
//run automatic commands to lift leg support when pressed 1x short 1x long // ## define joystick center ##
if (lastPressLong){ else
//define commands {
cmds[0] = // note: disabled joystick calibration due to accidential trigger
{ //joystick->defineCenter();
.motorCmds = { }
.left = {motorstate_t::REV, 90}, break;
.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 command queue case 2:
armchair.addCommands(cmds, 3); // ## switch to ADJUST_CHAIR mode ##
if (lastPressLong)
//change mode to AUTO {
control->changeMode(controlMode_t::AUTO); ESP_LOGW(TAG, "cmd %d: switch to ADJUST_CHAIR", count);
return; control->changeMode(controlMode_t::ADJUST_CHAIR);
} }
// ## toggle IDLE ##
//toggle idle when 2x pressed else
{
ESP_LOGW(TAG, "cmd %d: toggle IDLE", count); ESP_LOGW(TAG, "cmd %d: toggle IDLE", count);
control->toggleIdle(); //toggle between idle and previous/default mode control->toggleIdle(); // toggle between idle and previous/default mode
break; }
break;
case 3: case 3:
// ## switch to JOYSTICK mode ##
ESP_LOGW(TAG, "cmd %d: switch to JOYSTICK", count); ESP_LOGW(TAG, "cmd %d: switch to JOYSTICK", count);
control->changeMode(controlMode_t::JOYSTICK); //switch to JOYSTICK mode control->changeMode(controlMode_t::JOYSTICK); //switch to JOYSTICK mode
break; break;
case 4: case 4:
ESP_LOGW(TAG, "cmd %d: toggle between HTTP and JOYSTICK", count); // ## switch to HTTP mode ##
control->toggleModes(controlMode_t::HTTP, controlMode_t::JOYSTICK); //toggle between HTTP and JOYSTICK 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; break;
case 6: case 6:
ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count); // ## switch to MASSAGE mode ##
control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode ESP_LOGW(TAG, "switch to MASSAGE");
control->changeMode(controlMode_t::MASSAGE); //switch to MASSAGE mode
break; break;
case 8: case 8:
// ## toggle "sport-mode" ##
//toggle deceleration fading between on and off //toggle deceleration fading between on and off
//decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL); //decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL);
//motorRight->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); motorRight->toggleFade(fadeType_t::ACCEL);
ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled); ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled);
if (decelEnabled){ if (decelEnabled){
@ -151,12 +157,10 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
break; break;
case 12: case 12:
ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); // ## toggle alternative stick mapping ##
//-> toggle altStickMapping (executed in control task) control->toggleAltStickMapping();
control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1)?
break; break;
}
}
} }
@ -165,56 +169,78 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
//----------------------------- //-----------------------------
//------ startHandleLoop ------ //------ startHandleLoop ------
//----------------------------- //-----------------------------
//this function has to be started once in a separate task // when not in MENU_SETTINGS mode, repeatedly receives events from encoder button
//repeatedly evaluates and processes button events then takes the corresponding action // and takes the corresponding action
void buttonCommands::startHandleLoop() { // 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) { while (1)
vTaskDelay(20 / portTICK_PERIOD_MS); {
//run handle function of evaluatedSwitch object //-- disable functionality when in menu mode --
button->handle(); //(display task uses encoder in that mode)
if (control->getCurrentMode() == controlMode_t::MENU_SETTINGS
//--- count button presses and run action --- || control->getCurrentMode() == controlMode_t::MENU_MODE_SELECT)
switch(state) { {
case inputState_t::IDLE: //wait for initial button press //do nothing every loop cycle
if (button->risingEdge) { ESP_LOGD(TAG, "in MENU_SETTINGS or MENU_MODE_SELECT mode -> button commands disabled");
count = 1; vTaskDelay(1000 / portTICK_PERIOD_MS);
buzzer->beep(1, 65, 0); continue;
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;
} }
}
}
//-- 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

40
board_single/main/button.hpp
View File

@ -5,7 +5,6 @@
#include "control.hpp" #include "control.hpp"
#include "motorctl.hpp" #include "motorctl.hpp"
#include "auto.hpp" #include "auto.hpp"
#include "config.hpp"
#include "joystick.hpp" #include "joystick.hpp"
@ -17,14 +16,13 @@
class buttonCommands { class buttonCommands {
public: public:
//--- constructor --- //--- constructor ---
buttonCommands ( buttonCommands(
gpio_evaluatedSwitch * button_f, controlledArmchair *control_f,
evaluatedJoystick * joystick_f, evaluatedJoystick *joystick_f,
controlledArmchair * control_f, QueueHandle_t encoderQueue_f,
buzzer_t * buzzer_f, controlledMotor * motorLeft_f,
controlledMotor * motorLeft_f, controlledMotor *motorRight_f,
controlledMotor * motorRight_f buzzer_t *buzzer_f);
);
//--- functions --- //--- functions ---
//the following function has to be started once in a separate task. //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); void action(uint8_t count, bool lastPressLong);
//--- objects --- //--- objects ---
gpio_evaluatedSwitch* button;
evaluatedJoystick* joystick;
controlledArmchair * control; controlledArmchair * control;
buzzer_t* buzzer; evaluatedJoystick* joystick;
controlledMotor * motorLeft; controlledMotor * motorLeft;
controlledMotor * motorRight; controlledMotor * motorRight;
buzzer_t* buzzer;
QueueHandle_t encoderQueue;
//--- variables --- //--- variables ---
uint8_t count = 0; 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 );

334
board_single/main/config.cpp
View File

@ -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:
//=================================== extern "C"
//======= motor configuration ======= {
//=================================== #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 /* ==> currently using other driver
//--- configure left motor (hardware) --- //--- configure left motor (hardware) ---
single100a_config_t configDriverLeft = { single100a_config_t configDriverLeft = {
@ -11,8 +69,8 @@ single100a_config_t configDriverLeft = {
.gpio_b = GPIO_NUM_4, .gpio_b = GPIO_NUM_4,
.ledc_timer = LEDC_TIMER_0, .ledc_timer = LEDC_TIMER_0,
.ledc_channel = LEDC_CHANNEL_0, .ledc_channel = LEDC_CHANNEL_0,
.aEnabledPinState = false, //-> pins inverted (mosfets) .aEnabledPinState = false, //-> pins inverted (mosfets)
.bEnabledPinState = false, .bEnabledPinState = false,
.resolution = LEDC_TIMER_11_BIT, .resolution = LEDC_TIMER_11_BIT,
.pwmFreq = 10000 .pwmFreq = 10000
}; };
@ -24,176 +82,192 @@ single100a_config_t configDriverRight = {
.gpio_b = GPIO_NUM_14, .gpio_b = GPIO_NUM_14,
.ledc_timer = LEDC_TIMER_1, .ledc_timer = LEDC_TIMER_1,
.ledc_channel = LEDC_CHANNEL_1, .ledc_channel = LEDC_CHANNEL_1,
.aEnabledPinState = false, //-> pin inverted (mosfet) .aEnabledPinState = false, //-> pin inverted (mosfet)
.bEnabledPinState = true, //-> not inverted (direct) .bEnabledPinState = true, //-> not inverted (direct)
.resolution = LEDC_TIMER_11_BIT, .resolution = LEDC_TIMER_11_BIT,
.pwmFreq = 10000 .pwmFreq = 10000
}; };
*/ */
//--- configure sabertooth driver --- (controls both motors in one instance) //--- configure sabertooth driver --- (controls both motors in one instance)
sabertooth2x60_config_t sabertoothConfig = { sabertooth2x60_config_t sabertoothConfig = {
.gpio_TX = GPIO_NUM_25, .gpio_TX = GPIO_NUM_27,
.uart_num = UART_NUM_2 .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) --- //--- configure left motor (contol) ---
motorctl_config_t configMotorControlLeft = { motorctl_config_t configMotorControlLeft = {
.msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%) .name = "left",
.msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%) .loggingEnabled = true,
.currentLimitEnabled = false, .msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%)
.currentSensor_adc = ADC1_CHANNEL_4, //GPIO32 .msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%)
.currentSensor_ratedCurrent = 50, .currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
.currentSensor_ratedCurrent = 50,
.currentMax = 30, .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) --- //--- configure right motor (contol) ---
motorctl_config_t configMotorControlRight = { motorctl_config_t configMotorControlRight = {
.msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%) .name = "right",
.msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%) .loggingEnabled = false,
.currentLimitEnabled = false, .msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%)
.currentSensor_adc = ADC1_CHANNEL_5, //GPIO33 .msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%)
.currentSensor_ratedCurrent = 50, .currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
.currentSensor_ratedCurrent = 50,
.currentMax = 30, .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 = { control_config_t configControl = {
.defaultMode = controlMode_t::JOYSTICK, //default mode after startup and toggling IDLE .defaultMode = controlMode_t::JOYSTICK, // default mode after startup and toggling IDLE
//--- timeout --- //--- timeouts ---
.timeoutMs = 3*60*1000, //time of inactivity after which the mode gets switched to IDLE .timeoutSwitchToIdleMs = 5 * 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 .timeoutNotifyPowerStillOnMs = 6 * 60 * 60 * 1000 // time in IDLE after which buzzer beeps in certain interval (notify "forgot to turn off")
//--- http mode ---
}; };
//-------------------------------
//----- httpJoystick config -----
//=============================== //-------------------------------
//===== httpJoystick config =====
//===============================
httpJoystick_config_t configHttpJoystickMain{ 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, .toleranceZeroY_Per = 6,
.toleranceEndPer = 2, //percentage before joystick end the coordinate snaps to 1/-1 .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 .timeoutMs = 2500 // time no new data was received before the motors get turned off
}; };
//--------------------------------------
//------- joystick configuration -------
//====================================== //--------------------------------------
//======= joystick configuration =======
//======================================
joystick_config_t configJoystick = { joystick_config_t configJoystick = {
.adc_x = ADC1_CHANNEL_0, //GPIO36 .adc_x = ADC1_CHANNEL_0, // GPIO36
.adc_y = ADC1_CHANNEL_3, //GPIO39 .adc_y = ADC1_CHANNEL_3, // GPIO39
//percentage of joystick range the coordinate of the axis snaps to 0 (0-100) // percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
.tolerance_zeroX_per = 7, //6 .tolerance_zeroX_per = 7, // 6
.tolerance_zeroY_per = 10, //7 .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) // 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, .tolerance_end_per = 4,
//threshold the radius jumps to 1 before the stick is at max radius (range 0-1) // threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
.tolerance_radius = 0.09, .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_min = 1710, //=> x=-1
.x_max = 2980, //=> x=1 .x_max = 2980, //=> x=1
.y_min = 1700, //=> y=-1 .y_min = 1700, //=> y=-1
.y_max = 2940, //=> y=1 .y_max = 2940, //=> y=1
//invert adc measurement // invert adc measurement
.x_inverted = true, .x_inverted = false,
.y_inverted = true .y_inverted = true};
};
//----------------------------
//--- configure fan contol ---
//============================ //----------------------------
//=== configure fan contol === fan_config_t configFans = {
//============================
fan_config_t configCooling = {
.gpio_fan = GPIO_NUM_13, .gpio_fan = GPIO_NUM_13,
.dutyThreshold = 40, .dutyThreshold = 50,
.minOnMs = 1500, .minOnMs = 3500, // time motor duty has to be above the threshold for fans to turn on
.minOffMs = 3000, .minOffMs = 5000, // min time fans have to be off to be able to turn on again
.turnOffDelayMs = 5000, .turnOffDelayMs = 3000, // time fans continue to be on after duty is below threshold
}; };
//--------------------------------------------
//-------- speed sensor configuration --------
//============================================ //--------------------------------------------
//======== speed sensor configuration ========
//============================================
speedSensor_config_t speedLeft_config{ speedSensor_config_t speedLeft_config{
.gpioPin = GPIO_NUM_5, .gpioPin = GPIO_NUM_5,
.degreePerGroup = 360/5, .degreePerGroup = 360 / 16,
.tireCircumferenceMeter = 210.0*3.141/1000.0, .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
.directionInverted = false, //measured wihth scope while tires in the air:
.logName = "speedLeft", // 5-groups: 12ms
// 16-groups: 3.7ms
.tireCircumferenceMeter = 0.81,
.directionInverted = true,
.logName = "speedLeft"
}; };
speedSensor_config_t speedRight_config{ speedSensor_config_t speedRight_config{
.gpioPin = GPIO_NUM_14, .gpioPin = GPIO_NUM_14,
.degreePerGroup = 360/12, .degreePerGroup = 360 / 12,
.tireCircumferenceMeter = 210.0*3.141/1000.0, .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
.directionInverted = true, .tireCircumferenceMeter = 0.81,
.logName = "speedRight", .directionInverted = false,
.logName = "speedRight"
}; };
//================================= //-------------------------
//===== create global objects ===== //-------- display --------
//================================= //-------------------------
//TODO outsource global variables to e.g. global.cpp and only config options here? display_config_t display_config{
//create sabertooth motor driver instance // hardware initialization
sabertooth2x60a sabertoothDriver(sabertoothConfig); .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 //-------- encoder --------
//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) { //configure rotary encoder (next to joystick)
sabertoothDriver.setLeft(cmd); 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
};

6
board_single/main/config.h Normal file
View File

@ -0,0 +1,6 @@
#pragma once
// outsourced macros / definitions
//-- control.cpp --
//#define JOYSTICK_LOG_IN_IDLE

51
board_single/main/config.hpp
View File

@ -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;

818
board_single/main/control.cpp
View File

@ -9,43 +9,98 @@ extern "C"
#include "wifi.h" #include "wifi.h"
} }
#include "config.hpp" #include "config.h"
#include "control.hpp" #include "control.hpp"
#include "chairAdjust.hpp"
#include "display.hpp" // needed for getBatteryPercent()
//used definitions moved from config.hpp: //used definitions moved from config.h:
//#define JOYSTICK_TEST //#define JOYSTICK_LOG_IN_IDLE
//tag for logging //tag for logging
static const char * TAG = "control"; 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 -------- //-------- constructor --------
//----------------------------- //-----------------------------
controlledArmchair::controlledArmchair ( controlledArmchair::controlledArmchair(
control_config_t config_f, control_config_t config_f,
buzzer_t * buzzer_f, buzzer_t *buzzer_f,
controlledMotor* motorLeft_f, controlledMotor *motorLeft_f,
controlledMotor* motorRight_f, controlledMotor *motorRight_f,
evaluatedJoystick* joystick_f, evaluatedJoystick *joystick_f,
httpJoystick* httpJoystick_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 //copy configuration
config = config_f; config = config_f;
joystickGenerateCommands_config = *joystickGenerateCommands_config_f;
//copy object pointers //copy object pointers
buzzer = buzzer_f; buzzer = buzzer_f;
motorLeft = motorLeft_f; motorLeft = motorLeft_f;
motorRight = motorRight_f; motorRight = motorRight_f;
joystick_l = joystick_f, joystick_l = joystick_f,
httpJoystickMain_l = httpJoystick_f; httpJoystickMain_l = httpJoystick_f;
automatedArmchair = automatedArmchair_f;
legRest = legRest_f;
backRest = backRest_f;
nvsHandle = nvsHandle_f;
//set default mode from config //set default mode from config
modePrevious = config.defaultMode; 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 ----------- //---------- Handle loop -----------
//---------------------------------- //----------------------------------
//function that repeatedly generates motor commands depending on the current mode // start endless loop that repeatedly calls handle() and handleTimeout() methods
//also handles fading and current-limit void controlledArmchair::startHandleLoop()
void controlledArmchair::startHandleLoop() { {
while (1){ while (1)
ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]); {
// 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) { xSemaphoreGive(handleIteration_mutex);
default: } // end mutex
mode = controlMode_t::IDLE; else {
break; ESP_LOGE(TAG, "mutex timeout - stuck in changeMode? -> RESTART");
esp_restart();
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
} }
//--- slow loop ---
//--- run actions based on received button button event --- // this section is run approx every 5s (+500ms)
//note: buttonCount received by sendButtonEvent method called from button.cpp if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
//TODO: what if variable gets set from other task during this code? -> mutex around this code {
switch (buttonCount) { ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
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);
timestamp_SlowLoopLastRun = esp_log_timestamp(); timestamp_SlowLoopLastRun = esp_log_timestamp();
//--- handle timeouts ---
//run function which detects timeout (switch to idle) // run function that detects timeouts (switch to idle, or notify "forgot to turn off")
handleTimeout(); 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(){ void controlledArmchair::resetTimeout(){
//TODO mutex //TODO mutex
timestamp_lastActivity = esp_log_timestamp(); 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 ----------- //---------- handleTimeout -----------
//------------------------------------ //------------------------------------
//percentage the duty can vary since last timeout check and still counts as incative // switch to IDLE when no activity (prevent accidential movement)
//TODO: add this to config // notify "power still on" when in IDLE for a very long time (prevent battery drain when forgotten to turn off)
float inactivityTolerance = 10; // 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 // -- timeout switch to IDLE --
bool validateActivity(float dutyOld, float dutyNow, float tolerance){ // timeout to IDLE when not idling already
float dutyDelta = dutyNow - dutyOld; if (mode != controlMode_t::IDLE && noActivityDurationMs > config.timeoutSwitchToIdleMs)
if (fabs(dutyDelta) < tolerance) { {
return false; //no significant activity detected ESP_LOGW(TAG, "timeout check: [TIMEOUT], no activity for more than %ds -> switch to IDLE", config.timeoutSwitchToIdleMs / 1000);
} else { changeMode(controlMode_t::IDLE);
return true; //there was activity //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 // -- timeout notify "forgot to turn off" --
void controlledArmchair::handleTimeout(){ // repeatedly notify via buzzer when in IDLE for a very long time to prevent battery drain ("forgot to turn off")
//check for timeout only when not idling already // also battery charge-level has to be below certain threshold to prevent beeping in case connected to charger
if (mode != controlMode_t::IDLE) { // note: ignores user input while in IDLE (e.g. encoder rotation)
//get current duty from controlled motor objects else if ((esp_log_timestamp() - timestamp_lastModeChange) > config.timeoutNotifyPowerStillOnMs && getBatteryPercent() < TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT)
float dutyLeftNow = motorLeft->getStatus().duty; {
float dutyRightNow = motorRight->getStatus().duty; // beep in certain intervals
if ((esp_log_timestamp() - timestamp_lastTimeoutBeep) > TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS)
//activity detected on any of the two motors {
if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance) ESP_LOGW(TAG, "timeout: [TIMEOUT] in IDLE since %.3f hours -> beeping", (float)(esp_log_timestamp() - timestamp_lastModeChange) / 1000 / 60 / 60);
|| validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance) // TODO dont beep at certain time ranges (e.g. at night)
){ timestamp_lastTimeoutBeep = esp_log_timestamp();
ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset"); buzzer->beep(6, 100, 50);
//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);
} }
} }
} }
@ -300,130 +434,143 @@ void controlledArmchair::handleTimeout(){
//----------- changeMode ------------ //----------- changeMode ------------
//----------------------------------- //-----------------------------------
//function to change to a specified control mode //function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) { void controlledArmchair::changeMode(controlMode_t modeNew)
//reset timeout timer {
resetTimeout(); // 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 // exit if target mode is already active
if (mode == modeNew) { if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]); ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return; return;
} }
//copy previous mode // mutex to wait for current handle iteration (control-task) to finish
modePrevious = mode; // 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 ========== //========== commands change FROM mode ==========
//run functions when changing FROM certain mode // run functions when changing FROM certain mode
switch(modePrevious){ switch (modePrevious)
default: {
ESP_LOGI(TAG, "noting to execute when changing FROM this mode"); default:
break; ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
case controlMode_t::IDLE:
#ifdef JOYSTICK_LOG_IN_IDLE #ifdef JOYSTICK_LOG_IN_IDLE
case controlMode_t::IDLE: ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'"); esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); // FIXME: loglevel from config
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config
break;
#endif #endif
buzzer->beep(1, 200, 100);
break;
case controlMode_t::HTTP: case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi"); ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap");
//stop http server wifi_stop_ap();
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");
break; break;
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input"); 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... // TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, massagePreviousDecel);
motorRight->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, massagePreviousDecel);
//set upfading to default value // restore previously set acceleration limit
motorLeft->setFade(fadeType_t::ACCEL, true); motorLeft->setFade(fadeType_t::ACCEL, massagePreviousAccel);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, massagePreviousAccel);
//reset frozen input state // reset frozen input state
freezeInput = false; freezeInput = false;
break; break;
case controlMode_t::AUTO: case controlMode_t::AUTO:
ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default"); 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... // TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->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); motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true);
break; 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 ========== //========== commands change TO mode ==========
//run functions when changing TO certain mode // run functions when changing TO certain mode
switch(modeNew){ switch (modeNew)
{
default: default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode"); ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break; break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
buzzer->beep(1, 1500, 0); ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep");
#ifdef JOYSTICK_LOG_IN_IDLE idleBothMotors();
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); buzzer->beep(1, 900, 0);
#endif break;
break;
case controlMode_t::HTTP: case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching to http mode -> enabling http and wifi"); ESP_LOGW(TAG, "switching to HTTP mode -> starting wifi-ap");
//start wifi wifi_start_ap();
//TODO: decide wether ap or client should be started break;
ESP_LOGI(TAG, "init wifi...");
//FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp) case controlMode_t::ADJUST_CHAIR:
//wifi_init_client(); ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode: turning both motors off, beep");
//wifi_init_ap(); idleBothMotors();
buzzer->beep(3, 100, 50);
break;
//wait for wifi case controlMode_t::MENU_SETTINGS:
//ESP_LOGI(TAG, "waiting for wifi..."); idleBothMotors();
//vTaskDelay(1000 / portTICK_PERIOD_MS);
//start http server
ESP_LOGI(TAG, "init http server...");
http_init_server();
ESP_LOGI(TAG, "done initializing http mode");
break; break;
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading"); 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) // save currently set normal acceleration config (for restore when leavinge MASSAGE again)
motorLeft->setFade(fadeType_t::DECEL, false); massagePreviousAccel = motorLeft->getFade(fadeType_t::ACCEL);
motorRight->setFade(fadeType_t::DECEL, false); massagePreviousDecel = motorLeft->getFade(fadeType_t::DECEL);
//reduce upfading (increase acceleration) // disable downfading (max. deceleration)
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel); motorLeft->setFade(fadeType_t::DECEL, shake_msFadeDecel, false);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel); 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; 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? //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 //switch to secondary mode when primary is already active
if (mode == modePrimary){ if (mode == modePrimary){
ESP_LOGW(TAG, "toggleModes: switching from primaryMode %s to secondarMode %s", controlModeStr[(int)mode], controlModeStr[(int)modeSecondary]); 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 changeMode(modeSecondary); //switch to secondary mode
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { 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); //buzzer->beep(4,200,100);
changeMode(modePrimary); changeMode(modePrimary);
} }
} }
@ -466,14 +613,67 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active //switch to previous mode when primary is already active
if (mode == modePrimary){ 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); //buzzer->beep(2,200,100);
changeMode(modePrevious); //switch to previous mode changeMode(modePrevious); //switch to previous mode
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { 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); //buzzer->beep(4,200,100);
changeMode(modePrimary); 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;
}

127
board_single/main/control.hpp
View File

@ -1,30 +1,55 @@
#pragma once #pragma once
extern "C"
{
#include "nvs_flash.h"
#include "nvs.h"
}
#include "motordrivers.hpp" #include "motordrivers.hpp"
#include "motorctl.hpp" #include "motorctl.hpp"
#include "buzzer.hpp" #include "buzzer.hpp"
#include "http.hpp" #include "http.hpp"
#include "auto.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 --- //---- struct, enum, variable declarations ---
//-------------------------------------------- //--------------------------------------------
//enum that decides how the motors get controlled //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) //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 --- //--- control_config_t ---
//struct with config parameters //struct with config parameters
typedef struct control_config_t { typedef struct control_config_t {
controlMode_t defaultMode; //default mode after startup and toggling IDLE controlMode_t defaultMode; //default mode after startup and toggling IDLE
//timeout options //timeout options
uint32_t timeoutMs; //time of inactivity after which the mode gets switched to IDLE uint32_t timeoutSwitchToIdleMs; //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 timeoutNotifyPowerStillOnMs;
} control_config_t; } 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* motorLeft_f,
controlledMotor* motorRight_f, controlledMotor* motorRight_f,
evaluatedJoystick* joystick_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 --- //--- 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(); void startHandleLoop();
//function that changes to a specified control mode //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 //function that restarts timer which initiates the automatic timeout (switch to IDLE) after certain time of inactivity
void resetTimeout(); void resetTimeout();
//function for sending a button event (e.g. from button task at event) to control task //methods to get the current or previous control mode
//TODO: use queue instead? controlMode_t getCurrentMode() const {return mode;};
void sendButtonEvent(uint8_t count); 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: private:
//--- functions --- //--- 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 //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 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 --- //--- objects ---
buzzer_t* buzzer; buzzer_t* buzzer;
controlledMotor* motorLeft; controlledMotor* motorLeft;
controlledMotor* motorRight; controlledMotor* motorRight;
httpJoystick* httpJoystickMain_l; httpJoystick* httpJoystickMain_l;
evaluatedJoystick* joystick_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 --- //---variables ---
//struct for motor commands returned by generate functions of each mode //struct for motor commands returned by generate functions of each mode
motorCommands_t commands; motorCommands_t commands = cmds_bothMotorsIdle;
//struct with config parameters //struct with config parameters
control_config_t config; control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data //store joystick data
joystickData_t stickData; joystickData_t stickData = joystickData_center;
bool altStickMapping; //alternative joystick mapping (reverse mapped differently) joystickData_t stickDataLast = joystickData_center;
//variables for http mode //variables for http mode
uint32_t http_timestamp_lastData = 0; uint32_t http_timestamp_lastData = 0;
@ -97,7 +182,7 @@ class controlledArmchair {
bool freezeInput = false; bool freezeInput = false;
//variables for AUTO mode //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 //variable to store button event
uint8_t buttonCount = 0; uint8_t buttonCount = 0;
@ -108,23 +193,13 @@ class controlledArmchair {
//variable to store mode when toggling IDLE mode //variable to store mode when toggling IDLE mode
controlMode_t modePrevious; //default 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 //variable for slow loop
uint32_t timestamp_SlowLoopLastRun = 0; uint32_t timestamp_SlowLoopLastRun = 0;
//variables for detecting timeout (switch to idle, after inactivity) //variables for detecting timeout (switch to idle, or notify "forgot to turn off" after inactivity
float dutyLeft_lastActivity = 0; uint32_t timestamp_lastModeChange = 0;
float dutyRight_lastActivity = 0;
uint32_t timestamp_lastActivity = 0; uint32_t timestamp_lastActivity = 0;
uint32_t timestamp_lastTimeoutBeep = 0;
}; };

715
board_single/main/display.cpp
View File

@ -1,30 +1,27 @@
#include "display.hpp" #include "display.hpp"
extern "C"{ extern "C"{
#include <driver/adc.h> #include <driver/adc.h>
#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 ADC_BATT_VOLTAGE ADC1_CHANNEL_6
#define BAT_CELL_COUNT 7 #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 //TODO duplicate code: getVoltage also defined in currentsensor.cpp -> outsource this
//local function to get average voltage from adc //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 //measure voltage
int measure = 0; uint32_t measure = 0;
for (int j=0; j<samples; j++){ for (int j=0; j<samples; j++){
measure += adc1_get_raw(adc); measure += adc1_get_raw(adc);
ets_delay_us(50); ets_delay_us(50);
} }
return (float)measure / samples / 4096 * 3.3; //return (float)measure / samples / 4096 * 3.3;
return measure / samples;
} }
//========================== //======================
//======= variables ======== //===== variables ======
//========================== //======================
//display
SSD1306_t dev; SSD1306_t dev;
//int center, top, bottom;
char lineChar[20];
//top = 2;
//center = 3;
//bottom = 8;
//tag for logging //tag for logging
static const char * TAG = "display"; static const char * TAG = "display";
//define currently shown status page (continously displayed content when not in MENU_SETTINGS mode)
static displayStatusPage_t selectedStatusPage = STATUS_SCREEN_OVERVIEW;
//======================
//================= //==== display_init ====
//===== init ====== //======================
//================= void display_init(display_config_t config){
void display_init(){
adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11); //max voltage adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11); //max voltage
ESP_LOGW("display", "INTERFACE is i2c"); ESP_LOGI(TAG, "Initializing Display with config: sda=%d, sdl=%d, reset=%d, offset=%d, flip=%d, size: %dx%d",
ESP_LOGW("display", "SDA_GPIO=%d",SDA_GPIO); config.gpio_sda, config.gpio_scl, config.gpio_reset, config.offsetX, config.flip, config.width, config.height);
ESP_LOGW("display", "SCL_GPIO=%d",SCL_GPIO);
ESP_LOGW("display", "RESET_GPIO=%d",RESET_GPIO); i2c_master_init(&dev, config.gpio_sda, config.gpio_scl, config.gpio_reset);
i2c_master_init(&dev, SDA_GPIO, SCL_GPIO, RESET_GPIO); if (config.flip) {
#if FLIP dev._flip = true;
dev._flip = true; ESP_LOGW(TAG, "Flip upside down");
ESP_LOGW("display", "Flip upside down"); }
#endif ssd1306_init(&dev, config.width, config.height, config.offsetX);
ESP_LOGI("display", "Panel is 128x64");
ssd1306_init(&dev, 128, 64);
ssd1306_clear_screen(&dev, false); ssd1306_clear_screen(&dev, false);
ssd1306_contrast(&dev, 0xff); ssd1306_contrast(&dev, config.contrastNormal);
//store configuration locally (e.g. for accessing timeouts)
displayConfig = config;
} }
//===============================
//======= displayTextLine =======
//===============================
//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 isLarge, bool inverted, const char *format, ...)
{
char buf[17];
int len;
// format string + arguments to string
va_list args;
va_start(args, format);
len = vsnprintf(buf, sizeof(buf), format, args);
va_end(args);
// show line on display
if (isLarge)
ssd1306_display_text_x3(display, line, buf, len, inverted);
else
ssd1306_display_text(display, line, buf, len, inverted);
}
//===================================
//===== displayTextLineCentered =====
//===================================
//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)
#define MAX_LEN_NORMAL 16 //count of available digits on display (normal/large font)
#define MAX_LEN_LARGE 5
void displayTextLineCentered(SSD1306_t *display, int line, bool isLarge, bool inverted, const char *format, ...)
{
// variables
char buf[MAX_LEN_NORMAL*2 + 2];
char tmp[MAX_LEN_NORMAL + 1];
int len;
// format string + arguments to string (-> 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(){ float getBatteryVoltage(){
#define BAT_VOLTAGE_CONVERSION_FACTOR 11.9 // check if lookup table is configured correctly
float voltageRead = getVoltage1(ADC_BATT_VOLTAGE, 1000); int countAdc = sizeof(batteryAdcValues) / sizeof(float);
float battVoltage = voltageRead * 11.9; //note: factor comes from simple test with voltmeter int countVoltages = sizeof(batteryVoltages) / sizeof(float);
ESP_LOGD(TAG, "batteryVoltage - voltageAdc=%f, voltageConv=%f, factor=%.2f", voltageRead, battVoltage, BAT_VOLTAGE_CONVERSION_FACTOR); 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; return battVoltage;
} }
//---------------------------------- //----------------------------------
//------- getBatteryPercent -------- //------- getBatteryPercent --------
//---------------------------------- //----------------------------------
//TODO find better/more accurate table? // TODO find better/more accurate table?
//configure discharge curve of one cell with corresponding known voltage->chargePercent values // 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 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 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}; 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 getBatteryPercent()
float cellVoltage = voltage/BAT_CELL_COUNT; {
int size = sizeof(voltageLevels) / sizeof(voltageLevels[0]); // check if lookup table is configured correctly
int sizePer = sizeof(percentageLevels) / sizeof(percentageLevels[0]); int sizeVoltage = sizeof(cellVoltageLevels) / sizeof(cellVoltageLevels[0]);
//check if configured correctly int sizePer = sizeof(cellPercentageLevels) / sizeof(cellPercentageLevels[0]);
if (size != sizePer) { if (sizeVoltage != sizePer)
{
ESP_LOGE(TAG, "getBatteryPercent - count of configured percentages do not match count of voltages"); ESP_LOGE(TAG, "getBatteryPercent - count of configured percentages do not match count of voltages");
return 0; return 0;
} }
if (cellVoltage <= voltageLevels[0]) {
return 0.0; //get current battery voltage
} else if (cellVoltage >= voltageLevels[size - 1]) { float voltage = getBatteryVoltage();
return 100.0; 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 ESP_LOGW(TAG, "switching statusPage from %d to %d", (int)selectedStatusPage, (int)newStatusPage);
for (int i = 1; i < size; ++i) { selectedStatusPage = newStatusPage;
if (cellVoltage <= voltageLevels[i]) {
float voltageRange = voltageLevels[i] - voltageLevels[i - 1]; //-- run commands when switching TO certain mode --
float voltageOffset = cellVoltage - voltageLevels[i - 1]; switch (selectedStatusPage)
float percentageRange = percentageLevels[i] - percentageLevels[i - 1]; {
float percentageOffset = percentageLevels[i - 1]; case STATUS_SCREEN_SCREENSAVER:
float percent = percentageOffset + (voltageOffset / voltageRange) * percentageRange; ssd1306_clear_screen(&dev, false); // clear screen when switching
ESP_LOGD(TAG, "getBatPercent - cellVoltage=%.3f => percentage=%.3f", cellVoltage, percent); #ifdef HARDWARE_SCROLL_AVAILABLE
ESP_LOGD(TAG, "getBatPercent - matched range: %.2fV-%.2fV => %.1f%%-%.1f%%", voltageLevels[i-1], voltageLevels[i], percentageLevels[i-1], percentageLevels[i]); ssd1306_hardware_scroll(&dev, SCROLL_RIGHT);
return percent; #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"); else if (selectedStatusPage == STATUS_SCREEN_SCREENSAVER) // exit screensaver when there was recent activity
return 0.0; //unknown range {
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 ======= //======= display task =======
//============================ //============================
#define VERY_SLOW_LOOP_INTERVAL 30000 // TODO: separate task for each loop?
#define SLOW_LOOP_INTERVAL 1000 void display_task(void *pvParameters)
#define FAST_LOOP_INTERVAL 200 {
//TODO: separate taks for each loop? 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 ){ // initialize display
char buf[20]; display_init(objects->displayConfig);
char buf1[20]; // TODO check if successfully initialized
int len, len1;
int countFastloop = SLOW_LOOP_INTERVAL;
int countSlowLoop = VERY_SLOW_LOOP_INTERVAL;
display_init(); // show startup message
//TODO check if successfully initialized showStartupMsg();
vTaskDelay(STARTUP_MSG_TIMEOUT / portTICK_PERIOD_MS);
ssd1306_clear_screen(&dev, false);
//welcome msg // repeatedly update display with content depending on current mode
strcpy(buf, "Hello"); while (1)
ssd1306_display_text_x3(&dev, 0, buf, 5, false); {
vTaskDelay(1000 / portTICK_PERIOD_MS); 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 //// Fade Out
//ssd1306_fadeout(&dev); //ssd1306_fadeout(&dev);
#if 0
// Fade Out
for(int contrast=0xff;contrast>0;contrast=contrast-0x20) {
ssd1306_contrast(&dev, contrast);
vTaskDelay(40);
}
#endif
}

64
board_single/main/display.hpp
View File

@ -1,18 +1,80 @@
#pragma once
extern "C" { extern "C" {
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <stdarg.h>
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/task.h" #include "freertos/task.h"
#include "esp_log.h" #include "esp_log.h"
#include "nvs_flash.h"
#include "nvs.h"
#include "ssd1306.h" #include "ssd1306.h"
#include "font8x8_basic.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 //task that inititialized the display, displays welcome message
//and releatedly updates the display with certain content //and releatedly updates the display with certain content
void display_task( void * pvParameters ); 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, ...);

75
board_single/main/encoder.cpp Normal file
View File

@ -0,0 +1,75 @@
extern "C"
{
#include <stdio.h>
#include <esp_system.h>
#include <esp_event.h>
#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;
}
}
}
}

17
board_single/main/encoder.hpp Normal file
View File

@ -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);

22
board_single/main/fan.cpp
View File

@ -12,6 +12,28 @@ extern "C"
static const char * TAG = "fan-control"; 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 -------- //-------- constructor --------
//----------------------------- //-----------------------------

17
board_single/main/fan.hpp
View File

@ -16,7 +16,22 @@ typedef struct fan_config_t {
uint32_t minOnMs; uint32_t minOnMs;
uint32_t minOffMs; uint32_t minOffMs;
uint32_t turnOffDelayMs; 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 );

345
board_single/main/main.cpp
View File

@ -1,12 +1,10 @@
#include "hal/uart_types.h"
#include "motordrivers.hpp"
#include "types.hpp"
extern "C" extern "C"
{ {
#include <stdio.h> #include <stdio.h>
#include <esp_system.h> #include <esp_system.h>
#include <esp_event.h> #include <esp_event.h>
#include <nvs_flash.h> #include <nvs_flash.h>
#include "nvs.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/task.h" #include "freertos/task.h"
#include "driver/gpio.h" #include "driver/gpio.h"
@ -14,98 +12,94 @@ extern "C"
#include "sdkconfig.h" #include "sdkconfig.h"
#include "esp_spiffs.h" #include "esp_spiffs.h"
#include "driver/ledc.h"
//custom C files //custom C files
#include "wifi.h" #include "wifi.h"
} }
#include <new>
//custom C++ files //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 "control.hpp"
#include "button.hpp" #include "button.hpp"
#include "http.hpp"
#include "uart_common.hpp"
#include "display.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"; 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) //initialize spi flash filesystem (used for webserver)
void init_spiffs(){ void init_spiffs(){
ESP_LOGI(TAG, "init spiffs");
esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = { esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = {
.base_path = "/spiffs", .base_path = "/spiffs",
.partition_label = NULL, .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); //========= createObjects =========
//esp_log_level_set("buzzer", ESP_LOG_INFO); //=================================
//esp_log_level_set("motordriver", ESP_LOG_DEBUG); //create all shared objects
//esp_log_level_set("motor-control", ESP_LOG_INFO); //their references can be passed to the tasks that need access in main
//esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
//esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); //Note: the configuration structures (e.g. configMotorControlLeft) are outsourced to file 'config.cpp'
esp_log_level_set("button", ESP_LOG_INFO);
esp_log_level_set("control", ESP_LOG_INFO); void createObjects()
//esp_log_level_set("fan-control", ESP_LOG_INFO); {
esp_log_level_set("wifi", ESP_LOG_INFO); // create sabertooth motor driver instance
esp_log_level_set("http", ESP_LOG_INFO); // sabertooth2x60a sabertoothDriver(sabertoothConfig);
//esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG); // with configuration above
esp_log_level_set("display", ESP_LOG_INFO); //sabertoothDriver = new sabertooth2x60a(sabertoothConfig);
//esp_log_level_set("current-sensors", ESP_LOG_INFO);
//esp_log_level_set("speedSensor", ESP_LOG_INFO); // 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 ============ //=========== app_main ============
//================================= //=================================
extern "C" void app_main(void) { 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..."); ESP_LOGW(TAG, "enabling 5V regulator...");
gpio_pad_select_gpio(GPIO_NUM_17); gpio_pad_select_gpio(GPIO_NUM_17);
gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT);
gpio_set_level(GPIO_NUM_17, 1); gpio_set_level(GPIO_NUM_17, 1);
//---- define log levels ---- //--- initialize nvs-flash and netif ---
setLoglevels(); 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<sabertooth2x60a*>(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 --- //--- create task for controlling the motors ---
//---------------------------------------------- //----------------------------------------------
//task that receives commands, handles ramp and current limit and executes commands using the motordriver function //task for each motor that handles to following:
xTaskCreate(&task_motorctl, "task_motor-control", 2*4096, NULL, 6, NULL); //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 --- //--- 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 --- //--- create task for control ---
//------------------------------- //-------------------------------
//task that generates motor commands depending on the current mode and sends those to motorctl task //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 --- //--- create task for button ---
//------------------------------ //------------------------------
//task that evaluates and processes the button input and runs the configured commands //task that handles button/encoder events in any mode except 'MENU_SETTINGS' and 'MENU_MODE_SELECT' (e.g. switch modes by pressing certain count)
xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL); 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 --- //--- create task for fan control ---
//----------------------------------- //-----------------------------------
//task that evaluates and processes the button input and runs the configured commands //task that controls cooling fans of the motor driver
xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL); task_fans_parameters_t fans_param = {configFans, motorLeft, motorRight};
xTaskCreate(&task_fans, "task_fans", 2048, &fans_param, 1, NULL);
//----------------------------------- //-----------------------------------
//----- create task for display ----- //----- create task for display -----
//----------------------------------- //-----------------------------------
//task that handles the display //task that handles the display (show stats, handle menu in 'MENU_SETTINGS' and 'MENU_MODE_SELECT' mode)
xTaskCreate(&display_task, "display_task", 3*2048, NULL, 1, NULL); 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) --- //--- startup finished ---
wifi_initNvs_initNetif(); ESP_LOGW(TAG, "===== STARTUP FINISHED =====\n");
buzzer->beep(3, 70, 50);
//--- 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");
//--- testing encoder ---
//xTaskCreate(&task_encoderExample, "task_buzzer", 2048, encoderQueue, 2, NULL);
//--- testing http server --- //--- testing http server ---
// wifi_init_client(); //connect to existing wifi // wifi_init_client(); //connect to existing wifi
@ -235,15 +302,17 @@ extern "C" void app_main(void) {
// http_init_server(); // http_init_server();
//--- testing force http mode after startup --- //--- testing force specific mode after startup ---
//control.changeMode(controlMode_t::HTTP); //control->changeMode(controlMode_t::MENU_SETTINGS);
//--- main loop --- //--- main loop ---
//does nothing except for testing things //does nothing except for testing things
while(1){ while(1){
vTaskDelay(5000 / portTICK_PERIOD_MS); vTaskDelay(portMAX_DELAY);
//vTaskDelay(5000 / portTICK_PERIOD_MS);
//--------------------------------- //---------------------------------
//-------- TESTING section -------- //-------- TESTING section --------
//--------------------------------- //---------------------------------

1086
board_single/main/menu.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

37
board_single/main/menu.hpp Normal file
View File

@ -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);

15
board_single/sdkconfig
View File

@ -140,10 +140,10 @@ CONFIG_I2C_INTERFACE=y
# CONFIG_SPI_INTERFACE is not set # CONFIG_SPI_INTERFACE is not set
# CONFIG_SSD1306_128x32 is not set # CONFIG_SSD1306_128x32 is not set
CONFIG_SSD1306_128x64=y CONFIG_SSD1306_128x64=y
CONFIG_OFFSETX=0 CONFIG_OFFSETX=2
# CONFIG_FLIP is not set # CONFIG_FLIP is not set
CONFIG_SCL_GPIO=22 CONFIG_SCL_GPIO=22
CONFIG_SDA_GPIO=21 CONFIG_SDA_GPIO=23
CONFIG_RESET_GPIO=15 CONFIG_RESET_GPIO=15
CONFIG_I2C_PORT_0=y CONFIG_I2C_PORT_0=y
# CONFIG_I2C_PORT_1 is not set # 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_MBO_SUPPORT is not set
# CONFIG_WPA_DPP_SUPPORT is not set # CONFIG_WPA_DPP_SUPPORT is not set
# end of Supplicant # 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 # end of Component config
# #

1
common/CMakeLists.txt
View File

@ -10,6 +10,7 @@ idf_component_register(
"joystick.cpp" "joystick.cpp"
"http.cpp" "http.cpp"
"speedsensor.cpp" "speedsensor.cpp"
"chairAdjust.cpp"
INCLUDE_DIRS INCLUDE_DIRS
"." "."
PRIV_REQUIRES nvs_flash mdns json spiffs esp_http_server PRIV_REQUIRES nvs_flash mdns json spiffs esp_http_server

29
common/buzzer.cpp
View File

@ -2,6 +2,19 @@
static const char *TAG_BUZZER = "buzzer"; 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 ============ //========== init ============
//============================ //============================
@ -33,12 +46,18 @@ buzzer_t::buzzer_t(gpio_num_t gpio_pin_f, uint16_t msGap_f){
//=========== beep =========== //=========== beep ===========
//============================ //============================
//function to add a beep command to the queue //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){ 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 //create entry struct with provided data
struct beepEntry entryInsert = { struct beepEntry entryInsert = {
count = count, count,
msOn = msOn, msOn,
msOff = msOff msOff,
msDelayFinished
}; };
// Send a pointer to a struct AMessage object. Don't block if the // 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 // otherwise waits for at least 7 weeks
if( xQueueReceive( beepQueue, &entryRead, portMAX_DELAY ) ) 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 //beep requested count with requested delays
for (int i = entryRead.count; i--;){ for (int i = entryRead.count; i--;){
@ -83,7 +102,7 @@ void buzzer_t::processQueue(){
vTaskDelay(entryRead.msOff / portTICK_PERIOD_MS); vTaskDelay(entryRead.msOff / portTICK_PERIOD_MS);
} }
//wait for minimum gap between beep events //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 }else{ //wait for queue to become available
vTaskDelay(50 / portTICK_PERIOD_MS); vTaskDelay(50 / portTICK_PERIOD_MS);

12
common/buzzer.hpp
View File

@ -27,24 +27,27 @@ class buzzer_t {
//--- functions --- //--- functions ---
void processQueue(); //has to be run once in a separate task, waits for and processes queued events 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 beep(uint8_t count, uint16_t msOn, uint16_t msOff);
//void clear(); (TODO - not implemented yet) //void clear(); (TODO - not implemented yet)
//void createTask(); (TODO - not implemented yet) //void createTask(); (TODO - not implemented yet)
//--- variables --- //--- variables ---
uint16_t msGap; //gap between beep entries (when multiple queued)
private: private:
//--- functions --- //--- functions ---
void init(); void init();
//--- variables --- //--- variables ---
uint16_t msGap; //gap between beep entries (when multiple queued)
gpio_num_t gpio_pin; gpio_num_t gpio_pin;
struct beepEntry { struct beepEntry {
uint8_t count; uint8_t count;
uint16_t msOn; uint16_t msOn;
uint16_t msOff; uint16_t msOff;
uint16_t msDelay;
}; };
//queue for queueing up multiple events while one is still processing //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);

115
common/chairAdjust.cpp Normal file
View File

@ -0,0 +1,115 @@
extern "C"
{
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include <string.h>
}
#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);
}

41
common/chairAdjust.hpp Normal file
View File

@ -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);

14
common/currentsensor.cpp
View File

@ -3,6 +3,7 @@ extern "C" {
#include "esp_log.h" #include "esp_log.h"
} }
#include <math.h>
#include "currentsensor.hpp" #include "currentsensor.hpp"
//tag for logging //tag for logging
@ -29,10 +30,12 @@ float getVoltage(adc1_channel_t adc, uint32_t samples){
//============================= //=============================
//======== constructor ======== //======== 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 //copy config
adcChannel = adcChannel_f; adcChannel = adcChannel_f;
ratedCurrent = ratedCurrent_f; ratedCurrent = ratedCurrent_f;
isInverted = isInverted_f;
snapToZeroThreshold = snapToZeroThreshold_f;
//init adc //init adc
adc1_config_width(ADC_WIDTH_BIT_12); //max resolution 4096 adc1_config_width(ADC_WIDTH_BIT_12); //max resolution 4096
adc1_config_channel_atten(adcChannel, ADC_ATTEN_DB_11); //max voltage adc1_config_channel_atten(adcChannel, ADC_ATTEN_DB_11); //max voltage
@ -58,6 +61,15 @@ float currentSensor::read(void){
current = 0; 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); ESP_LOGI(TAG, "read sensor adc=%d: voltage=%.3fV, centerVoltage=%.3fV => current=%.3fA", (int)adcChannel, voltage, centerVoltage, current);
return current; return current;
} }

4
common/currentsensor.hpp
View File

@ -7,12 +7,14 @@
class currentSensor{ class currentSensor{
public: 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 void calibrateZeroAmpere(void); //set current voltage to voltage representing 0A
float read(void); //get current ampere float read(void); //get current ampere
private: private:
adc1_channel_t adcChannel; adc1_channel_t adcChannel;
float ratedCurrent; float ratedCurrent;
bool isInverted;
float snapToZeroThreshold;
uint32_t measure; uint32_t measure;
float voltage; float voltage;
float current; float current;

41
common/http.cpp
View File

@ -178,50 +178,39 @@ esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){
//------------------- //-------------------
//----- getData ----- //----- 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(){ joystickData_t httpJoystick::getData(){
//--- get joystick data from queue --- //--- 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", ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f",
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle); dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
timeLastData = esp_log_timestamp();
} }
//--- timeout --- //--- 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 { else {
//send error message when last received data did NOT result in CENTER position if (dataRead.position != joystickPos_t::CENTER && (esp_log_timestamp() - timeLastData) > config.timeoutMs) {
if (dataRead.position != joystickPos_t::CENTER) {
//change data to "joystick center" data to stop the motors //change data to "joystick center" data to stop the motors
dataRead = dataCenter; 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; 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 ===== //===== init http server =====
//============================ //============================
//function that initializes http server and configures available urls //function that initializes http server and configures available url's
void http_init_server()
//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 ---- //---- configure webserver ----
httpd_config_t config = HTTPD_DEFAULT_CONFIG(); httpd_config_t config = HTTPD_DEFAULT_CONFIG();
@ -236,7 +225,7 @@ void http_init_server()
httpd_uri_t joystick_url = { httpd_uri_t joystick_url = {
.uri = "/api/joystick", .uri = "/api/joystick",
.method = HTTP_POST, .method = HTTP_POST,
.handler = on_joystick_url, .handler = onJoystickUrl,
}; };
httpd_register_uri_handler(server, &joystick_url); httpd_register_uri_handler(server, &joystick_url);
@ -265,8 +254,8 @@ void http_init_server()
//function that destroys the http server //function that destroys the http server
void http_stop_server() void http_stop_server()
{ {
printf("stopping http\n"); ESP_LOGW(TAG, "stopping HTTP-Server");
httpd_stop(server); httpd_stop(server);
} }

29
common/http.hpp
View File

@ -13,7 +13,18 @@ extern "C"
//===== init http server ===== //===== init http server =====
//============================ //============================
//function that initializes http server and configures available urls //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 ===== //===== stop http server =====
//============================ //============================
//function that destroys the 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{ class httpJoystick{
public: public:
//--- constructor --- //--- constructor ---
httpJoystick( httpJoystick_config_t config_f ); httpJoystick(httpJoystick_config_t config_f);
//--- functions --- //--- functions ---
joystickData_t getData(); //wait for and return joystick data from queue, if timeout return CENTER 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; httpJoystick_config_t config;
QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) ); QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) );
//struct for receiving data from http function, and storing data of last update //struct for receiving data from http function, and storing data of last update
joystickData_t dataRead; uint32_t timeLastData = 0;
const joystickData_t dataCenter = { const joystickData_t dataCenter = {
.position = joystickPos_t::CENTER, .position = joystickPos_t::CENTER,
.x = 0, .x = 0,
@ -67,11 +78,5 @@ class httpJoystick{
.radius = 0, .radius = 0,
.angle = 0 .angle = 0
}; };
}; joystickData_t dataRead = dataCenter;
};
//===== global object =====
//create global instance of httpJoystick
//note: is constructed/configured in config.cpp
extern httpJoystick httpJoystickMain;

342
common/joystick.cpp
View File

@ -19,8 +19,9 @@ static const char * TAG_CMD = "joystickCommands";
//-------- constructor -------- //-------- constructor --------
//----------------------------- //-----------------------------
//copy provided struct with all configuration and run init function //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; config = config_f;
nvsHandle = nvsHandle_f;
init(); init();
} }
@ -30,7 +31,7 @@ evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){
//---------- init ------------ //---------- init ------------
//---------------------------- //----------------------------
void evaluatedJoystick::init(){ void evaluatedJoystick::init(){
ESP_LOGI(TAG, "initializing joystick"); ESP_LOGW(TAG, "initializing ADC's and loading calibration...");
//initialize adc //initialize adc
adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096 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_x, ADC_ATTEN_DB_11); //max voltage
adc1_config_channel_atten(config.adc_y, 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 //define joystick center from current position
defineCenter(); //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..."); ESP_LOGV(TAG, "getting X coodrdinate...");
uint32_t adcRead; uint32_t adcRead;
adcRead = readAdc(config.adc_x, config.x_inverted); 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; data.x = x;
ESP_LOGD(TAG, "X: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => x=%.3f", 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..."); ESP_LOGV(TAG, "getting Y coodrinate...");
adcRead = readAdc(config.adc_y, config.y_inverted); 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; data.y = y;
ESP_LOGD(TAG, "Y: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => y=%.3lf", 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 //calculate radius
data.radius = sqrt(pow(data.x,2) + pow(data.y,2)); 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 ========= //========= joystick_CommandsDriving =========
//============================================ //============================================
//function that generates commands for both motors from the joystick data //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 //--- interpret config parameters ---
//typedef struct joystickData_t { float dutyOffset = config->dutyOffset; // immediately starts with this duty
// joystickPos_t position; float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
// float x; // calculate configured boost duty (added when turning)
// float y; float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
// float radius; // limit to maximum possible duty
// float angle; float dutyAvailable = 100 - config->maxDutyStraight;
//} joystickData_t; if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable;
//--- 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
//--- snap ratio to max at angle threshold ---
//(-> more joystick area where inner wheel is off when turning) //--- calculate paramaters with current data ---
/* motorCommands_t commands; // store new motor commands
//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
float ratioClipThreshold = 0.3; // -- calculate ratio --
if (ratio < ratioClipThreshold) ratio = 0; // get current ratio from stick angle
else if (ratio > 1-ratioClipThreshold) ratio = 1; float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
//TODO subtract this clip threshold from available joystick range at ratio usage 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 --- //--- experimental alternative control mode ---
if (altStickMapping == true){ if (config->altStickMapping == true){
//swap BOTTOM_LEFT and BOTTOM_RIGHT //swap BOTTOM_LEFT and BOTTOM_RIGHT
if (data.position == joystickPos_t::BOTTOM_LEFT){ if (data.position == joystickPos_t::BOTTOM_LEFT){
data.position = joystickPos_t::BOTTOM_RIGHT; data.position = joystickPos_t::BOTTOM_RIGHT;
@ -375,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altSt
case joystickPos_t::TOP_RIGHT: case joystickPos_t::TOP_RIGHT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::TOP_LEFT: case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_LEFT: case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_RIGHT: case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
} }
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f", // log input data
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); // 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; return commands;
} }
@ -418,13 +438,21 @@ uint32_t shake_timestamp_turnedOn = 0;
uint32_t shake_timestamp_turnedOff = 0; uint32_t shake_timestamp_turnedOff = 0;
bool shake_state = false; bool shake_state = false;
joystickPos_t lastStickPos = joystickPos_t::CENTER; 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 //--- configure shake mode --- TODO: move this to config
uint32_t shake_msOffMax = 80; uint32_t shake_msOffMax = 60;
uint32_t shake_msOnMax = 120; 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 //function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
@ -432,25 +460,29 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
//--- handle pulsing shake variable --- //--- handle pulsing shake variable ---
//TODO remove this, make individual per mode? //TODO remove this, make individual per mode?
//TODO only run this when not CENTER anyways? //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 float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
static uint32_t cycleCount = 0;
//calculate on/off duration //calculate on/off duration
uint32_t msOn = shake_msOnMax * data.radius; float msOn = (shake_msOnMax - shake_minDelay) * data.radius + shake_minDelay;
uint32_t msOff = shake_msOffMax * data.radius; 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 ){ if (data.radius > 0 ){
//currently off //currently off:
if (shake_state == false){ if (shake_state == false){
//off long enough //off long enough
if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) { if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) {
//turn on //turn on
cycleCount++;
shake_state = true; shake_state = true;
shake_timestamp_turnedOn = esp_log_timestamp(); 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 { else {
//on long enough //on long enough
if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) { if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) {
@ -475,79 +507,49 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
// float angle; // float angle;
//} joystickData_t; //} joystickData_t;
//--- evaluate stick position --- // force off when stick pos changes - TODO: is this necessary?
//4 quadrants and center only - with X and Y axis as hysteresis static joystickPos_t stickPosPrev = joystickPos_t::CENTER;
switch (data.position){ if (data.position != stickPosPrev) {
ESP_LOGW(TAG, "massage: stick quadrant changed, stopping for one cycle");
case joystickPos_t::CENTER: shake_state = false;
//immediately set to center at center shake_timestamp_turnedOff = esp_log_timestamp();
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;
} }
stickPosPrev = data.position; // update last position
//--- handle different modes (joystick in any of 4 quadrants) --- //--- handle different modes (joystick in any of 4 quadrants) ---
switch (stickQuadrant){ switch (data.position){
// idle
case joystickPos_t::CENTER: 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.left.state = motorstate_t::IDLE;
commands.right.state = motorstate_t::IDLE; commands.right.state = motorstate_t::IDLE;
commands.left.duty = 0; commands.left.duty = 0;
commands.right.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; return commands;
break; 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_RIGHT:
case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
break; break;
case joystickPos_t::TOP_LEFT: // shake left right
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
break;
case joystickPos_t::BOTTOM_LEFT: case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::FWD;
break;
case joystickPos_t::BOTTOM_RIGHT: case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
break; 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 --- //--- turn motors on/off depending on pulsing shake variable ---
if (shake_state == true){ if (shake_state == true){
@ -562,11 +564,127 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
commands.right.duty = 0; commands.right.duty = 0;
} }
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);
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);
return commands; 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;
}

59
common/joystick.hpp
View File

@ -8,6 +8,9 @@ extern "C"
#include "driver/adc.h" #include "driver/adc.h"
#include "esp_log.h" #include "esp_log.h"
#include "esp_err.h" #include "esp_err.h"
#include "nvs_flash.h"
#include "nvs.h"
#include <stdbool.h>
} }
#include <cmath> #include <cmath>
@ -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}; enum class joystickPos_t {CENTER, Y_AXIS, X_AXIS, TOP_RIGHT, TOP_LEFT, BOTTOM_LEFT, BOTTOM_RIGHT};
extern const char* joystickPosStr[7]; 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 //struct with current data of the joystick
typedef struct joystickData_t { typedef struct joystickData_t {
@ -65,36 +69,59 @@ typedef struct joystickData_t {
float angle; float angle;
} joystickData_t; } 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 ----- //----- evaluatedJoystick class -----
//------------------------------------ //------------------------------------
class evaluatedJoystick { class evaluatedJoystick
public: {
//--- constructor --- public:
evaluatedJoystick(joystick_config_t config_f); //--- constructor ---
evaluatedJoystick(joystick_config_t config_f, nvs_handle_t * nvsHandle);
//--- functions --- //--- functions ---
joystickData_t getData(); //read joystick, calculate values and return the data in a struct joystickData_t getData(); // read joystick, calculate values and return the data in a struct
void defineCenter(); //define joystick center from current position // 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: private:
//--- functions --- //--- functions ---
//initialize adc inputs, define center // initialize adc inputs, define center
void init(); void init();
//read adc while making multiple samples with option to invert the result // loads selected calibration value from nvs or default values from config if no data stored
int readAdc(adc1_channel_t adc_channel, bool inverted = false); 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 --- //--- variables ---
// handle for using the nvs flash (persistent config variables)
nvs_handle_t *nvsHandle;
joystick_config_t config; joystick_config_t config;
int x_min;
int x_max;
int y_min;
int y_max;
int x_center; int x_center;
int y_center; int y_center;
joystickData_t data; joystickData_t data;
float x; float x;
float y; float y;
}; };
@ -103,7 +130,7 @@ class evaluatedJoystick {
//============================================ //============================================
//function that generates commands for both motors from the joystick data //function that generates commands for both motors from the joystick data
//motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick); //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);

651
common/motorctl.cpp
View File

@ -5,25 +5,47 @@
//tag for logging //tag for logging
static const char * TAG = "motor-control"; 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 ========
//============================= //=============================
//constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') //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): controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f):
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) { //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 //copy parameters for controlling the motor
config = config_control; config = config_control;
log = config.loggingEnabled;
//pointer to update motot dury method //pointer to update motot dury method
motorSetCommand = setCommandFunc; motorSetCommand = setCommandFunc;
//copy configured default fading durations to actually used variables //pointer to nvs handle
msFadeAccel = config.msFadeAccel; nvsHandle = nvsHandle_f;
msFadeDecel = config.msFadeDecel; //pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values
init(); 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(){ void controlledMotor::init(){
commandQueue = xQueueCreate( 1, sizeof( struct motorCommand_t ) ); 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 //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
//--- receive commands from queue --- //--- RECEIVE DATA FROM QUEUE ---
if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) ) 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; state = commandReceive.state;
dutyTarget = commandReceive.duty; dutyTarget = commandReceive.duty;
receiveTimeout = false; receiveTimeout = false;
timestamp_commandReceived = esp_log_timestamp(); 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 --- //--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate // define target duty (-100 to 100) from provided duty and motorstate
//this value is more suitable for the fading algorithm // this value is more suitable for t
switch(commandReceive.state){ // todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm
case motorstate_t::BRAKE: switch (commandReceive.state)
//update state {
state = motorstate_t::BRAKE; case motorstate_t::BRAKE:
//dutyTarget = 0; // update state
dutyTarget = fabs(commandReceive.duty); state = motorstate_t::BRAKE;
break; // dutyTarget = 0;
case motorstate_t::IDLE: dutyTarget = fabs(commandReceive.duty);
dutyTarget = 0; break;
break; case motorstate_t::IDLE:
case motorstate_t::FWD: dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty); break;
break; case motorstate_t::FWD:
case motorstate_t::REV: dutyTarget = fabs(commandReceive.duty);
dutyTarget = - fabs(commandReceive.duty); break;
break; case motorstate_t::REV:
dutyTarget = -fabs(commandReceive.duty);
break;
} }
} break;
//--- timeout, no data --- #define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 1 //difference from target where no change is made yet
//turn motors off if no data received for long time (e.g. no uart data / control offline) #define CURRENT_CONTROL_MIN_AMPERE 0.7 //current where motor is turned off
//TODO no timeout when braking? //TODO define different, fixed fading configuration in current mode, fade down can be significantly less (500/500ms fade up worked fine)
if ((esp_log_timestamp() - timestamp_commandReceived) > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout){ case motorControlMode_t::CURRENT: // regulate to desired current flow
receiveTimeout = true; ampereNow = cSensor.read();
state = motorstate_t::IDLE; ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config
dutyTarget = 0; if (commandReceive.state == motorstate_t::REV) ampereTarget = - ampereTarget; //target is negative when driving reverse
ESP_LOGE(TAG, "TIMEOUT, no target data received for more than %ds -> switch to IDLE", TIMEOUT_IDLE_WHEN_NO_COMMAND/1000); 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 --- //--- when IDLE to keep the current at target zero motor needs to be on for some duty (to compensate generator current)
//calculate increment for fading UP with passed time since last run and configured fade time if (commandReceive.duty == 0 && fabs(ampereNow) < CURRENT_CONTROL_MIN_AMPERE){ //stop motors completely when current is very low already
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs; dutyTarget = 0;
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 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 { } 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 { //--- TIMEOUT NO DATA ---
dutyIncrementDecel = 100; // 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 ---
//brake immediately, update state, duty and exit this cycle of handle function //brake immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){ if (state == motorstate_t::BRAKE){
ESP_LOGD(TAG, "braking - skip fading"); if(log) ESP_LOGD(TAG, "braking - skip fading");
motorSetCommand({motorstate_t::BRAKE, dutyTarget}); 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; //dutyNow = 0;
return; //no need to run the fade algorithm 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 ----- //----- 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) //TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
if (dutyNow < 0) { //reverse, decelerating if (dutyNow < 0) { //reverse, decelerating
@ -174,25 +396,103 @@ void controlledMotor::handle(){
} }
//----- CURRENT LIMIT ----- //----- CURRENT LIMIT -----
currentNow = cSensor.read(); currentNow = cSensor.read();
if ((config.currentLimitEnabled) && (dutyDelta != 0)){ if ((config.currentLimitEnabled) && (dutyDelta != 0)){
if (fabs(currentNow) > config.currentMax){ if (fabs(currentNow) > config.currentMax){
float dutyOld = dutyNow; float dutyOld = dutyNow;
//adaptive decrement: //adaptive decrement:
//Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values //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 float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0
if (dutyNow < -currentLimitDecrement) { if (dutyNow < -currentLimitDecrement) {
dutyNow += currentLimitDecrement; dutyNow += currentLimitDecrement;
} else if (dutyNow > currentLimitDecrement) { } else if (dutyNow > currentLimitDecrement) {
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) //--- define new motorstate --- (-100 to 100 => direction)
state=getStateFromDuty(dutyNow); state=getStateFromDuty(dutyNow);
@ -202,25 +502,27 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting //FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE //FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only? //TODO check when changed only?
if ( //not enough time between last direction state if (config.deadTimeMs > 0) { //deadTime is enabled
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) if ( //not enough time between last direction state
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs)) ( 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 if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
deadTimeWaiting = true; if (!deadTimeWaiting){ //log start
ESP_LOGW(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); 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; //force IDLE state during wait
dutyNow = 0; state = motorstate_t::IDLE;
} else { dutyNow = 0;
if (deadTimeWaiting){ //log end } else {
deadTimeWaiting = false; if (deadTimeWaiting){ //log end
ESP_LOGW(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); deadTimeWaiting = false;
} if(log) ESP_LOGI(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(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
}
//--- save current actual motorstate and timestamp --- //--- save current actual motorstate and timestamp ---
@ -232,7 +534,7 @@ void controlledMotor::handle(){
//--- apply new target to motor --- //--- apply new target to motor ---
motorSetCommand({state, (float)fabs(dutyNow)}); 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 //note: BRAKE state is handled earlier
@ -247,17 +549,18 @@ void controlledMotor::handle(){
//=============================== //===============================
//function to set the target mode and duty of a motor //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 //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){ void controlledMotor::setTarget(motorCommand_t commandSend){
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);
.state = state_f,
.duty = duty_f
};
ESP_LOGD(TAG, "Inserted command to queue: state=%s, duty=%.2f", motorstateStr[(int)commandSend.state], commandSend.duty);
//send command to queue (overwrite if an old command is still in the queue and not processed) //send command to queue (overwrite if an old command is still in the queue and not processed)
xQueueOverwrite( commandQueue, ( void * )&commandSend); xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 ); //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 =========== //=========== setFade ===========
//=============================== //===============================
//function for editing or enabling the fading/ramp of the motor control //function for editing or enabling the fading/ramp of the motor control
//set/update fading duration/amount //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 //TODO: mutex for msFade variable also used in handle function
switch(fadeType){ switch(fadeType){
case fadeType_t::ACCEL: 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; break;
case fadeType_t::DECEL: 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; break;
} }
} }
@ -324,16 +670,16 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
bool enabled = false; bool enabled = false;
switch(fadeType){ switch(fadeType){
case fadeType_t::ACCEL: case fadeType_t::ACCEL:
if (msFadeAccel == 0){ if (config.msFadeAccel == 0){
msFadeNew = config.msFadeAccel; msFadeNew = configDefault.msFadeAccel;
enabled = true; enabled = true;
} else { } else {
msFadeNew = 0; msFadeNew = 0;
} }
break; break;
case fadeType_t::DECEL: case fadeType_t::DECEL:
if (msFadeDecel == 0){ if (config.msFadeDecel == 0){
msFadeNew = config.msFadeAccel; msFadeNew = configDefault.msFadeAccel;
enabled = true; enabled = true;
} else { } else {
msFadeNew = 0; msFadeNew = 0;
@ -347,3 +693,114 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
return enabled; 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;
}

81
common/motorctl.hpp
View File

@ -7,10 +7,13 @@ extern "C"
#include "freertos/queue.h" #include "freertos/queue.h"
#include "esp_log.h" #include "esp_log.h"
#include "esp_timer.h" #include "esp_timer.h"
#include "nvs_flash.h"
#include "nvs.h"
} }
#include "motordrivers.hpp" #include "motordrivers.hpp"
#include "currentsensor.hpp" #include "currentsensor.hpp"
#include "speedsensor.hpp"
//======================================= //=======================================
@ -21,6 +24,7 @@ extern "C"
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//=================================== //===================================
//====== controlledMotor class ====== //====== controlledMotor class ======
@ -28,49 +32,88 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor { class controlledMotor {
public: public:
//--- functions --- //--- 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 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(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) 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, 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 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 //TODO set current limit method
private: private:
//--- functions --- //--- 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 --- //--- objects ---
//queue for sending commands to the separate task running the handle() function very fast //queue for sending commands to the separate task running the handle() function very fast
QueueHandle_t commandQueue = NULL; QueueHandle_t commandQueue = NULL;
//current sensor //current sensor
currentSensor cSensor; 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) //function pointer that sets motor duty (driver)
motorSetCommandFunc_t motorSetCommand; motorSetCommandFunc_t motorSetCommand;
//--- variables --- //--- variables ---
//TODO add name for logging?
//struct for storing control specific parameters //struct for storing control specific parameters
motorctl_config_t config; motorctl_config_t config;
const motorctl_config_t configDefault; //backup default configuration (unchanged)
bool log = false;
motorstate_t state = motorstate_t::IDLE; 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 currentMax;
float currentNow; float currentNow;
float dutyTarget; //speed mode
float dutyNow; float speedTarget = 0;
float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0;
float dutyNow = 0;
float dutyIncrementAccel; float dutyIncrementAccel;
float dutyIncrementDecel; float dutyIncrementDecel;
float dutyDelta; float dutyDelta;
uint32_t timeoutWaitForCommand = 0;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
uint32_t ramp; uint32_t ramp;
int64_t timestampLastRunUs; int64_t timestampLastRunUs = 0;
bool deadTimeWaiting = false; bool deadTimeWaiting = false;
uint32_t timestampsModeLastActive[4] = {}; uint32_t timestampsModeLastActive[4] = {};
@ -81,4 +124,24 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0; uint32_t timestamp_commandReceived = 0;
bool receiveTimeout = false; 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 );

167
common/speedsensor.cpp
View File

@ -7,6 +7,9 @@
static const char* TAG = "speedSensor"; 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){ uint32_t min(uint32_t a, uint32_t b){
if (a>b) return 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 //determines direction and rotational speed with a speedSensor object
void IRAM_ATTR onEncoderChange(void* arg) { void IRAM_ATTR onEncoderRising(void *arg)
speedSensor* sensor = (speedSensor*)arg; {
speedSensor *sensor = (speedSensor *)arg;
int currentState = gpio_get_level(sensor->config.gpioPin); int currentState = gpio_get_level(sensor->config.gpioPin);
//detect rising edge LOW->HIGH (reached end of gap in encoder disk) // time since last edge in us
if (currentState == 1 && sensor->prevState == 0) { uint32_t currentTime = esp_timer_get_time();
//time since last edge in us uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
uint32_t currentTime = esp_timer_get_time(); sensor->lastEdgeTime = currentTime; // update last edge time
uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
sensor->lastEdgeTime = currentTime; //update last edge time
//store duration of last pulse // store duration of last pulse
sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; sensor->pulseDurations[sensor->pulseCounter] = timeElapsed;
sensor->pulseCounter++; sensor->pulseCounter++;
//check if 3rd pulse has occoured // check if 3rd pulse has occoured (one sequence recorded)
if (sensor->pulseCounter >= 3) { if (sensor->pulseCounter >= 3)
sensor->pulseCounter = 0; //reset counter {
sensor->pulseCounter = 0; // reset count
//simplify variable names // simplify variable names
uint32_t pulse1 = sensor->pulseDurations[0]; uint32_t pulse1 = sensor->pulseDurations[0];
uint32_t pulse2 = sensor->pulseDurations[1]; uint32_t pulse2 = sensor->pulseDurations[1];
uint32_t pulse3 = sensor->pulseDurations[2]; uint32_t pulse3 = sensor->pulseDurations[2];
//find shortest pulse // save all recored pulses of this sequence (for logging only)
uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3)); sensor->pulse1 = pulse1;
sensor->pulse2 = pulse2;
sensor->pulse3 = pulse3;
//Determine direction based on pulse order // find shortest pulse
int directionNew = 0; sensor->shortestPulse = min(pulse1, min(pulse2, pulse3));
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
}
}
//save and invert direction if necessay // ignore this pulse sequence if one pulse is too short (possible noise)
//TODO mutex? if (sensor->shortestPulse < sensor->config.minPulseDurationUs)
if (sensor->config.directionInverted) sensor->direction = -directionNew; {
else sensor->direction = directionNew; sensor->debug_countIgnoredSequencesTooShort++;
return;
//calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
} }
//--- 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){ speedSensor::speedSensor(speedSensor_config_t config_f){
//copy config //copy config
config = config_f; 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 gpio and ISR
//init(); init();
} }
@ -102,15 +121,16 @@ void speedSensor::init() {
gpio_pad_select_gpio(config.gpioPin); gpio_pad_select_gpio(config.gpioPin);
gpio_set_direction(config.gpioPin, GPIO_MODE_INPUT); gpio_set_direction(config.gpioPin, GPIO_MODE_INPUT);
gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY); gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY);
ESP_LOGW(TAG, "%s, configured gpio-pin %d", config.logName, (int)config.gpioPin);
//configure interrupt //configure interrupt
gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE); gpio_set_intr_type(config.gpioPin, GPIO_INTR_POSEDGE);
gpio_install_isr_service(0); if (!isrIsInitialized) {
gpio_isr_handler_add(config.gpioPin, onEncoderChange, this); gpio_install_isr_service(0);
ESP_LOGW(TAG, "%s, configured interrupt", config.logName); isrIsInitialized = true;
ESP_LOGW(TAG, "Initialized ISR service");
isInitialized = true; }
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 //get rotational speed in revolutions per minute
float speedSensor::getRpm(){ float speedSensor::getRpm(){
//check if initialized
if (!isInitialized) init();
uint32_t timeElapsed = esp_timer_get_time() - lastEdgeTime; uint32_t timeElapsed = esp_timer_get_time() - lastEdgeTime;
//timeout (standstill) //timeout (standstill)
//TODO variable timeout considering config.degreePerGroup //TODO variable timeout considering config.degreePerGroup
if ((currentRpm != 0) && (esp_timer_get_time() - lastEdgeTime) > TIMEOUT_NO_ROTATION*1000){ 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); config.logName, TIMEOUT_NO_ROTATION, timeElapsed/1000);
currentRpm = 0; currentRpm = 0;
} }
//debug output (also log variables when this function is called) //debug output (also log variables when this function is called)
ESP_LOGI(TAG, "%s - getRpm: returning stored rpm=%.3f", config.logName, currentRpm); ESP_LOGD(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", ESP_LOGV(TAG, "[%s] rpm=%f, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, totalTooShortCount=%d",
config.logName, config.logName,
currentRpm, currentRpm,
direction, pulseCounter,
pulseCounter, pulse1 / 1000,
(int)pulseDurations[0]/1000, pulse2 / 1000,
(int)pulseDurations[1]/1000, pulse3 / 1000,
(int)pulseDurations[2]/1000, shortestPulse / 1000,
(int)lastEdgeTime); debug_countIgnoredSequencesTooShort);
//return currently stored rpm //return currently stored rpm
return currentRpm; return currentRpm;
} }
//========================== //===========================
//========= getKmph ========= //========= getKmph =========
//========================== //===========================
//get speed in kilometers per hour //get speed in kilometers per hour
float speedSensor::getKmph(){ float speedSensor::getKmph(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000; 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; return currentSpeed;
} }
@ -165,7 +182,7 @@ float speedSensor::getKmph(){
//========================== //==========================
//get speed in meters per second //get speed in meters per second
float speedSensor::getMps(){ float speedSensor::getMps(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter; float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60;
ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); ESP_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
return currentSpeed; return currentSpeed;
} }

26
common/speedsensor.hpp
View File

@ -12,8 +12,9 @@ extern "C" {
typedef struct { typedef struct {
gpio_num_t gpioPin; gpio_num_t gpioPin;
float degreePerGroup; //360 / [count of short,medium,long groups on encoder disk] 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; float tireCircumferenceMeter;
//positive direction is pulse order "short, medium, long" //default positive direction is pulse order "short, medium, long"
bool directionInverted; bool directionInverted;
char* logName; char* logName;
} speedSensor_config_t; } speedSensor_config_t;
@ -24,30 +25,31 @@ class speedSensor {
public: public:
//constructor //constructor
speedSensor(speedSensor_config_t config); speedSensor(speedSensor_config_t config);
//initializes gpio pin and configures interrupt // initializes gpio pin, configures and starts interrupt
void init(); void init();
//negative values = reverse direction //negative values = reverse direction
//positive values = forward direction //positive values = forward direction
float getKmph(); //kilometers per hour float getKmph(); //kilometers per hour
float getMps(); //meters per second float getMps(); //meters per second
float getRpm(); //rotations per minute float getRpm(); //rotations per minute
uint32_t getTimeLastUpdate() {return timeLastUpdate;};
//1=forward, -1=reverse //variables for handling the encoder (public because ISR needs access)
int direction;
//variables for handling the encoder
speedSensor_config_t config; speedSensor_config_t config;
int prevState = 0; uint32_t pulseDurations[3] = {};
uint64_t pulseDurations[3] = {}; uint32_t pulse1, pulse2, pulse3;
uint64_t lastEdgeTime = 0; uint32_t shortestPulse = 0;
uint32_t shortestPulsePrev = 0;
uint32_t lastEdgeTime = 0;
uint8_t pulseCounter = 0; uint8_t pulseCounter = 0;
int debugCount = 0; int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0;
double currentRpm = 0; double currentRpm = 0;
bool isInitialized = false; uint32_t timeLastUpdate = 0;
private: private:
static bool isrIsInitialized; // default false due to static
}; };

8
common/types.hpp
View File

@ -12,6 +12,7 @@ extern "C"
//====== struct/type declarations ====== //====== struct/type declarations ======
//======================================= //=======================================
//global structs and types that need to be available for all boards //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 //struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t { 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 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%) uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
bool currentLimitEnabled; bool currentLimitEnabled;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc; adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent; float currentSensor_ratedCurrent;
float currentMax; float currentMax;
bool currentInverted;
float currentSnapToZeroThreshold;
uint32_t deadTimeMs; //time motor stays in IDLE before direction change uint32_t deadTimeMs; //time motor stays in IDLE before direction change
uint32_t brakePauseBeforeResume;
uint32_t brakeDecel;
} motorctl_config_t; } motorctl_config_t;
//enum fade type (acceleration, deceleration) //enum fade type (acceleration, deceleration)

84
common/wifi.c
View File

@ -21,26 +21,45 @@ static const char *TAG = "wifi";
static esp_event_handler_instance_t instance_any_id; static esp_event_handler_instance_t instance_any_id;
//============================================ //##########################################
//============ init nvs and netif ============ //############ common functions ############
//============================================ //##########################################
//initialize nvs-flash and netif (needed for both AP and CLIENT)
//============================
//========= init nvs =========
//============================
//initialize nvs-flash (needed for both AP and CLIENT)
//has to be run once at startup //has to be run once at startup
void wifi_initNvs_initNetif(){ void wifi_initNvs(){
//Initialize NVS (needed for wifi) //Initialize NVS (needed for wifi)
esp_err_t ret = nvs_flash_init(); esp_err_t err = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
ESP_ERROR_CHECK(nvs_flash_erase()); {
ret = nvs_flash_init(); 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_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default()); ESP_ERROR_CHECK(esp_event_loop_create_default());
} }
//===========================================
//============ init access point ============
//=========================================== //############################################
//############### access point ###############
//############################################
//-------------------------------------------- //--------------------------------------------
//------ configuration / declarations -------- //------ 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(); 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));
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 -------- //------ 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); 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(); s_wifi_event_group = xEventGroupCreate();
sta = esp_netif_create_default_wifi_sta(); sta = esp_netif_create_default_wifi_sta();
@ -249,10 +273,10 @@ void wifi_init_client(void)
//================================= //===============================
//========= deinit client ========= //========= stop client =========
//================================= //===============================
void wifi_deinit_client(void) void wifi_stop_client(void)
{ {
/* The event will not be processed after unregister */ /* 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)); ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));

20
common/wifi.h
View File

@ -3,20 +3,20 @@
//TODO: currently wifi names and passwords are configured in wifi.c -> move this to config? //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) //initialize nvs-flash and netif (needed for both AP and CLIENT)
//has to be run once at startup //both functions have 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();
void wifi_initNvs_initNetif(); void wifi_initNetif();
//function to start an access point //function to start an access point (config in wifi.c)
void wifi_init_ap(void); void wifi_start_ap(void);
//function to disable/deinit access point //function to disable/stop access point
void wifi_deinit_ap(void); void wifi_stop_ap(void);
//function to connect to existing wifi network //function to connect to existing wifi network (config in wifi.c)
void wifi_init_client(void); void wifi_start_client(void);
//function to disable/deinit client //function to disable/deinit client
void wifi_deinit_client(void); void wifi_stop_client(void);

21
components/encoder/.eil.yml Normal file
View File

@ -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

13
components/encoder/CMakeLists.txt Normal file
View File

@ -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}
)

27
components/encoder/Kconfig Normal file
View File

@ -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

26
components/encoder/LICENSE Normal file
View File

@ -0,0 +1,26 @@
Copyright 2019 Ruslan V. Uss <unclerus@gmail.com>
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.

7
components/encoder/component.mk Normal file
View File

@ -0,0 +1,7 @@
COMPONENT_ADD_INCLUDEDIRS = .
ifdef CONFIG_IDF_TARGET_ESP8266
COMPONENT_DEPENDS = esp8266 freertos log
else
COMPONENT_DEPENDS = driver freertos log
endif

250
components/encoder/encoder.c Normal file
View File

@ -0,0 +1,250 @@
/*
* Copyright (c) 2019 Ruslan V. Uss <unclerus@gmail.com>
*
* 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 <unclerus@gmail.com>
*
* BSD Licensed as described in the file LICENSE
*/
#include "encoder.h"
#include <esp_log.h>
#include <string.h>
#include <freertos/semphr.h>
#include <esp_timer.h>
#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;
}

125
components/encoder/encoder.h Normal file
View File

@ -0,0 +1,125 @@
/*
* Copyright (c) 2019 Ruslan V. Uss <unclerus@gmail.com>
*
* 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 <unclerus@gmail.com>
*
* BSD Licensed as described in the file LICENSE
*/
#ifndef __ENCODER_H__
#define __ENCODER_H__
#include <esp_err.h>
#include <driver/gpio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#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__ */

4
components/ssd1306/ssd1306.c
View File

@ -17,8 +17,10 @@ typedef union out_column_t {
uint8_t u8[4]; uint8_t u8[4];
} PACK8 out_column_t; } 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) { if (dev->_address == SPIAddress) {
spi_init(dev, width, height); spi_init(dev, width, height);
} else { } else {

4
components/ssd1306/ssd1306.h
View File

@ -98,6 +98,7 @@ typedef struct {
int _scDirection; int _scDirection;
PAGE_t _page[8]; PAGE_t _page[8];
bool _flip; bool _flip;
int _offsetX; //added offset here instead of using macro variable
} SSD1306_t; } SSD1306_t;
#ifdef __cplusplus #ifdef __cplusplus
@ -105,7 +106,7 @@ extern "C"
{ {
#endif #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_width(SSD1306_t * dev);
int ssd1306_get_height(SSD1306_t * dev); int ssd1306_get_height(SSD1306_t * dev);
int ssd1306_get_pages(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_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_invert(uint8_t *buf, size_t blen);
void ssd1306_flip(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_copy_bit(uint8_t src, int srcBits, uint8_t dst, int dstBits);
uint8_t ssd1306_rotate_byte(uint8_t ch1); uint8_t ssd1306_rotate_byte(uint8_t ch1);
void ssd1306_fadeout(SSD1306_t * dev); void ssd1306_fadeout(SSD1306_t * dev);

3
components/ssd1306/ssd1306_i2c.c
View File

@ -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 (page >= dev->_pages) return;
if (seg >= dev->_width) 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 columLow = _seg & 0x0F;
uint8_t columHigh = (_seg >> 4) & 0x0F; uint8_t columHigh = (_seg >> 4) & 0x0F;

3
components/ssd1306/ssd1306_spi.c
View File

@ -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 (page >= dev->_pages) return;
if (seg >= dev->_width) 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 columLow = _seg & 0x0F;
uint8_t columHigh = (_seg >> 4) & 0x0F; uint8_t columHigh = (_seg >> 4) & 0x0F;

BIN
connection-plan.drawio.pdf
View File

Binary file not shown.

BIN
doc/2023.09.09_armchair-frame.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 354 KiB

BIN
doc/MLX90333-Datasheet_IC-Stick-small.PDF Normal file
View File

Binary file not shown.

BIN
doc/MLX91204-Datasheet_IC-Stick-large.pdf Normal file
View File

Binary file not shown.

BIN
doc/schematic_custom-pcb.pdf Normal file
View File

Binary file not shown.

BIN
function-diagram.drawio.pdf
View File

Binary file not shown.