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
# VS-code
settings.json
# drawio
*.dtmp
*.bkp
# diagrams are mostly temporary (pdf files are tracked)
*.drawio
# React

174
README.md
View File

@ -1,7 +1,61 @@
# Overview
Firmware for a homemade automated electric armchair.
More details about this project:
V1: https://pfusch.zone/electric-armchair
V2: https://pfusch.zone/electric-armchair-v2
Extensive details about this project can be found on the website:
- ~~V1: [Electric Armchair V1](https://pfusch.zone/electric-armchair)~~
- V2: [Electric Armchair V2](https://pfusch.zone/electric-armchair-v2)
Note: In the current version V2.2, only the esp-project in the [board_single/](board_single) folder and the custom libraries in [common/](common) are used.
The projects in the folders `board_control/` and `board_motorctl/` are no longer compatible and legacy from V2.1.
<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
```
### Instal node packages
For the react app packages have to be installed with npm TODO: add this to cmake?
For the react app packages have to be installed using npm. TODO: add this to cmake?
```
cd react-app
npm install
@ -28,11 +82,12 @@ npm install
# Building the Project
## react-webapp
For the webapp to work on the esp32 it has to be built.
When flashing, the folder react-app/build is flashed to siffs (which is used as webroot) onto the esp32.
The following command builds the react webapp and creates this folder
TODO: add this to flash target with cmake?
## React-webapp
When flashing to the ESP32, the files in the `react-app/build/` folder are written to a SPIFFS partition.
These files are then served via HTTP in the Wi-Fi network "armchair" created by the ESP32.
In HTTP control mode, you can control the armchair using a joystick on the provided website.
Initially, or when changing the React code, you need to manually build the React app:
```bash
cd react-app
#compile
@ -42,7 +97,8 @@ rm build/static/js/main.8f9aec76.js.LICENSE.txt
```
Note: Use `npm start` for starting the webapp locally for testing
## esp project
## Firmware
### Set up environment
```bash
source /opt/esp-idf/export.sh
@ -65,84 +121,52 @@ idf.py flash
```
- once "connecting...' was successfully, BOOT button can be released
### Monitor
- connect FTDI programmer to board (VCC to VCC; TX to RX; RX to TX)
- press REST and BOOT button
- release RESET button (keep pressing boot)
- run monitor command:
To view log output for debugging, follow the same steps as in the Upload section, but run:
```bash
idf.py monitor
```
- once connected release BOOT button
- press RESET button once for restart
# Hardware setup
## pcb
Used pcb developed in this project: https://pfusch.zone/project-work-2020
## connection plan
A diagram which shows what components are connected to which terminals of the pcb exists here:
[connection-plan.drawio.pdf](connection-plan.drawio.pdf)
# Usage / User Interface
# Planned Features
- More sensors:
- Accelerometer
- Lidar sensor
- GPS receiver
- Anti slip regulation
- Self driving algorithm
- Lights
- Improved webinterface
- App
## Encoder Functions
**When not in MENU mode**, the button (encoder click) has the following functions:
| Count | Type | Action | Description |
|-------|---------------|----------------------|---------------------------------------------------------------------------------------------|
| 1x long | switch mode | **MENU_MODE_SELECT** | Open menu for selecting the current control mode |
| 1x | control | [MASSAGE] **freeze** input | When in massage mode: lock or unlock joystick input at current position. |
| 1x short, 1x long | switch mode | **ADJUST-CHAIR** | Switch to mode where the armchair leg and backrest are controlled via joystick. |
| 2x | toggle mode | **IDLE** <=> previous| Enable/disable chair armchair (e.g., enable after startup or switch to previous mode after timeout). |
| 3x | switch mode | **JOYSTICK** | Switch to JOYSTICK mode, to control armchair using joystick (default). |
| 4x | switch mode | **HTTP** | Switch to **remote control** via web-app `http://191.168.4.1` in wifi `armchair`. |
| 5x | switch mode | **MENU_SETTINGS** | Open menu to set various options, controlled via display and rotary encoder. |
| 6x | switch mode | **MASSAGE** | Switch to MASSAGE mode where armchair shakes differently, depending on joystick position. |
| 7x | | | |
| 8x | toggle option| **deceleration limit** | Disable/enable deceleration limit (default on) => more responsive. |
| 12x | toggle option| **alt stick mapping** | Toggle between default and alternative stick mapping (reverse direction swapped). |
# Todo
**Add switch functions**
- set loglevel
- define max-speed
**When in MENU_SETTINGS mode** (5x click), the encoder controls the settings menu: (similar in MENU_MODE_SELECT)
| Encoder Event | Current Menu | Action |
|---------------|--------------|--------------------------------------------------------------|
| long press | main-menu | Exit MENU mode to previous control mode (e.g., JOYSTICK). |
| long press | value-select | Exit to main-menu without changing the value. |
| click | main-menu | Select currently highlighted menu item -> enter value-select screen. |
| click | value-select | Confirm value / run action. |
| rotate | main-menu | Scroll through menu items. |
| rotate | value-select | Change value. |
## HTTP Mode
Control the armchair via a virtual joystick on the web interface.
# Usage
## Switch functions
**Currently implemented**
| Count | Type | Action | Description |
| --- | --- | --- | --- |
| 1x | configure | [JOYSTICK] **calibrate stick** | when in joystick mode: set joystick center to current joystick pos |
| 1x | control | [MASSAGE] **freeze** input | when in massage mode: lock or unlock joystick input at current position |
| 2x | toggle mode | **IDLE** <=> previous | enable/disable chair armchair e.g. enable after startup or timeout |
| 3x | switch mode | **JOYSTICK** | switch to default mode JOYSTICK |
| 4x | toggle mode | **HTTP** <=> JOYSTICK | switch to '**remote control** via web-app `http://191.168.4.1`' or back to JOYSTICK mode |
| 5x | | | |
| 6x | toggle mode | **MASSAGE** <=> JOYSTICK | switch to MASSAGE mode or back to JOYSTICK mode |
| 7x | | | |
| 8x | toggle option | **deceleration limit** | disable/enable deceleration limit (default on) => more responsive |
| | | | |
| 12x | toggle option | **alt stick mapping** | toggle between default and alternative stick mapping (reverse swapped) |
| >1s | system | **restart** | Restart the controller when pressing the button longer than 1 second |
| 1x short, 1x long | auto command | **eject** foot support | automatically go forward and reverse for certain time with no acceleration limits, so foot support ejects |
## HTTP mode
Control armchair via virtual joystick on a webinterface.
**Usage**
- Connect to wifi `armchar`, no password
- Access http://192.168.4.1 (note: **http** NOT https, some browsers automatically add https!)
**Current Features**
- Control direction and speed with joystick
**Todo**
- Set parameters
- max duty
- max current
- Control other modes e.g. massage
- Execute preset movement commands
- Change seating position
also see github issue
**Usage:**
- Switch to HTTP mode (4 button presses).
- Connect to WiFi `armchar`, no password.
- Access http://192.168.4.1 (note: **http** NOT https, some browsers automatically add https!).

1
board_control/main/CMakeLists.txt
View File

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

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)
set(EXTRA_COMPONENT_DIRS "../components ../common")
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(
SRCS
"main.cpp"
"config.cpp"
"control.cpp"
"button.cpp"
"fan.cpp"
"auto.cpp"
"display.cpp"
"menu.cpp"
"encoder.cpp"
INCLUDE_DIRS
"."
)

26
board_single/main/auto.cpp
View File

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

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:
//--- methods ---
//constructor
automatedArmchair(void);
automatedArmchair_c(controlledMotor * motorLeft, controlledMotor * motorRight);
//function to generate motor commands
//can be also seen as handle function
//TODO: go with other approach: separate task for handling auto mode
@ -62,6 +62,8 @@ class automatedArmchair {
private:
//--- methods ---
//--- objects ---
controlledMotor * motorLeft;
controlledMotor * motorRight;
//TODO: add buzzer here
//--- variables ---
//queue for storing pending commands
@ -85,3 +87,50 @@ class automatedArmchair {
};
//=========== EXAMPLE USAGE ============
//the following was once used in button.cpp to make move that ejects the leg support of armchair v1
/**
if (trigger){
//define commands
cmds[0] =
{
.motorCmds = {
.left = {motorstate_t::REV, 90},
.right = {motorstate_t::REV, 90}
},
.msDuration = 1200,
.fadeDecel = 800,
.fadeAccel = 1300,
.instruction = auto_instruction_t::NONE
};
cmds[1] =
{
.motorCmds = {
.left = {motorstate_t::FWD, 70},
.right = {motorstate_t::FWD, 70}
},
.msDuration = 70,
.fadeDecel = 0,
.fadeAccel = 300,
.instruction = auto_instruction_t::NONE
};
cmds[2] =
{
.motorCmds = {
.left = {motorstate_t::IDLE, 0},
.right = {motorstate_t::IDLE, 0}
},
.msDuration = 10,
.fadeDecel = 800,
.fadeAccel = 1300,
.instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE
};
//send commands to automatedArmchair_c command queue
armchair.addCommands(cmds, 3);
//change mode to AUTO
control->changeMode(controlMode_t::AUTO);
return;
}
*/

322
board_single/main/button.cpp
View File

@ -8,139 +8,145 @@ extern "C"
}
#include "button.hpp"
#include "encoder.hpp"
#include "display.hpp"
// tag for logging
static const char *TAG = "button";
//tag for logging
static const char * TAG = "button";
//======================================
//============ button task =============
//======================================
// task that handles the button interface/commands
void task_button(void *task_button_parameters)
{
task_button_parameters_t *objects = (task_button_parameters_t *)task_button_parameters;
ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
// create button instance
buttonCommands commandButton(objects->control, objects->joystick, objects->encoderQueue, objects->motorLeft, objects->motorRight, objects->buzzer);
// start handle loop
commandButton.startHandleLoop();
}
//-----------------------------
//-------- constructor --------
//-----------------------------
buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, evaluatedJoystick * joystick_f, controlledArmchair * control_f, buzzer_t * buzzer_f, controlledMotor * motorLeft_f, controlledMotor * motorRight_f){
//copy object pointers
button = button_f;
joystick = joystick_f;
buttonCommands::buttonCommands(
controlledArmchair *control_f,
evaluatedJoystick *joystick_f,
QueueHandle_t encoderQueue_f,
controlledMotor *motorLeft_f,
controlledMotor *motorRight_f,
buzzer_t *buzzer_f)
{
// copy object pointers
control = control_f;
buzzer = buzzer_f;
joystick = joystick_f;
encoderQueue = encoderQueue_f;
motorLeft = motorLeft_f;
motorRight = motorRight_f;
//TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)?
buzzer = buzzer_f;
// TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)?
}
//----------------------------
//--------- action -----------
//----------------------------
//function that runs commands depending on a count value
void buttonCommands::action (uint8_t count, bool lastPressLong){
//--- variable declarations ---
//--- variables ---
bool decelEnabled; //for different beeping when toggling
commandSimple_t cmds[8]; //array for commands for automatedArmchair
commandSimple_t cmds[8]; //array for commands for automatedArmchair_c
//--- get joystick position ---
//in case joystick is used for additional cases:
//joystickData_t stickData = joystick->getData();
//--- actions based on count ---
switch (count){
//no such command
default:
ESP_LOGE(TAG, "no command for count=%d defined", count);
buzzer->beep(3, 400, 100);
break;
//--- run actions based on count ---
switch (count)
{
// ## no command ##
default:
ESP_LOGE(TAG, "no command for count=%d and long=%d defined", count, lastPressLong);
buzzer->beep(3, 200, 100);
break;
case 1:
//restart contoller when 1x long pressed
if (lastPressLong){
ESP_LOGW(TAG, "RESTART");
buzzer->beep(1,1000,1);
vTaskDelay(500 / portTICK_PERIOD_MS);
//esp_restart();
//-> define joystick center or toggle freeze input (executed in control task)
control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed
return;
}
//note: disabled joystick calibration due to accidential trigger
//
// ESP_LOGW(TAG, "cmd %d: sending button event to control task", count);
// //-> define joystick center or toggle freeze input (executed in control task)
// control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed
break;
case 2:
//run automatic commands to lift leg support when pressed 1x short 1x long
if (lastPressLong){
//define commands
cmds[0] =
{
.motorCmds = {
.left = {motorstate_t::REV, 90},
.right = {motorstate_t::REV, 90}
},
.msDuration = 1200,
.fadeDecel = 800,
.fadeAccel = 1300,
.instruction = auto_instruction_t::NONE
};
cmds[1] =
{
.motorCmds = {
.left = {motorstate_t::FWD, 70},
.right = {motorstate_t::FWD, 70}
},
.msDuration = 70,
.fadeDecel = 0,
.fadeAccel = 300,
.instruction = auto_instruction_t::NONE
};
cmds[2] =
{
.motorCmds = {
.left = {motorstate_t::IDLE, 0},
.right = {motorstate_t::IDLE, 0}
},
.msDuration = 10,
.fadeDecel = 800,
.fadeAccel = 1300,
.instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE
};
case 1:
// ## switch to MENU_SETTINGS state ##
if (lastPressLong)
{
ESP_LOGW(TAG, "1x long press -> clear encoder queue and change to mode 'menu mode select'");
buzzer->beep(5, 50, 30);
// clear encoder event queue (prevent menu from exiting immediately due to long press event just happend)
vTaskDelay(200 / portTICK_PERIOD_MS);
//TODO move encoder queue clear to changeMode() method?
rotary_encoder_event_t ev;
while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS);
control->changeMode(controlMode_t::MENU_MODE_SELECT);
}
// ## toggle joystick freeze ##
else if (control->getCurrentMode() == controlMode_t::MASSAGE)
{
control->toggleFreezeInputMassage();
}
// ## define joystick center ##
else
{
// note: disabled joystick calibration due to accidential trigger
//joystick->defineCenter();
}
break;
//send commands to automatedArmchair command queue
armchair.addCommands(cmds, 3);
//change mode to AUTO
control->changeMode(controlMode_t::AUTO);
return;
}
//toggle idle when 2x pressed
case 2:
// ## switch to ADJUST_CHAIR mode ##
if (lastPressLong)
{
ESP_LOGW(TAG, "cmd %d: switch to ADJUST_CHAIR", count);
control->changeMode(controlMode_t::ADJUST_CHAIR);
}
// ## toggle IDLE ##
else
{
ESP_LOGW(TAG, "cmd %d: toggle IDLE", count);
control->toggleIdle(); //toggle between idle and previous/default mode
break;
control->toggleIdle(); // toggle between idle and previous/default mode
}
break;
case 3:
// ## switch to JOYSTICK mode ##
ESP_LOGW(TAG, "cmd %d: switch to JOYSTICK", count);
control->changeMode(controlMode_t::JOYSTICK); //switch to JOYSTICK mode
break;
case 4:
ESP_LOGW(TAG, "cmd %d: toggle between HTTP and JOYSTICK", count);
control->toggleModes(controlMode_t::HTTP, controlMode_t::JOYSTICK); //toggle between HTTP and JOYSTICK mode
// ## switch to HTTP mode ##
ESP_LOGW(TAG, "cmd %d: switch to HTTP", count);
control->changeMode(controlMode_t::HTTP); //switch to HTTP mode
break;
case 5:
// ## switch to MENU_SETTINGS state ##
ESP_LOGW(TAG, "5x press -> clear encoder queue and change to mode 'menu settings'");
buzzer->beep(20, 20, 10);
vTaskDelay(200 / portTICK_PERIOD_MS);
// clear encoder event queue (prevent menu from using previous events)
rotary_encoder_event_t ev;
while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS);
control->changeMode(controlMode_t::MENU_SETTINGS);
break;
case 6:
ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count);
control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode
// ## switch to MASSAGE mode ##
ESP_LOGW(TAG, "switch to MASSAGE");
control->changeMode(controlMode_t::MASSAGE); //switch to MASSAGE mode
break;
case 8:
// ## toggle "sport-mode" ##
//toggle deceleration fading between on and off
//decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL);
//motorRight->toggleFade(fadeType_t::DECEL);
decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL);
decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL); //TODO remove/simplify this using less functions
motorRight->toggleFade(fadeType_t::ACCEL);
ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled);
if (decelEnabled){
@ -151,12 +157,10 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
break;
case 12:
ESP_LOGW(TAG, "cmd %d: sending button event to control task", count);
//-> toggle altStickMapping (executed in control task)
control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1)?
// ## toggle alternative stick mapping ##
control->toggleAltStickMapping();
break;
}
}
}
@ -165,56 +169,78 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
//-----------------------------
//------ startHandleLoop ------
//-----------------------------
//this function has to be started once in a separate task
//repeatedly evaluates and processes button events then takes the corresponding action
void buttonCommands::startHandleLoop() {
// when not in MENU_SETTINGS mode, repeatedly receives events from encoder button
// and takes the corresponding action
// this function has to be started once in a separate task
#define INPUT_TIMEOUT 500 // duration of no button events, after which action is run (implicitly also is 'long-press' time)
void buttonCommands::startHandleLoop()
{
//-- variables --
bool isPressed = false;
static rotary_encoder_event_t event; // store event data
// int count = 0; (from class)
while(1) {
vTaskDelay(20 / portTICK_PERIOD_MS);
//run handle function of evaluatedSwitch object
button->handle();
//--- count button presses and run action ---
switch(state) {
case inputState_t::IDLE: //wait for initial button press
if (button->risingEdge) {
count = 1;
buzzer->beep(1, 65, 0);
timestamp_lastAction = esp_log_timestamp();
state = inputState_t::WAIT_FOR_INPUT;
ESP_LOGI(TAG, "first button press detected -> waiting for further events");
}
break;
case inputState_t::WAIT_FOR_INPUT: //wait for further presses
//button pressed again
if (button->risingEdge){
count++;
buzzer->beep(1, 65, 0);
timestamp_lastAction = esp_log_timestamp();
ESP_LOGI(TAG, "another press detected -> count=%d -> waiting for further events", count);
}
//timeout
else if (esp_log_timestamp() - timestamp_lastAction > 1000) {
state = inputState_t::IDLE;
buzzer->beep(count, 50, 50);
//TODO: add optional "bool wait" parameter to beep function to delay until finished beeping
ESP_LOGI(TAG, "timeout - running action function for count=%d", count);
//--- run action function ---
//check if still pressed
bool lastPressLong = false;
if (button->state == true){
//run special case when last press was longer than timeout
lastPressLong = true;
}
//run action function with current count of button presses
action(count, lastPressLong);
}
break;
while (1)
{
//-- disable functionality when in menu mode --
//(display task uses encoder in that mode)
if (control->getCurrentMode() == controlMode_t::MENU_SETTINGS
|| control->getCurrentMode() == controlMode_t::MENU_MODE_SELECT)
{
//do nothing every loop cycle
ESP_LOGD(TAG, "in MENU_SETTINGS or MENU_MODE_SELECT mode -> button commands disabled");
vTaskDelay(1000 / portTICK_PERIOD_MS);
continue;
}
}
}
//-- get events from encoder --
if (xQueueReceive(encoderQueue, &event, INPUT_TIMEOUT / portTICK_PERIOD_MS))
{
control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (event.type)
{
break;
case RE_ET_BTN_PRESSED:
ESP_LOGD(TAG, "Button pressed");
buzzer->beep(1, 65, 0);
isPressed = true;
count++; // count each pressed event
break;
case RE_ET_BTN_RELEASED:
ESP_LOGD(TAG, "Button released");
isPressed = false; // rest stored state
break;
case RE_ET_CHANGED: // scroll through status pages when simply rotating encoder
if (event.diff > 0)
{
display_rotateStatusPage(true, true); //select NEXT status screen, stau at last element (dont rotate to first)
buzzer->beep(1, 65, 0);
}
else
{
display_rotateStatusPage(false, true); //select PREVIOUS status screen, stay at first element (dont rotate to last)
buzzer->beep(1, 65, 0);
}
break;
case RE_ET_BTN_LONG_PRESSED:
case RE_ET_BTN_CLICKED:
default:
break;
}
}
else // timeout (no event received within TIMEOUT)
{
if (count > 0)
{
//-- run action with count of presses --
ESP_LOGI(TAG, "timeout: count=%d, lastPressLong=%d -> running action", count, isPressed);
buzzer->beep(count, 50, 50, 200); //beep count, with 200ms gap before next queued beeps can start
action(count, isPressed); // run action - if currently still on the last press is considered long
count = 0; // reset count
}
else {
ESP_LOGD(TAG, "no button event received in this cycle (count=0)");
}
} //end queue
} //end while(1)
} //end function

40
board_single/main/button.hpp
View File

@ -5,7 +5,6 @@
#include "control.hpp"
#include "motorctl.hpp"
#include "auto.hpp"
#include "config.hpp"
#include "joystick.hpp"
@ -17,14 +16,13 @@
class buttonCommands {
public:
//--- constructor ---
buttonCommands (
gpio_evaluatedSwitch * button_f,
evaluatedJoystick * joystick_f,
controlledArmchair * control_f,
buzzer_t * buzzer_f,
controlledMotor * motorLeft_f,
controlledMotor * motorRight_f
);
buttonCommands(
controlledArmchair *control_f,
evaluatedJoystick *joystick_f,
QueueHandle_t encoderQueue_f,
controlledMotor * motorLeft_f,
controlledMotor *motorRight_f,
buzzer_t *buzzer_f);
//--- functions ---
//the following function has to be started once in a separate task.
@ -36,12 +34,12 @@ class buttonCommands {
void action(uint8_t count, bool lastPressLong);
//--- objects ---
gpio_evaluatedSwitch* button;
evaluatedJoystick* joystick;
controlledArmchair * control;
buzzer_t* buzzer;
evaluatedJoystick* joystick;
controlledMotor * motorLeft;
controlledMotor * motorRight;
buzzer_t* buzzer;
QueueHandle_t encoderQueue;
//--- variables ---
uint8_t count = 0;
@ -51,3 +49,21 @@ class buttonCommands {
};
//======================================
//============ button task =============
//======================================
// struct with variables passed to task from main
typedef struct task_button_parameters_t
{
controlledArmchair *control;
evaluatedJoystick *joystick;
QueueHandle_t encoderQueue;
controlledMotor *motorLeft;
controlledMotor *motorRight;
buzzer_t *buzzer;
} task_button_parameters_t;
//task that handles the button interface/commands
void task_button( void * task_button_parameters );

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:
//===================================
//======= motor configuration =======
//===================================
extern "C"
{
#include "esp_log.h"
}
#include "motordrivers.hpp"
#include "motorctl.hpp"
#include "joystick.hpp"
#include "http.hpp"
#include "speedsensor.hpp"
#include "buzzer.hpp"
#include "control.hpp"
#include "fan.hpp"
#include "auto.hpp"
#include "chairAdjust.hpp"
#include "display.hpp"
#include "encoder.h"
//==================================
//======== define loglevels ========
//==================================
void setLoglevels(void)
{
// set loglevel for all tags:
esp_log_level_set("*", ESP_LOG_WARN);
//--- set loglevel for individual tags ---
esp_log_level_set("main", ESP_LOG_INFO);
esp_log_level_set("buzzer", ESP_LOG_ERROR);
// esp_log_level_set("motordriver", ESP_LOG_DEBUG);
esp_log_level_set("motor-control", ESP_LOG_WARN);
// esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
esp_log_level_set("joystickCommands", ESP_LOG_WARN);
esp_log_level_set("button", ESP_LOG_INFO);
esp_log_level_set("control", ESP_LOG_INFO);
// esp_log_level_set("fan-control", ESP_LOG_INFO);
esp_log_level_set("wifi", ESP_LOG_INFO);
esp_log_level_set("http", ESP_LOG_INFO);
// esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG);
esp_log_level_set("display", ESP_LOG_INFO);
// esp_log_level_set("current-sensors", ESP_LOG_INFO);
esp_log_level_set("speedSensor", ESP_LOG_WARN);
esp_log_level_set("chair-adjustment", ESP_LOG_INFO);
esp_log_level_set("menu", ESP_LOG_INFO);
esp_log_level_set("encoder", ESP_LOG_INFO);
esp_log_level_set("TESTING", ESP_LOG_ERROR);
}
//==================================
//======== configuration ===========
//==================================
//-----------------------------------
//------- motor configuration -------
//-----------------------------------
/* ==> currently using other driver
//--- configure left motor (hardware) ---
single100a_config_t configDriverLeft = {
@ -11,8 +69,8 @@ single100a_config_t configDriverLeft = {
.gpio_b = GPIO_NUM_4,
.ledc_timer = LEDC_TIMER_0,
.ledc_channel = LEDC_CHANNEL_0,
.aEnabledPinState = false, //-> pins inverted (mosfets)
.bEnabledPinState = false,
.aEnabledPinState = false, //-> pins inverted (mosfets)
.bEnabledPinState = false,
.resolution = LEDC_TIMER_11_BIT,
.pwmFreq = 10000
};
@ -24,176 +82,192 @@ single100a_config_t configDriverRight = {
.gpio_b = GPIO_NUM_14,
.ledc_timer = LEDC_TIMER_1,
.ledc_channel = LEDC_CHANNEL_1,
.aEnabledPinState = false, //-> pin inverted (mosfet)
.bEnabledPinState = true, //-> not inverted (direct)
.aEnabledPinState = false, //-> pin inverted (mosfet)
.bEnabledPinState = true, //-> not inverted (direct)
.resolution = LEDC_TIMER_11_BIT,
.pwmFreq = 10000
};
*/
};
*/
//--- configure sabertooth driver --- (controls both motors in one instance)
sabertooth2x60_config_t sabertoothConfig = {
.gpio_TX = GPIO_NUM_25,
.uart_num = UART_NUM_2
};
.gpio_TX = GPIO_NUM_27,
.uart_num = UART_NUM_2};
//TODO add motor name string -> then use as log tag?
// TODO add motor name string -> then use as log tag?
//--- configure left motor (contol) ---
motorctl_config_t configMotorControlLeft = {
.msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, //GPIO32
.currentSensor_ratedCurrent = 50,
.name = "left",
.loggingEnabled = true,
.msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
.currentSensor_ratedCurrent = 50,
.currentMax = 30,
.deadTimeMs = 0 //minimum time motor is off between direction change
.currentInverted = true,
.currentSnapToZeroThreshold = 0.15,
.deadTimeMs = 0, // minimum time motor is off between direction change
.brakePauseBeforeResume = 1500,
.brakeDecel = 400,
};
//--- configure right motor (contol) ---
motorctl_config_t configMotorControlRight = {
.msFadeAccel = 1500, //acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, //GPIO33
.currentSensor_ratedCurrent = 50,
.name = "right",
.loggingEnabled = false,
.msFadeAccel = 1800, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1600, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
.currentSensor_ratedCurrent = 50,
.currentMax = 30,
.deadTimeMs = 0 //minimum time motor is off between direction change
.currentInverted = false,
.currentSnapToZeroThreshold = 0.25,
.deadTimeMs = 0, // minimum time motor is off between direction change
.brakePauseBeforeResume = 1500,
.brakeDecel = 400,
};
//==============================
//======= control config =======
//==============================
//------------------------------
//------- control config -------
//------------------------------
control_config_t configControl = {
.defaultMode = controlMode_t::JOYSTICK, //default mode after startup and toggling IDLE
//--- timeout ---
.timeoutMs = 3*60*1000, //time of inactivity after which the mode gets switched to IDLE
.timeoutTolerancePer = 5, //percentage the duty can vary between timeout checks considered still inactive
//--- http mode ---
.defaultMode = controlMode_t::JOYSTICK, // default mode after startup and toggling IDLE
//--- timeouts ---
.timeoutSwitchToIdleMs = 5 * 60 * 1000, // time of inactivity after which the mode gets switched to IDLE
.timeoutNotifyPowerStillOnMs = 6 * 60 * 60 * 1000 // time in IDLE after which buzzer beeps in certain interval (notify "forgot to turn off")
};
//===============================
//===== httpJoystick config =====
//===============================
//-------------------------------
//----- httpJoystick config -----
//-------------------------------
httpJoystick_config_t configHttpJoystickMain{
.toleranceZeroX_Per = 1, //percentage around joystick axis the coordinate snaps to 0
.toleranceZeroX_Per = 1, // percentage around joystick axis the coordinate snaps to 0
.toleranceZeroY_Per = 6,
.toleranceEndPer = 2, //percentage before joystick end the coordinate snaps to 1/-1
.timeoutMs = 2500 //time no new data was received before the motors get turned off
.toleranceEndPer = 2, // percentage before joystick end the coordinate snaps to 1/-1
.timeoutMs = 2500 // time no new data was received before the motors get turned off
};
//======================================
//======= joystick configuration =======
//======================================
//--------------------------------------
//------- joystick configuration -------
//--------------------------------------
joystick_config_t configJoystick = {
.adc_x = ADC1_CHANNEL_0, //GPIO36
.adc_y = ADC1_CHANNEL_3, //GPIO39
//percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
.tolerance_zeroX_per = 7, //6
.tolerance_zeroY_per = 10, //7
//percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100)
.tolerance_end_per = 4,
//threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
.adc_x = ADC1_CHANNEL_0, // GPIO36
.adc_y = ADC1_CHANNEL_3, // GPIO39
// percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
.tolerance_zeroX_per = 7, // 6
.tolerance_zeroY_per = 10, // 7
// percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100)
.tolerance_end_per = 4,
// threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
.tolerance_radius = 0.09,
//min and max adc values of each axis, !!!AFTER INVERSION!!! is applied:
// min and max adc values of each axis, !!!AFTER INVERSION!!! is applied:
.x_min = 1710, //=> x=-1
.x_max = 2980, //=> x=1
.y_min = 1700, //=> y=-1
.y_max = 2940, //=> y=1
//invert adc measurement
.x_inverted = true,
.y_inverted = true
};
// invert adc measurement
.x_inverted = false,
.y_inverted = true};
//============================
//=== configure fan contol ===
//============================
fan_config_t configCooling = {
//----------------------------
//--- configure fan contol ---
//----------------------------
fan_config_t configFans = {
.gpio_fan = GPIO_NUM_13,
.dutyThreshold = 40,
.minOnMs = 1500,
.minOffMs = 3000,
.turnOffDelayMs = 5000,
.dutyThreshold = 50,
.minOnMs = 3500, // time motor duty has to be above the threshold for fans to turn on
.minOffMs = 5000, // min time fans have to be off to be able to turn on again
.turnOffDelayMs = 3000, // time fans continue to be on after duty is below threshold
};
//============================================
//======== speed sensor configuration ========
//============================================
//--------------------------------------------
//-------- speed sensor configuration --------
//--------------------------------------------
speedSensor_config_t speedLeft_config{
.gpioPin = GPIO_NUM_5,
.degreePerGroup = 360/5,
.tireCircumferenceMeter = 210.0*3.141/1000.0,
.directionInverted = false,
.logName = "speedLeft",
.gpioPin = GPIO_NUM_5,
.degreePerGroup = 360 / 16,
.minPulseDurationUs = 3000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection
//measured wihth scope while tires in the air:
// 5-groups: 12ms
// 16-groups: 3.7ms
.tireCircumferenceMeter = 0.81,
.directionInverted = true,
.logName = "speedLeft"
};
speedSensor_config_t speedRight_config{
.gpioPin = GPIO_NUM_14,
.degreePerGroup = 360/12,
.tireCircumferenceMeter = 210.0*3.141/1000.0,
.directionInverted = true,
.logName = "speedRight",
.gpioPin = GPIO_NUM_14,
.degreePerGroup = 360 / 12,
.minPulseDurationUs = 4000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection
.tireCircumferenceMeter = 0.81,
.directionInverted = false,
.logName = "speedRight"
};
//=================================
//===== create global objects =====
//=================================
//TODO outsource global variables to e.g. global.cpp and only config options here?
//create sabertooth motor driver instance
sabertooth2x60a sabertoothDriver(sabertoothConfig);
//-------------------------
//-------- display --------
//-------------------------
display_config_t display_config{
// hardware initialization
.gpio_scl = GPIO_NUM_22,
.gpio_sda = GPIO_NUM_23,
.gpio_reset = -1, // negative number disables reset feature
.width = 128,
.height = 64,
.offsetX = 2,
.flip = false,
.contrastNormal = 170, // max: 255
// display task
.contrastReduced = 30, // max: 255
.timeoutReduceContrastMs = 5 * 60 * 1000, // actions at certain inactivity
.timeoutSwitchToScreensaverMs = 30 * 60 * 1000
};
//--- controlledMotor ---
//functions for updating the duty via certain/current driver that can then be passed to controlledMotor
//-> makes it possible to easily use different motor drivers
//note: ignoring warning "capture of variable 'sabertoothDriver' with non-automatic storage duration", since sabertoothDriver object does not get destroyed anywhere - no lifetime issue
motorSetCommandFunc_t setLeftFunc = [&sabertoothDriver](motorCommand_t cmd) {
sabertoothDriver.setLeft(cmd);
//-------------------------
//-------- encoder --------
//-------------------------
//configure rotary encoder (next to joystick)
rotary_encoder_t encoder_config = {
.pin_a = GPIO_NUM_25,
.pin_b = GPIO_NUM_26,
.pin_btn = GPIO_NUM_21,
.code = 1,
.store = 0, //encoder count
.index = 0,
.btn_pressed_time_us = 20000,
.btn_state = RE_BTN_RELEASED //default state
};
motorSetCommandFunc_t setRightFunc = [&sabertoothDriver](motorCommand_t cmd) {
sabertoothDriver.setRight(cmd);
};
//create controlled motor instances (motorctl.hpp)
controlledMotor motorLeft(setLeftFunc, configMotorControlLeft);
controlledMotor motorRight(setRightFunc, configMotorControlRight);
//create speedsensor instances
speedSensor speedLeft (speedLeft_config);
speedSensor speedRight (speedRight_config);
//create global joystic instance (joystick.hpp)
evaluatedJoystick joystick(configJoystick);
//create global evaluated switch instance for button next to joystick
gpio_evaluatedSwitch buttonJoystick(GPIO_NUM_21, true, false); //pullup true, not inverted (switch to GND use pullup of controller)
//create buzzer object on pin 12 with gap between queued events of 100ms
buzzer_t buzzer(GPIO_NUM_12, 100);
//create global httpJoystick object (http.hpp)
httpJoystick httpJoystickMain(configHttpJoystickMain);
//create global control object (control.hpp)
controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &joystick, &httpJoystickMain);
//create global automatedArmchair object (for auto-mode) (auto.hpp)
automatedArmchair armchair;
//-----------------------------------
//--- joystick command generation ---
//-----------------------------------
//configure parameters for motor command generation from joystick data
joystickGenerateCommands_config_t joystickGenerateCommands_config{
//-- maxDuty --
// max duty when both motors are at equal ratio e.g. driving straight forward
// better to be set less than 100% to have some reserve for boosting the outer tire when turning
.maxDutyStraight = 65,
//-- maxBoost --
// boost is amount of duty added to maxDutyStraight to outer tire while turning
// => turning: inner tire gets slower, outer tire gets faster
// 0: boost = 0 (disabled)
// 100: boost = maxDutyStraight (e.g. when maxDuty is 50, outer motor can still reach 100 (50+50))
.maxRelativeBoostPercentOfMaxDuty = 60,
// 60: when maxDuty is set above 62% (equals 0.6*62 = 38% boost) the outer tire can still reach 100% - below 62 maxDuty the boosted speed is also reduced.
// => setting this value lower prevents desired low max duty configuration from being way to fast in curves.
.dutyOffset = 5, // duty at which motors start immediately
.ratioSnapToOneThreshold = 0.9, // threshold ratio snaps to 1 to have some area of max turning before entering X-Axis-full-rotate mode
.altStickMapping = false // invert reverse direction
};

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 "config.hpp"
#include "config.h"
#include "control.hpp"
#include "chairAdjust.hpp"
#include "display.hpp" // needed for getBatteryPercent()
//used definitions moved from config.hpp:
//#define JOYSTICK_TEST
//used definitions moved from config.h:
//#define JOYSTICK_LOG_IN_IDLE
//tag for logging
static const char * TAG = "control";
const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO"};
static const char * ERROR_STR = "ERR";
const char* controlModeStr[10] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO", "ADJUST_CHAIR", "MENU_SETTINGS", "MENU_MODE_SELECT"};
const uint8_t controlModeMaxCount = sizeof(controlModeStr) / sizeof(char *);
#define MUTEX_TIMEOUT 10000 // restart when stuck waiting for handle() mutex
//==========================
//==== controlModeToStr ====
//==========================
// convert controlMode enum or mode index to string for logging, returns "ERR" when index is out of range of existing modes
const char * controlModeToStr(int modeIndex){
// return string when in allowed range
if (modeIndex >= 0 && modeIndex < controlModeMaxCount)
return controlModeStr[modeIndex];
else
// log and return error when not in range
ESP_LOGE(TAG, "controlModeToStr: mode index '%d' is not in valid range - max 0-%d", modeIndex, controlModeMaxCount);
return ERROR_STR;
}
const char * controlModeToStr(controlMode_t mode){
return controlModeToStr((int)mode);
}
//-----------------------------
//-------- constructor --------
//-----------------------------
controlledArmchair::controlledArmchair (
control_config_t config_f,
buzzer_t * buzzer_f,
controlledMotor* motorLeft_f,
controlledMotor* motorRight_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
){
controlledArmchair::controlledArmchair(
control_config_t config_f,
buzzer_t *buzzer_f,
controlledMotor *motorLeft_f,
controlledMotor *motorRight_f,
evaluatedJoystick *joystick_f,
joystickGenerateCommands_config_t *joystickGenerateCommands_config_f,
httpJoystick *httpJoystick_f,
automatedArmchair_c *automatedArmchair_f,
cControlledRest *legRest_f,
cControlledRest *backRest_f,
nvs_handle_t * nvsHandle_f)
{
//copy configuration
config = config_f;
joystickGenerateCommands_config = *joystickGenerateCommands_config_f;
//copy object pointers
buzzer = buzzer_f;
motorLeft = motorLeft_f;
motorRight = motorRight_f;
joystick_l = joystick_f,
httpJoystickMain_l = httpJoystick_f;
automatedArmchair = automatedArmchair_f;
legRest = legRest_f;
backRest = backRest_f;
nvsHandle = nvsHandle_f;
//set default mode from config
modePrevious = config.defaultMode;
//TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)?
// override default config value if maxDuty is found in nvs
loadMaxDuty();
// update brake start threshold with actual max duty for motorctl
ESP_LOGW(TAG, "setting brake start threshold for both motors to %.0f", joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();
}
//=======================================
//============ control task =============
//=======================================
// task that controls the armchair modes
// generates commands depending on current mode and sends those to corresponding task
// parameter: pointer to controlledArmchair object
void task_control( void * pvParameters ){
controlledArmchair * control = (controlledArmchair *)pvParameters;
ESP_LOGW(TAG, "Initializing controlledArmchair and starting handle loop");
control->startHandleLoop();
}
@ -53,177 +108,269 @@ controlledArmchair::controlledArmchair (
//----------------------------------
//---------- Handle loop -----------
//----------------------------------
//function that repeatedly generates motor commands depending on the current mode
//also handles fading and current-limit
void controlledArmchair::startHandleLoop() {
while (1){
ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]);
// start endless loop that repeatedly calls handle() and handleTimeout() methods
void controlledArmchair::startHandleLoop()
{
while (1)
{
// mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed
if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE)
{
//--- handle current mode ---
ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]);
handle();
switch(mode) {
default:
mode = controlMode_t::IDLE;
break;
case controlMode_t::IDLE:
//copy preset commands for idling both motors
commands = cmds_bothMotorsIdle;
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(200 / portTICK_PERIOD_MS);
#ifdef JOYSTICK_LOG_IN_IDLE
//get joystick data here (without using it)
//since loglevel is DEBUG, calculateion details is output
joystick_l->getData(); //get joystick data here
#endif
break;
case controlMode_t::JOYSTICK:
vTaskDelay(20 / portTICK_PERIOD_MS);
//get current joystick data with getData method of evaluatedJoystick
stickData = joystick_l->getData();
//additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
//generate motor commands
commands = joystick_generateCommandsDriving(stickData, altStickMapping);
//apply motor commands
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
//TODO make motorctl.setTarget also accept motorcommand struct directly
break;
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
//only update joystick data when input not frozen
if (!freezeInput){
stickData = joystick_l->getData();
}
//--- generate motor commands ---
//pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData);
//apply motor commands
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
break;
case controlMode_t::HTTP:
//--- get joystick data from queue ---
//Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned
//TODO: as described above, when changing modes it might delay a few seconds for the change to apply
stickData = httpJoystickMain_l->getData();
//scale coordinates additionally (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
//--- generate motor commands ---
//Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, altStickMapping);
//--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
break;
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
//generate commands
commands = armchair.generateCommands(&instruction);
//--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
//process received instruction
switch (instruction) {
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
toggleMode(controlMode_t::AUTO);
break;
case auto_instruction_t::SWITCH_JOYSTICK_MODE:
changeMode(controlMode_t::JOYSTICK);
break;
case auto_instruction_t::RESET_ACCEL_DECEL:
//enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_ACCEL:
//set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_DECEL:
//enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
break;
}
break;
//TODO: add other modes here
xSemaphoreGive(handleIteration_mutex);
} // end mutex
else {
ESP_LOGE(TAG, "mutex timeout - stuck in changeMode? -> RESTART");
esp_restart();
}
//--- run actions based on received button button event ---
//note: buttonCount received by sendButtonEvent method called from button.cpp
//TODO: what if variable gets set from other task during this code? -> mutex around this code
switch (buttonCount) {
case 1: //define joystick center or freeze input
if (mode == controlMode_t::JOYSTICK){
//joystick mode: calibrate joystick
joystick_l->defineCenter();
} else if (mode == controlMode_t::MASSAGE){
//massage mode: toggle freeze of input (lock joystick at current values)
freezeInput = !freezeInput;
if (freezeInput){
buzzer->beep(5, 40, 25);
} else {
buzzer->beep(1, 300, 100);
}
}
break;
case 12: //toggle alternative joystick mapping (reverse swapped)
altStickMapping = !altStickMapping;
if (altStickMapping){
buzzer->beep(6, 70, 50);
} else {
buzzer->beep(1, 500, 100);
}
break;
}
//--- reset button event --- (only one action per run)
if (buttonCount > 0){
ESP_LOGI(TAG, "resetting button event/count");
buttonCount = 0;
}
//-----------------------
//------ slow loop ------
//-----------------------
//this section is run about every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
//--- slow loop ---
// this section is run approx every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
{
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
timestamp_SlowLoopLastRun = esp_log_timestamp();
//run function which detects timeout (switch to idle)
//--- handle timeouts ---
// run function that detects timeouts (switch to idle, or notify "forgot to turn off")
handleTimeout();
}
vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex
// TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock
}
}
}//end while(1)
}//end startHandleLoop
//-------------------------------------
//---------- Handle control -----------
//-------------------------------------
// function that repeatedly generates motor commands and runs actions depending on the current mode
void controlledArmchair::handle()
{
switch (mode)
{
default:
//switch to IDLE mode when current mode is not implemented
changeMode(controlMode_t::IDLE);
break;
//------- handle IDLE -------
case controlMode_t::IDLE:
vTaskDelay(500 / portTICK_PERIOD_MS);
// TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass
#ifdef JOYSTICK_LOG_IN_IDLE
// get joystick data and log it
joystickData_t data joystick_l->getData();
ESP_LOGI("JOYSTICK_LOG_IN_IDLE", "x=%.3f, y=%.3f, radius=%.3f, angle=%.3f, pos=%s, adcx=%d, adcy=%d",
data.x, data.y, data.radius, data.angle,
joystickPosStr[(int)data.position],
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
vTaskDelay(50 / portTICK_PERIOD_MS);
// get current joystick data with getData method of evaluatedJoystick
stickDataLast = stickData;
stickData = joystick_l->getData();
// additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.7, 0.45); // TODO: add scaling parameters to config
// generate motor commands
// only generate when the stick data actually changed (e.g. stick stayed in center)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGV(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
// only update joystick data when input not frozen
stickDataLast = stickData;
if (!freezeInput)
stickData = joystick_l->getData();
// reset timeout when joystick data changed
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
resetTimeout(); // user input -> reset switch to IDLE timeout
//--- generate motor commands ---
// pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
break;
//------- handle HTTP mode -------
case controlMode_t::HTTP:
//--- get joystick data from queue ---
stickDataLast = stickData;
stickData = httpJoystickMain_l->getData(); // get last stored data from receive queue (waits up to 500ms for new event to arrive)
// scale coordinates additionally (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); // TODO: add scaling parameters to config
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
//--- generate motor commands ---
// only generate when the stick data actually changed (e.g. no new data recevied via http)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
// generate commands
commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
// process received instruction
switch (instruction)
{
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
toggleMode(controlMode_t::AUTO);
break;
case auto_instruction_t::SWITCH_JOYSTICK_MODE:
changeMode(controlMode_t::JOYSTICK);
break;
case auto_instruction_t::RESET_ACCEL_DECEL:
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
// set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_ACCEL:
// set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_DECEL:
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
break;
}
break;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
//--- read joystick ---
stickDataLast = stickData;
stickData = joystick_l->getData();
//--- control armchair position with joystick input ---
// dont update when stick data did not change
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
controlChairAdjustment(joystick_l->getData(), legRest, backRest);
}
break;
//------- handle MENU modes -------
case controlMode_t::MENU_SETTINGS:
case controlMode_t::MENU_MODE_SELECT:
// nothing to do here, display task handles the menu
vTaskDelay(500 / portTICK_PERIOD_MS);
break;
// TODO: add other modes here
}
} // end - handle method
//---------------------------------------
//------ toggleFreezeInputMassage -------
//---------------------------------------
// releases or locks joystick in place when in massage mode
bool controlledArmchair::toggleFreezeInputMassage()
{
if (mode == controlMode_t::MASSAGE)
{
// massage mode: toggle freeze of input (lock joystick at current values)
freezeInput = !freezeInput;
if (freezeInput)
{
buzzer->beep(5, 40, 25);
ESP_LOGW(TAG, "joystick input is now locked in place");
}
else
{
buzzer->beep(1, 300, 100);
ESP_LOGW(TAG, "joystick input gets updated again");
}
return freezeInput;
}
else
{
ESP_LOGE(TAG, "can not freeze/unfreeze joystick input - not in MASSAGE mode!");
return 0;
}
}
//-------------------------------------
//------- toggleAltStickMapping -------
//-------------------------------------
// toggle between normal and alternative stick mapping (joystick reverse position inverted)
bool controlledArmchair::toggleAltStickMapping()
{
joystickGenerateCommands_config.altStickMapping = !joystickGenerateCommands_config.altStickMapping;
if (joystickGenerateCommands_config.altStickMapping)
{
buzzer->beep(6, 70, 50);
ESP_LOGW(TAG, "changed to alternative stick mapping");
}
else
{
buzzer->beep(1, 500, 100);
ESP_LOGW(TAG, "changed to default stick mapping");
}
return joystickGenerateCommands_config.altStickMapping;
}
//-----------------------------------
//--------- idleBothMotors ----------
//-----------------------------------
// turn both motors off
void controlledArmchair::idleBothMotors(){
motorRight->setTarget(cmd_motorIdle);
motorLeft->setTarget(cmd_motorIdle);
}
//-----------------------------------
@ -232,64 +379,51 @@ void controlledArmchair::startHandleLoop() {
void controlledArmchair::resetTimeout(){
//TODO mutex
timestamp_lastActivity = esp_log_timestamp();
ESP_LOGV(TAG, "timeout: activity detected, resetting timeout");
}
//------------------------------------
//--------- sendButtonEvent ----------
//------------------------------------
void controlledArmchair::sendButtonEvent(uint8_t count){
//TODO mutex - if not replaced with queue
ESP_LOGI(TAG, "setting button event");
buttonCount = count;
}
//------------------------------------
//---------- handleTimeout -----------
//------------------------------------
//percentage the duty can vary since last timeout check and still counts as incative
//TODO: add this to config
float inactivityTolerance = 10;
// switch to IDLE when no activity (prevent accidential movement)
// notify "power still on" when in IDLE for a very long time (prevent battery drain when forgotten to turn off)
// this function has to be run repeatedly (can be slow interval)
#define TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS 5 * 60 * 1000 // beep every 5 minutes for someone to notice
#define TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT 96 // only notify/beep when below certain percentage (prevent beeping when connected to charger)
// note: timeout durations are configured in config.cpp
void controlledArmchair::handleTimeout()
{
uint32_t noActivityDurationMs = esp_log_timestamp() - timestamp_lastActivity;
// log current inactivity and configured timeouts
ESP_LOGD(TAG, "timeout check: last activity %dmin and %ds ago - timeout IDLE after %ds - notify after power on after %dh",
noActivityDurationMs / 1000 / 60,
noActivityDurationMs / 1000 % 60,
config.timeoutSwitchToIdleMs / 1000,
config.timeoutNotifyPowerStillOnMs / 1000 / 60 / 60);
//local function that checks whether two values differ more than a given tolerance
bool validateActivity(float dutyOld, float dutyNow, float tolerance){
float dutyDelta = dutyNow - dutyOld;
if (fabs(dutyDelta) < tolerance) {
return false; //no significant activity detected
} else {
return true; //there was activity
// -- timeout switch to IDLE --
// timeout to IDLE when not idling already
if (mode != controlMode_t::IDLE && noActivityDurationMs > config.timeoutSwitchToIdleMs)
{
ESP_LOGW(TAG, "timeout check: [TIMEOUT], no activity for more than %ds -> switch to IDLE", config.timeoutSwitchToIdleMs / 1000);
changeMode(controlMode_t::IDLE);
//TODO switch to previous status-screen when activity detected
}
}
//function that evaluates whether there is no activity/change on the motor duty for a certain time. If so, a switch to IDLE is issued. - has to be run repeatedly in a slow interval
void controlledArmchair::handleTimeout(){
//check for timeout only when not idling already
if (mode != controlMode_t::IDLE) {
//get current duty from controlled motor objects
float dutyLeftNow = motorLeft->getStatus().duty;
float dutyRightNow = motorRight->getStatus().duty;
//activity detected on any of the two motors
if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance)
|| validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance)
){
ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset");
//reset last duty and timestamp
dutyLeft_lastActivity = dutyLeftNow;
dutyRight_lastActivity = dutyRightNow;
resetTimeout();
}
//no activity on any motor and msTimeout exceeded
else if (esp_log_timestamp() - timestamp_lastActivity > config.timeoutMs){
ESP_LOGI(TAG, "timeout check: [TIMEOUT], no activity for more than %.ds -> switch to idle", config.timeoutMs/1000);
//toggle to idle mode
toggleIdle();
}
else {
ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f s ago, timeout after %d s", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000, config.timeoutMs/1000);
// -- timeout notify "forgot to turn off" --
// repeatedly notify via buzzer when in IDLE for a very long time to prevent battery drain ("forgot to turn off")
// also battery charge-level has to be below certain threshold to prevent beeping in case connected to charger
// note: ignores user input while in IDLE (e.g. encoder rotation)
else if ((esp_log_timestamp() - timestamp_lastModeChange) > config.timeoutNotifyPowerStillOnMs && getBatteryPercent() < TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT)
{
// beep in certain intervals
if ((esp_log_timestamp() - timestamp_lastTimeoutBeep) > TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS)
{
ESP_LOGW(TAG, "timeout: [TIMEOUT] in IDLE since %.3f hours -> beeping", (float)(esp_log_timestamp() - timestamp_lastModeChange) / 1000 / 60 / 60);
// TODO dont beep at certain time ranges (e.g. at night)
timestamp_lastTimeoutBeep = esp_log_timestamp();
buzzer->beep(6, 100, 50);
}
}
}
@ -300,130 +434,143 @@ void controlledArmchair::handleTimeout(){
//----------- changeMode ------------
//-----------------------------------
//function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) {
//reset timeout timer
resetTimeout();
void controlledArmchair::changeMode(controlMode_t modeNew)
{
// variable to store configured accel limit before entering massage mode, to restore it later
static uint32_t massagePreviousAccel = motorLeft->getFade(fadeType_t::ACCEL);
static uint32_t massagePreviousDecel = motorLeft->getFade(fadeType_t::DECEL);
//exit if target mode is already active
if (mode == modeNew) {
// exit if target mode is already active
if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return;
}
//copy previous mode
modePrevious = mode;
// mutex to wait for current handle iteration (control-task) to finish
// prevents race conditions where operations when changing mode are run but old mode gets handled still
ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE)
{
// copy previous mode
modePrevious = mode;
// store time changed (needed for timeout)
timestamp_lastModeChange = esp_log_timestamp();
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//========== commands change FROM mode ==========
//run functions when changing FROM certain mode
switch(modePrevious){
default:
ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
//========== commands change FROM mode ==========
// run functions when changing FROM certain mode
switch (modePrevious)
{
default:
ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
case controlMode_t::IDLE:
#ifdef JOYSTICK_LOG_IN_IDLE
case controlMode_t::IDLE:
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config
break;
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); // FIXME: loglevel from config
#endif
buzzer->beep(1, 200, 100);
break;
case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi");
//stop http server
ESP_LOGI(TAG, "disabling http server...");
http_stop_server();
//FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
//stop wifi
//TODO: decide whether ap or client is currently used - which has to be disabled?
//ESP_LOGI(TAG, "deinit wifi...");
//wifi_deinit_client();
//wifi_deinit_ap();
ESP_LOGI(TAG, "done stopping http mode");
case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap");
wifi_stop_ap();
break;
case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input");
//TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
//enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
//reset frozen input state
// TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, massagePreviousDecel);
motorRight->setFade(fadeType_t::DECEL, massagePreviousDecel);
// restore previously set acceleration limit
motorLeft->setFade(fadeType_t::ACCEL, massagePreviousAccel);
motorRight->setFade(fadeType_t::ACCEL, massagePreviousAccel);
// reset frozen input state
freezeInput = false;
break;
case controlMode_t::AUTO:
ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default");
//TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
//enable downfading (set to default value)
// TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value
// set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
}
case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors...");
// prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving
legRest->setState(REST_OFF);
backRest->setState(REST_OFF);
break;
}
//========== commands change TO mode ==========
//run functions when changing TO certain mode
switch(modeNew){
//========== commands change TO mode ==========
// run functions when changing TO certain mode
switch (modeNew)
{
default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break;
case controlMode_t::IDLE:
buzzer->beep(1, 1500, 0);
#ifdef JOYSTICK_LOG_IN_IDLE
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
#endif
break;
case controlMode_t::IDLE:
ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep");
idleBothMotors();
buzzer->beep(1, 900, 0);
break;
case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching to http mode -> enabling http and wifi");
//start wifi
//TODO: decide wether ap or client should be started
ESP_LOGI(TAG, "init wifi...");
ESP_LOGW(TAG, "switching to HTTP mode -> starting wifi-ap");
wifi_start_ap();
break;
//FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
//wifi_init_client();
//wifi_init_ap();
case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode: turning both motors off, beep");
idleBothMotors();
buzzer->beep(3, 100, 50);
break;
//wait for wifi
//ESP_LOGI(TAG, "waiting for wifi...");
//vTaskDelay(1000 / portTICK_PERIOD_MS);
//start http server
ESP_LOGI(TAG, "init http server...");
http_init_server();
ESP_LOGI(TAG, "done initializing http mode");
case controlMode_t::MENU_SETTINGS:
idleBothMotors();
break;
case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
uint32_t shake_msFadeAccel = 500; //TODO: move this to config
uint32_t shake_msFadeAccel = 350; // TODO: move this to config
uint32_t shake_msFadeDecel = 0; // TODO: move this to config
//disable downfading (max. deceleration)
motorLeft->setFade(fadeType_t::DECEL, false);
motorRight->setFade(fadeType_t::DECEL, false);
//reduce upfading (increase acceleration)
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
// save currently set normal acceleration config (for restore when leavinge MASSAGE again)
massagePreviousAccel = motorLeft->getFade(fadeType_t::ACCEL);
massagePreviousDecel = motorLeft->getFade(fadeType_t::DECEL);
// disable downfading (max. deceleration)
motorLeft->setFade(fadeType_t::DECEL, shake_msFadeDecel, false);
motorRight->setFade(fadeType_t::DECEL, shake_msFadeDecel, false);
// reduce upfading (increase acceleration) but do not update nvs
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel, false);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel, false);
break;
}
//--- update mode to new mode ---
mode = modeNew;
// unlock mutex for control task to continue handling modes
xSemaphoreGive(handleIteration_mutex);
} // end mutex
else
{
ESP_LOGE(TAG, "mutex timeout - stuck in handle() loop? -> RESTART");
esp_restart();
}
//--- update mode to new mode ---
//TODO: add mutex
mode = modeNew;
}
//TODO simplify the following 3 functions? can be replaced by one?
//-----------------------------------
@ -445,13 +592,13 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
//switch to secondary mode when primary is already active
if (mode == modePrimary){
ESP_LOGW(TAG, "toggleModes: switching from primaryMode %s to secondarMode %s", controlModeStr[(int)mode], controlModeStr[(int)modeSecondary]);
buzzer->beep(2,200,100);
//buzzer->beep(2,200,100);
changeMode(modeSecondary); //switch to secondary mode
}
//switch to primary mode when any other mode is active
else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
buzzer->beep(4,200,100);
ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100);
changeMode(modePrimary);
}
}
@ -466,14 +613,67 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active
if (mode == modePrimary){
ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
ESP_LOGW(TAG, "toggleMode: switching from primaryMode '%s' to previousMode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
//buzzer->beep(2,200,100);
changeMode(modePrevious); //switch to previous mode
}
//switch to primary mode when any other mode is active
else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100);
changeMode(modePrimary);
}
}
//-----------------------------
//-------- loadMaxDuty --------
//-----------------------------
// update local config value when maxDuty is stored in nvs
void controlledArmchair::loadMaxDuty(void)
{
// default value is already loaded (constructor)
// read from nvs
uint16_t valueRead;
esp_err_t err = nvs_get_u16(*nvsHandle, "c-maxDuty", &valueRead);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, valueRead/100.0);
joystickGenerateCommands_config.maxDutyStraight = (float)(valueRead/100.0);
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
}
}
//-----------------------------------
//---------- writeMaxDuty -----------
//-----------------------------------
// write provided value to nvs to be persistent and update local variable in joystickGenerateCommmands_config struct
// note: duty percentage gets stored as uint with factor 100 (to get more precision)
void controlledArmchair::writeMaxDuty(float newValue){
// check if unchanged
if(joystickGenerateCommands_config.maxDutyStraight == newValue){
ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue);
return;
}
// update nvs value
ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, newValue) ;
esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100));
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
joystickGenerateCommands_config.maxDutyStraight = newValue;
}

127
board_single/main/control.hpp
View File

@ -1,30 +1,55 @@
#pragma once
extern "C"
{
#include "nvs_flash.h"
#include "nvs.h"
}
#include "motordrivers.hpp"
#include "motorctl.hpp"
#include "buzzer.hpp"
#include "http.hpp"
#include "auto.hpp"
#include "speedsensor.hpp"
#include "chairAdjust.hpp"
//percentage stick has to be moved in the opposite driving direction of current motor direction for braking to start
#define BRAKE_START_STICK_PERCENTAGE 95
//--------------------------------------------
//---- struct, enum, variable declarations ---
//--------------------------------------------
//enum that decides how the motors get controlled
enum class controlMode_t {IDLE, JOYSTICK, MASSAGE, HTTP, MQTT, BLUETOOTH, AUTO};
enum class controlMode_t {IDLE, JOYSTICK, MASSAGE, HTTP, MQTT, BLUETOOTH, AUTO, ADJUST_CHAIR, MENU_SETTINGS, MENU_MODE_SELECT};
//string array representing the mode enum (for printing the state as string)
extern const char* controlModeStr[7];
extern const char* controlModeStr[10];
extern const uint8_t controlModeMaxCount;
//--- control_config_t ---
//struct with config parameters
typedef struct control_config_t {
controlMode_t defaultMode; //default mode after startup and toggling IDLE
//timeout options
uint32_t timeoutMs; //time of inactivity after which the mode gets switched to IDLE
float timeoutTolerancePer; //percentage the duty can vary between timeout checks considered still inactive
uint32_t timeoutSwitchToIdleMs; //time of inactivity after which the mode gets switched to IDLE
uint32_t timeoutNotifyPowerStillOnMs;
} control_config_t;
//==========================
//==== controlModeToStr ====
//==========================
// convert controlMode enum or index to string for logging
const char * controlModeToStr(controlMode_t mode);
const char * controlModeToStr(int modeIndex);
//=======================================
//============ control task =============
//=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver
//parameter: pointer to controlledArmchair object
void task_control( void * controlledArmchair );
//==================================
@ -41,11 +66,16 @@ class controlledArmchair {
controlledMotor* motorLeft_f,
controlledMotor* motorRight_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
joystickGenerateCommands_config_t* joystickGenerateCommands_config_f,
httpJoystick* httpJoystick_f,
automatedArmchair_c* automatedArmchair,
cControlledRest * legRest,
cControlledRest * backRest,
nvs_handle_t * nvsHandle_f
);
//--- functions ---
//task that repeatedly generates motor commands depending on the current mode
//endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex
void startHandleLoop();
//function that changes to a specified control mode
@ -63,32 +93,87 @@ class controlledArmchair {
//function that restarts timer which initiates the automatic timeout (switch to IDLE) after certain time of inactivity
void resetTimeout();
//function for sending a button event (e.g. from button task at event) to control task
//TODO: use queue instead?
void sendButtonEvent(uint8_t count);
//methods to get the current or previous control mode
controlMode_t getCurrentMode() const {return mode;};
controlMode_t getPreviousMode() const {return modePrevious;};
const char *getCurrentModeStr() const { return controlModeStr[(int)mode]; };
//--- mode specific ---
// releases or locks joystick in place when in massage mode, returns true when input is frozen
bool toggleFreezeInputMassage();
// toggle between normal and alternative stick mapping (joystick reverse position inverted), returns true when alt mapping is active
bool toggleAltStickMapping();
// configure max dutycycle (in joystick or http mode)
void setMaxDuty(float maxDutyNew) {
writeMaxDuty(maxDutyNew);
motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100);
motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100);
};
float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; };
// configure max boost (in joystick or http mode)
void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; };
float getMaxRelativeBoostPer() const {return joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty; };
uint32_t getInactivityDurationMs() {return esp_log_timestamp() - timestamp_lastActivity;};
private:
//--- functions ---
//generate motor commands or run actions depending on the current mode
void handle();
//function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
void handleTimeout();
void loadMaxDuty(); //load stored value for maxDuty from nvs
void writeMaxDuty(float newMaxDuty); //write new value for maxDuty to nvs
void idleBothMotors(); //turn both motors off
//--- objects ---
buzzer_t* buzzer;
controlledMotor* motorLeft;
controlledMotor* motorRight;
httpJoystick* httpJoystickMain_l;
evaluatedJoystick* joystick_l;
joystickGenerateCommands_config_t joystickGenerateCommands_config;
automatedArmchair_c *automatedArmchair;
cControlledRest * legRest;
cControlledRest * backRest;
//handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle;
//--- constants ---
//command preset for idling motors
const motorCommand_t cmd_motorIdle = {
.state = motorstate_t::IDLE,
.duty = 0
};
const motorCommands_t cmds_bothMotorsIdle = {
.left = cmd_motorIdle,
.right = cmd_motorIdle
};
const joystickData_t joystickData_center = {
.position = joystickPos_t::CENTER,
.x = 0,
.y = 0,
.radius = 0,
.angle = 0
};
//---variables ---
//struct for motor commands returned by generate functions of each mode
motorCommands_t commands;
motorCommands_t commands = cmds_bothMotorsIdle;
//struct with config parameters
control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data
joystickData_t stickData;
bool altStickMapping; //alternative joystick mapping (reverse mapped differently)
joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center;
//variables for http mode
uint32_t http_timestamp_lastData = 0;
@ -97,7 +182,7 @@ class controlledArmchair {
bool freezeInput = false;
//variables for AUTO mode
auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair
auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair_c
//variable to store button event
uint8_t buttonCount = 0;
@ -108,23 +193,13 @@ class controlledArmchair {
//variable to store mode when toggling IDLE mode
controlMode_t modePrevious; //default mode
//command preset for idling motors
const motorCommand_t cmd_motorIdle = {
.state = motorstate_t::IDLE,
.duty = 0
};
const motorCommands_t cmds_bothMotorsIdle = {
.left = cmd_motorIdle,
.right = cmd_motorIdle
};
//variable for slow loop
uint32_t timestamp_SlowLoopLastRun = 0;
//variables for detecting timeout (switch to idle, after inactivity)
float dutyLeft_lastActivity = 0;
float dutyRight_lastActivity = 0;
//variables for detecting timeout (switch to idle, or notify "forgot to turn off" after inactivity
uint32_t timestamp_lastModeChange = 0;
uint32_t timestamp_lastActivity = 0;
uint32_t timestamp_lastTimeoutBeep = 0;
};

715
board_single/main/display.cpp
View File

@ -1,30 +1,27 @@
#include "display.hpp"
extern "C"{
#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 BAT_CELL_COUNT 7
// continously vary display contrast from 0 to 250 in OVERVIEW status screen
//#define BRIGHTNESS_TEST
// if display and driver support hardware scrolling the SCREENSAVER status-screen will be smoother:
//#define HARDWARE_SCROLL_AVAILABLE
//=== variables ===
// every function can access the display configuration from config.cpp
static display_config_t displayConfig;
//--------------------------
@ -32,178 +29,603 @@ extern "C"{
//--------------------------
//TODO duplicate code: getVoltage also defined in currentsensor.cpp -> outsource this
//local function to get average voltage from adc
float getVoltage1(adc1_channel_t adc, uint32_t samples){
int readAdc(adc1_channel_t adc, uint32_t samples){
//measure voltage
int measure = 0;
uint32_t measure = 0;
for (int j=0; j<samples; j++){
measure += adc1_get_raw(adc);
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;
//int center, top, bottom;
char lineChar[20];
//top = 2;
//center = 3;
//bottom = 8;
//tag for logging
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;
//=================
//===== init ======
//=================
void display_init(){
//======================
//==== display_init ====
//======================
void display_init(display_config_t config){
adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11); //max voltage
ESP_LOGW("display", "INTERFACE is i2c");
ESP_LOGW("display", "SDA_GPIO=%d",SDA_GPIO);
ESP_LOGW("display", "SCL_GPIO=%d",SCL_GPIO);
ESP_LOGW("display", "RESET_GPIO=%d",RESET_GPIO);
i2c_master_init(&dev, SDA_GPIO, SCL_GPIO, RESET_GPIO);
#if FLIP
dev._flip = true;
ESP_LOGW("display", "Flip upside down");
#endif
ESP_LOGI("display", "Panel is 128x64");
ssd1306_init(&dev, 128, 64);
ESP_LOGI(TAG, "Initializing Display with config: sda=%d, sdl=%d, reset=%d, offset=%d, flip=%d, size: %dx%d",
config.gpio_sda, config.gpio_scl, config.gpio_reset, config.offsetX, config.flip, config.width, config.height);
i2c_master_init(&dev, config.gpio_sda, config.gpio_scl, config.gpio_reset);
if (config.flip) {
dev._flip = true;
ESP_LOGW(TAG, "Flip upside down");
}
ssd1306_init(&dev, config.width, config.height, config.offsetX);
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(){
#define BAT_VOLTAGE_CONVERSION_FACTOR 11.9
float voltageRead = getVoltage1(ADC_BATT_VOLTAGE, 1000);
float battVoltage = voltageRead * 11.9; //note: factor comes from simple test with voltmeter
ESP_LOGD(TAG, "batteryVoltage - voltageAdc=%f, voltageConv=%f, factor=%.2f", voltageRead, battVoltage, BAT_VOLTAGE_CONVERSION_FACTOR);
// check if lookup table is configured correctly
int countAdc = sizeof(batteryAdcValues) / sizeof(float);
int countVoltages = sizeof(batteryVoltages) / sizeof(float);
if (countAdc != countVoltages)
{
ESP_LOGE(TAG, "getBatteryVoltage - count of configured adc-values do not match count of voltages");
return 0;
}
//read adc
int adcRead = readAdc(ADC_BATT_VOLTAGE, 1000);
//convert adc to voltage using lookup table
float battVoltage = scaleUsingLookupTable(batteryAdcValues, batteryVoltages, countAdc, adcRead);
ESP_LOGD(TAG, "batteryVoltage - adcRaw=%d => voltage=%.3f, scaled using lookuptable with %d elements", adcRead, battVoltage, countAdc);
return battVoltage;
}
//----------------------------------
//------- getBatteryPercent --------
//----------------------------------
//TODO find better/more accurate table?
//configure discharge curve of one cell with corresponding known voltage->chargePercent values
const float voltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20};
const float percentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0};
// TODO find better/more accurate table?
// configure discharge curve of one cell with corresponding known voltage->chargePercent values
const float cellVoltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20};
const float cellPercentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0};
float getBatteryPercent(float voltage){
float cellVoltage = voltage/BAT_CELL_COUNT;
int size = sizeof(voltageLevels) / sizeof(voltageLevels[0]);
int sizePer = sizeof(percentageLevels) / sizeof(percentageLevels[0]);
//check if configured correctly
if (size != sizePer) {
float getBatteryPercent()
{
// check if lookup table is configured correctly
int sizeVoltage = sizeof(cellVoltageLevels) / sizeof(cellVoltageLevels[0]);
int sizePer = sizeof(cellPercentageLevels) / sizeof(cellPercentageLevels[0]);
if (sizeVoltage != sizePer)
{
ESP_LOGE(TAG, "getBatteryPercent - count of configured percentages do not match count of voltages");
return 0;
}
if (cellVoltage <= voltageLevels[0]) {
return 0.0;
} else if (cellVoltage >= voltageLevels[size - 1]) {
return 100.0;
//get current battery voltage
float voltage = getBatteryVoltage();
float cellVoltage = voltage / BAT_CELL_COUNT;
//convert voltage to battery percentage using lookup table
float percent = scaleUsingLookupTable(cellVoltageLevels, cellPercentageLevels, sizeVoltage, cellVoltage);
ESP_LOGD(TAG, "batteryPercentage - Battery=%.3fV, Cell=%.3fV => percentage=%.3f, scaled using lookuptable with %d elements", voltage, cellVoltage, percent, sizePer);
return percent;
}
//#############################
//#### showScreen Overview ####
//#############################
//shows overview on entire display:
//Battery percentage, voltage, current, mode, rpm, speed
#define STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL 400
void showStatusScreenOverview(display_task_parameters_t *objects)
{
//-- battery percentage --
// TODO update when no load (currentsensors = ~0A) only
//-- large batt percent --
displayTextLine(&dev, 0, true, false, "B:%02.0f%%", getBatteryPercent());
//-- voltage and current --
displayTextLine(&dev, 3, false, false, "%04.1fV %04.1f:%04.1fA",
getBatteryVoltage(),
fabs(objects->motorLeft->getCurrentA()),
fabs(objects->motorRight->getCurrentA()));
//-- control state --
//print large line
displayTextLine(&dev, 4, true, false, "%s ", objects->control->getCurrentModeStr());
//-- speed and RPM --
displayTextLine(&dev, 7, false, false, "%3.1fkm/h %03.0f:%03.0fR",
fabs((objects->speedLeft->getKmph() + objects->speedRight->getKmph()) / 2),
objects->speedLeft->getRpm(),
objects->speedRight->getRpm());
// debug speed sensors
ESP_LOGD(TAG, "%3.1fkm/h %03.0f:%03.0fR",
fabs((objects->speedLeft->getKmph() + objects->speedRight->getKmph()) / 2),
objects->speedLeft->getRpm(),
objects->speedRight->getRpm());
vTaskDelay(STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL / portTICK_PERIOD_MS);
//-- brightness test --
#ifdef BRIGHTNESS_TEST
// continously vary brightness/contrast for testing
displayConfig.contrastNormal += 10;
if (displayConfig.contrastNormal > 255)
displayConfig.contrastNormal = 0;
ssd1306_contrast(&dev, displayConfig.contrastNormal);
vTaskDelay(100 / portTICK_PERIOD_MS);
ESP_LOGW(TAG, "TEST BRIGHTNESS, setting to %d", displayConfig.contrastNormal);
#endif
}
//############################
//##### showScreen Speed #####
//############################
// shows speed of each motor in km/h large in two lines and RPM in last line
#define STATUS_SCREEN_SPEED_UPDATE_INTERVAL 300
void showStatusScreenSpeed(display_task_parameters_t * objects)
{
// title
displayTextLine(&dev, 0, false, false, "Speed L,R - km/h");
// show km/h large in two lines
displayTextLine(&dev, 1, true, false, "%+.2f", objects->speedLeft->getKmph());
displayTextLine(&dev, 4, true, false, "%+.2f", objects->speedRight->getKmph());
// show both rotational speeds in one line
displayTextLineCentered(&dev, 7, false, false, "%+04.0f:%+04.0f RPM",
objects->speedLeft->getRpm(),
objects->speedRight->getRpm());
vTaskDelay(STATUS_SCREEN_SPEED_UPDATE_INTERVAL / portTICK_PERIOD_MS);
}
//#############################
//#### showScreen Joystick ####
//#############################
// shows speed of each motor in km/h large in two lines and RPM in last line
#define STATUS_SCREEN_JOYSTICK_UPDATE_INTERVAL 100
void showStatusScreenJoystick(display_task_parameters_t * objects)
{
// print all joystick data
joystickData_t data = objects->joystick->getData();
displayTextLine(&dev, 0, false, false, "joystick status:");
displayTextLine(&dev, 1, false, false, "x = %.3f ", data.x);
displayTextLine(&dev, 2, false, false, "y = %.3f ", data.y);
displayTextLine(&dev, 3, false, false, "radius = %.3f", data.radius);
displayTextLine(&dev, 4, false, false, "angle = %-06.3f ", data.angle);
displayTextLine(&dev, 5, false, false, "pos=%-12s ", joystickPosStr[(int)data.position]);
displayTextLine(&dev, 6, false, false, "adc: %d:%d ", objects->joystick->getRawX(), objects->joystick->getRawY());
displayTextLine(&dev, 7, false, false, "mode=%s ", objects->control->getCurrentModeStr());
vTaskDelay(STATUS_SCREEN_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS);
}
//#############################
//##### showScreen motors #####
//#############################
// shows speed of each motor in km/h large in two lines and RPM in last line
#define STATUS_SCREEN_MOTORS_UPDATE_INTERVAL 150
void showStatusScreenMotors(display_task_parameters_t *objects)
{
displayTextLine(&dev, 0, true, false, "%-4.0fW ", fabs(objects->motorLeft->getCurrentA()) * getBatteryVoltage());
displayTextLine(&dev, 3, true, false, "%-4.0fW ", fabs(objects->motorRight->getCurrentA()) * getBatteryVoltage());
//displayTextLine(&dev, 0, true, false, "L:%02.0f%%", objects->motorLeft->getStatus().duty);
//displayTextLine(&dev, 3, true, false, "R:%02.0f%%", objects->motorRight->getStatus().duty);
displayTextLineCentered(&dev, 6, false, false, "%+03.0f%% | %+03.0f%% DTY",
objects->motorLeft->getStatus().duty,
objects->motorRight->getStatus().duty);
displayTextLineCentered(&dev, 7, false, false, "%+04.0f | %+04.0f RPM",
objects->speedLeft->getRpm(),
objects->speedRight->getRpm());
vTaskDelay(STATUS_SCREEN_MOTORS_UPDATE_INTERVAL / portTICK_PERIOD_MS);
}
// ################################
// #### showScreen Screensaver ####
// ################################
// show inactivity duration and battery perventage scrolling across screen the entire screen to prevent burn in
#define STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS 10 * 1000
#define STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL 500
#define DISPLAY_HORIZONTAL_CHARACTER_COUNT 16
#define DISPLAY_VERTICAL_LINE_COUNT 8
void showStatusScreenScreensaver(display_task_parameters_t *objects)
{
//-- variables for line rotation --
static int msPassed = 0;
static int currentLine = 0;
static bool lineChanging = false;
// clear display once when rotating to next line
if (lineChanging)
{
ssd1306_clear_screen(&dev, false);
lineChanging = false;
}
//-- print 2 lines scrolling horizontally --
#ifdef HARDWARE_SCROLL_AVAILABLE // when display supports hardware scrolling -> only the content has to be updated
// note: scrolling is enabled at screen change (display_selectStatusPage())
// update text every iteration to prevent empty screen at start
displayTextLine(&dev, currentLine, false, false, "IDLE since:");
displayTextLine(&dev, currentLine + 1, false, false, "%.1fh, B:%02.0f%%",
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent());
// note: scrolling is disabled at screen change (display_selectStatusPage())
#else // custom implementation to scroll the text 1 character to the right every iteration (also wraps over the end to beginning)
static int offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT;
char buf1[64], buf2[64];
// scroll text left to right (taken window of the string moves to the left => offset 16->0, 16->0 ...)
offset -= 1;
if (offset < 0)
offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT - 1; // 0 = no crop -> start over with crop
// note: these strings have to be symetrical and 2x display character count long
snprintf(buf1, 64, "IDLE since: IDLE since: ");
snprintf(buf2, 64, "%.1fh, B:%02.0f%% %.1fh, B:%02.0f%% ",
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent(),
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent());
// print strings on display while limiting to certain window (ignore certain count of characters at start)
displayTextLine(&dev, currentLine, false, false, "%s", buf1 + offset);
displayTextLine(&dev, currentLine + 1, false, false, "%s", buf2 + offset);
#endif
//-- handle line rotation --
// to not block the display task for several seconds returning every e.g. 500ms here
// -> ensures detection of activity (exit condition) in task loop is handled regularly
if (msPassed > STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS) // switch to next line is due
{
msPassed = 0; // rest seconds count
// increment / rotate to next line
if (++currentLine >= DISPLAY_VERTICAL_LINE_COUNT - 1) // rotate to next line
currentLine = 0;
lineChanging = true; // clear screen in next run
}
//-- wait update interval --
// wait and increment passed time after each run
vTaskDelay(STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL / portTICK_PERIOD_MS);
msPassed += STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL;
}
//########################
//#### showStartupMsg ####
//########################
//shows welcome message and information about current version
void showStartupMsg(){
const esp_app_desc_t * desc = esp_ota_get_app_description();
//show message
displayTextLine(&dev, 0, true, false, "START");
//show git-tag
displayTextLine(&dev, 4, false, false, "%s", desc->version);
//show build-date (note: date,time of last clean build)
displayTextLine(&dev, 6, false, false, "%s", desc->date);
//show build-time
displayTextLine(&dev, 7, false, false, "%s", desc->time);
}
//============================
//===== selectStatusPage =====
//============================
void display_selectStatusPage(displayStatusPage_t newStatusPage)
{
// get number of available screens
const displayStatusPage_t max = STATUS_SCREEN_SCREENSAVER;
const uint8_t maxItems = (uint8_t)max;
// limit to available pages
if (newStatusPage > maxItems) newStatusPage = (displayStatusPage_t)(maxItems);
else if (newStatusPage < 0) newStatusPage = (displayStatusPage_t)0;
//-- run commands when switching FROM certain mode --
switch (selectedStatusPage)
{
#ifdef HARDWARE_SCROLL_AVAILABLE
case STATUS_SCREEN_SCREENSAVER:
ssd1306_hardware_scroll(&dev, SCROLL_STOP); // disable scrolling when exiting screensaver
break;
#endif
default:
break;
}
//scale voltage linear to percent in matched range
for (int i = 1; i < size; ++i) {
if (cellVoltage <= voltageLevels[i]) {
float voltageRange = voltageLevels[i] - voltageLevels[i - 1];
float voltageOffset = cellVoltage - voltageLevels[i - 1];
float percentageRange = percentageLevels[i] - percentageLevels[i - 1];
float percentageOffset = percentageLevels[i - 1];
float percent = percentageOffset + (voltageOffset / voltageRange) * percentageRange;
ESP_LOGD(TAG, "getBatPercent - cellVoltage=%.3f => percentage=%.3f", cellVoltage, percent);
ESP_LOGD(TAG, "getBatPercent - matched range: %.2fV-%.2fV => %.1f%%-%.1f%%", voltageLevels[i-1], voltageLevels[i], percentageLevels[i-1], percentageLevels[i]);
return percent;
ESP_LOGW(TAG, "switching statusPage from %d to %d", (int)selectedStatusPage, (int)newStatusPage);
selectedStatusPage = newStatusPage;
//-- run commands when switching TO certain mode --
switch (selectedStatusPage)
{
case STATUS_SCREEN_SCREENSAVER:
ssd1306_clear_screen(&dev, false); // clear screen when switching
#ifdef HARDWARE_SCROLL_AVAILABLE
ssd1306_hardware_scroll(&dev, SCROLL_RIGHT);
#endif
break;
default:
break;
}
}
//============================
//===== rotateStatusPage =====
//============================
// select next/previous status screen and rotate to start/end (uses all available in struct)
void display_rotateStatusPage(bool reverseDirection, bool noRotate)
{
// get number of available screens
const displayStatusPage_t max = STATUS_SCREEN_SCREENSAVER;
const uint8_t maxItems = (uint8_t)max - 1; // screensaver is not relevant
if (reverseDirection == false) // rotate next
{
if (selectedStatusPage >= maxItems) // already at last item
{
if (noRotate)
return; // stay at last item when rotating disabled
display_selectStatusPage((displayStatusPage_t)0); // rotate to first item
}
else
// select next screen
display_selectStatusPage((displayStatusPage_t)((int)selectedStatusPage + 1));
ssd1306_clear_screen(&dev, false); // clear screen when switching
}
else // rotate back
{
if (selectedStatusPage <= 0) // already at first item
{
if (noRotate)
return; // stay at first item when rotating disabled
display_selectStatusPage((displayStatusPage_t)(maxItems)); // rotate to last item
}
else
// select previous screen
display_selectStatusPage((displayStatusPage_t)((int)selectedStatusPage - 1));
ssd1306_clear_screen(&dev, false); // clear screen when switching
}
}
//==========================
//=== handleStatusScreen ===
//==========================
//show currently selected status screen on display
//function is repeatedly called by display task when not in MENU mode
void handleStatusScreen(display_task_parameters_t *objects)
{
switch (selectedStatusPage)
{
default:
case STATUS_SCREEN_OVERVIEW:
showStatusScreenOverview(objects);
break;
case STATUS_SCREEN_SPEED:
showStatusScreenSpeed(objects);
break;
case STATUS_SCREEN_JOYSTICK:
showStatusScreenJoystick(objects);
break;
case STATUS_SCREEN_MOTORS:
showStatusScreenMotors(objects);
break;
case STATUS_SCREEN_SCREENSAVER:
showStatusScreenScreensaver(objects);
break;
}
//--- handle timeouts ---
uint32_t inactiveMs = objects->control->getInactivityDurationMs();
//-- screensaver --
// handle switch to screensaver when no user input for a long time
if (inactiveMs > displayConfig.timeoutSwitchToScreensaverMs) // timeout - switch to screensaver is due
{
if (selectedStatusPage != STATUS_SCREEN_SCREENSAVER)
{ // switch/log only once at change
ESP_LOGW(TAG, "no activity for more than %d min, switching to screensaver", inactiveMs / 1000 / 60);
display_selectStatusPage(STATUS_SCREEN_SCREENSAVER);
}
}
ESP_LOGE(TAG, "getBatteryPercent - unknown voltage range");
return 0.0; //unknown range
else if (selectedStatusPage == STATUS_SCREEN_SCREENSAVER) // exit screensaver when there was recent activity
{
ESP_LOGW(TAG, "recent activity detected, disabling screensaver");
display_selectStatusPage(STATUS_SCREEN_OVERVIEW);
}
//-- reduce brightness --
// handle brightness reduction when no user input for some time
static bool brightnessIsReduced = false;
if (inactiveMs > displayConfig.timeoutReduceContrastMs) // threshold exceeded - reduction of brightness is due
{
if (!brightnessIsReduced) // change / log only once at change
{
// reduce display brightness (less burn in)
ESP_LOGW(TAG, "no activity for more than %d min, reducing display brightness to %d/255", inactiveMs / 1000 / 60, displayConfig.contrastReduced);
ssd1306_contrast(&dev, displayConfig.contrastReduced);
brightnessIsReduced = true;
}
}
else if (brightnessIsReduced) // threshold not exceeded anymore, but still reduced
{
// increase display brighness again
ESP_LOGW(TAG, "recent activity detected, increasing brightness again");
ssd1306_contrast(&dev, displayConfig.contrastNormal);
brightnessIsReduced = false;
}
}
float getBatteryPercent(){
float voltage = getBatteryVoltage();
return getBatteryPercent(voltage);
}
//============================
//======= display task =======
//============================
#define VERY_SLOW_LOOP_INTERVAL 30000
#define SLOW_LOOP_INTERVAL 1000
#define FAST_LOOP_INTERVAL 200
//TODO: separate taks for each loop?
// TODO: separate task for each loop?
void display_task(void *pvParameters)
{
ESP_LOGW(TAG, "Initializing display and starting handle loop");
//get struct with pointers to all needed global objects from task parameter
display_task_parameters_t *objects = (display_task_parameters_t *)pvParameters;
void display_task( void * pvParameters ){
char buf[20];
char buf1[20];
int len, len1;
int countFastloop = SLOW_LOOP_INTERVAL;
int countSlowLoop = VERY_SLOW_LOOP_INTERVAL;
// initialize display
display_init(objects->displayConfig);
// TODO check if successfully initialized
display_init();
//TODO check if successfully initialized
// show startup message
showStartupMsg();
vTaskDelay(STARTUP_MSG_TIMEOUT / portTICK_PERIOD_MS);
ssd1306_clear_screen(&dev, false);
//welcome msg
strcpy(buf, "Hello");
ssd1306_display_text_x3(&dev, 0, buf, 5, false);
vTaskDelay(1000 / portTICK_PERIOD_MS);
// repeatedly update display with content depending on current mode
while (1)
{
switch (objects->control->getCurrentMode())
{
case controlMode_t::MENU_SETTINGS:
// uses encoder events to control menu (settings) and updates display
handleMenu_settings(objects, &dev);
break;
case controlMode_t::MENU_MODE_SELECT:
// uses encoder events to control menu (mode select) and updates display
handleMenu_modeSelect(objects, &dev);
break;
default:
// show selected status screen in any other mode
handleStatusScreen(objects);
break;
} // end mode switch-case
// TODO add pages and menus here
} // end while(1)
} // end display-task
//update stats
while(1){
if (countFastloop >= SLOW_LOOP_INTERVAL / FAST_LOOP_INTERVAL){
//---- very slow loop ----
if (countSlowLoop >= VERY_SLOW_LOOP_INTERVAL/SLOW_LOOP_INTERVAL){
//clear display - workaround for bugged line order after a few minutes
countSlowLoop = 0;
ssd1306_clear_screen(&dev, false);
}
//---- slow loop ----
countSlowLoop ++;
countFastloop = 0;
//--- battery stats ---
//TODO update only when no load (currentsensors = ~0A)
float battVoltage = getBatteryVoltage();
float battPercent = getBatteryPercent(battVoltage);
len = snprintf(buf, sizeof(buf), "Bat:%.1fV %.2fV", battVoltage, battVoltage/BAT_CELL_COUNT);
len1 = snprintf(buf1, sizeof(buf1), "B:%02.0f%%", battPercent);
ssd1306_display_text_x3(&dev, 0, buf1, len1, false);
ssd1306_display_text(&dev, 3, buf, len, false);
ssd1306_display_text(&dev, 4, buf, len, true);
}
//---- fast loop ----
//update speed/rpm
float sLeft = speedLeft.getKmph();
float rLeft = speedLeft.getRpm();
float sRight = speedRight.getKmph();
float rRight = speedRight.getRpm();
len = snprintf(buf, sizeof(buf), "L:%.1f R:%.1fkm/h", fabs(sLeft), fabs(sRight));
ssd1306_display_text(&dev, 5, buf, len, false);
len = snprintf(buf, sizeof(buf), "L:%4.0f R:%4.0fRPM", rLeft, rRight);
ssd1306_display_text(&dev, 6, buf, len, false);
//debug speed sensors
ESP_LOGD(TAG, "%s", buf);
//TODO show currentsensor values
vTaskDelay(FAST_LOOP_INTERVAL / portTICK_PERIOD_MS);
countFastloop++;
}
//TODO add pages and menus
@ -309,15 +731,4 @@ void display_task( void * pvParameters ){
//// Fade Out
//ssd1306_fadeout(&dev);
#if 0
// Fade Out
for(int contrast=0xff;contrast>0;contrast=contrast-0x20) {
ssd1306_contrast(&dev, contrast);
vTaskDelay(40);
}
#endif
}
//ssd1306_fadeout(&dev);

64
board_single/main/display.hpp
View File

@ -1,18 +1,80 @@
#pragma once
extern "C" {
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "nvs.h"
#include "ssd1306.h"
#include "font8x8_basic.h"
}
#include "config.hpp"
#include "joystick.hpp"
#include "control.hpp"
#include "speedsensor.hpp"
// configuration for initializing display (passed to task as well)
typedef struct display_config_t {
// initialization
gpio_num_t gpio_scl;
gpio_num_t gpio_sda;
int gpio_reset; // negative number means reset pin is not connected or not used
int width;
int height;
int offsetX;
bool flip;
// display-task
int contrastNormal;
int contrastReduced;
uint32_t timeoutReduceContrastMs;
uint32_t timeoutSwitchToScreensaverMs;
} display_config_t;
// struct with variables passed to task from main()
typedef struct display_task_parameters_t {
display_config_t displayConfig;
controlledArmchair * control;
evaluatedJoystick * joystick;
QueueHandle_t encoderQueue;
controlledMotor * motorLeft;
controlledMotor * motorRight;
speedSensor * speedLeft;
speedSensor * speedRight;
buzzer_t *buzzer;
nvs_handle_t * nvsHandle;
} display_task_parameters_t;
// enum for selecting the currently shown status page (display content when not in MENU_SETTINGS mode)
typedef enum displayStatusPage_t {STATUS_SCREEN_OVERVIEW=0, STATUS_SCREEN_SPEED, STATUS_SCREEN_JOYSTICK, STATUS_SCREEN_MOTORS, STATUS_SCREEN_SCREENSAVER, __NUMBER_OF_AVAILABLE_SCREENS} displayStatusPage_t; //note: SCREENSAVER has to be last one since it is ignored by rotate and used to determine count
// get precise battery voltage (using lookup table)
float getBatteryVoltage();
// get battery charge level in percent (using lookup table as discharge curve)
float getBatteryPercent();
// function to select one of the defined status screens which are shown on display when not in MENU_SETTINGS or MENU_SELECT_MODE mode
void display_selectStatusPage(displayStatusPage_t newStatusPage);
// select next/previous status screen to be shown, when noRotate is set is stays at first/last screen
void display_rotateStatusPage(bool reverseDirection = false, bool noRotate = false);
//task that inititialized the display, displays welcome message
//and releatedly updates the display with certain content
void display_task( void * pvParameters );
//abstracted function for printing one line on the display, using a format string directly
//and options: Large-font (3 lines, max 5 digits), or inverted color
void displayTextLine(SSD1306_t *display, int line, bool large, bool inverted, const char *format, ...);
//abstracted function for printing a string CENTERED on the display, using a format string
//adds spaces left and right to fill the line (if not too long already)
void displayTextLineCentered(SSD1306_t *display, int line, bool isLarge, bool inverted, const char *format, ...);

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";
//=======================================
//============== 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 --------
//-----------------------------

17
board_single/main/fan.hpp
View File

@ -16,7 +16,22 @@ typedef struct fan_config_t {
uint32_t minOnMs;
uint32_t minOffMs;
uint32_t turnOffDelayMs;
} fan_config;
} fan_config_t;
// struct with variables passed to task from main
typedef struct task_fans_parameters_t {
fan_config_t fan_config;
controlledMotor * motorLeft;
controlledMotor * motorRight;
} task_fans_parameters_t;
//====================================
//========== motorctl task ===========
//====================================
//note: pointer to task_fans_parameters_t has to be passed as task-parameter (config, motor objects)
void task_fans( void * task_fans_parameters );

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"
{
#include <stdio.h>
#include <esp_system.h>
#include <esp_event.h>
#include <nvs_flash.h>
#include "nvs.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
@ -14,98 +12,94 @@ extern "C"
#include "sdkconfig.h"
#include "esp_spiffs.h"
#include "driver/ledc.h"
//custom C files
#include "wifi.h"
}
#include <new>
//custom C++ files
#include "config.hpp"
//folder common
#include "uart_common.hpp"
#include "motordrivers.hpp"
#include "http.hpp"
#include "speedsensor.hpp"
#include "motorctl.hpp"
//folder single_board
#include "control.hpp"
#include "button.hpp"
#include "http.hpp"
#include "uart_common.hpp"
#include "display.hpp"
#include "encoder.hpp"
//tag for logging
//only extends this file (no library):
//outsourced all configuration related structures
#include "config.cpp"
//================================
//======== declarations ==========
//================================
//--- declare all pointers to shared objects ---
controlledMotor *motorLeft;
controlledMotor *motorRight;
// TODO initialize driver in createOjects like everything else
// (as in 6e9b3d96d96947c53188be1dec421bd7ff87478e)
// issue with laggy encoder wenn calling methods via pointer though
//sabertooth2x60a *sabertoothDriver;
sabertooth2x60a sabertoothDriver(sabertoothConfig);
evaluatedJoystick *joystick;
buzzer_t *buzzer;
controlledArmchair *control;
automatedArmchair_c *automatedArmchair;
httpJoystick *httpJoystickMain;
speedSensor *speedLeft;
speedSensor *speedRight;
cControlledRest *legRest;
cControlledRest *backRest;
//--- lambda functions motor-driver ---
// functions for updating the duty via currently used motor driver (hardware) that can then be passed to controlledMotor
//-> makes it possible to easily use different motor drivers
motorSetCommandFunc_t setLeftFunc = [&sabertoothDriver](motorCommand_t cmd)
{
//TODO why encoder lag when call via pointer?
sabertoothDriver.setLeft(cmd);
};
motorSetCommandFunc_t setRightFunc = [&sabertoothDriver](motorCommand_t cmd)
{
sabertoothDriver.setRight(cmd);
};
//--- lambda function http-joystick ---
// function that initializes the http server requires a function pointer to function that handels each url
// the httpd_uri config struct does not accept a pointer to a method of a class instance, directly
// thus this lambda function is necessary:
// declare pointer to receiveHttpData method of httpJoystick class
esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData;
esp_err_t on_joystick_url(httpd_req_t *req)
{
// run pointer to receiveHttpData function of httpJoystickMain instance
return (httpJoystickMain->*pointerToReceiveFunc)(req);
}
//--- tag for logging ---
static const char * TAG = "main";
//-- handle passed to tasks for accessing nvs --
nvs_handle_t nvsHandle;
//====================================
//========== motorctl task ===========
//====================================
//task for handling the motors (ramp, current limit, driver)
void task_motorctl( void * pvParameters ){
ESP_LOGI(TAG, "starting handle loop...");
while(1){
motorRight.handle();
motorLeft.handle();
//10khz -> T=100us
vTaskDelay(10 / portTICK_PERIOD_MS);
}
}
//======================================
//============ buzzer task =============
//======================================
//TODO: move the task creation to buzzer class (buzzer.cpp)
//e.g. only have function buzzer.createTask() in app_main
void task_buzzer( void * pvParameters ){
ESP_LOGI("task_buzzer", "Start of buzzer task...");
//run function that waits for a beep events to arrive in the queue
//and processes them
buzzer.processQueue();
}
//=======================================
//============ control task =============
//=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver
void task_control( void * pvParameters ){
ESP_LOGI(TAG, "Initializing controlledArmchair and starting handle loop");
//start handle loop (control object declared in config.hpp)
control.startHandleLoop();
}
//======================================
//============ button task =============
//======================================
//task that handles the button interface/commands
void task_button( void * pvParameters ){
ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
//create button instance
buttonCommands commandButton(&buttonJoystick, &joystick, &control, &buzzer, &motorLeft, &motorRight);
//start handle loop
commandButton.startHandleLoop();
}
//=======================================
//============== fan task ===============
//=======================================
//task that controlls fans for cooling the drivers
void task_fans( void * pvParameters ){
ESP_LOGI(TAG, "Initializing fans and starting fan handle loop");
//create fan instances with config defined in config.cpp
controlledFan fan(configCooling, &motorLeft, &motorRight);
//repeatedly run fan handle function in a slow loop
while(1){
fan.handle();
vTaskDelay(500 / portTICK_PERIOD_MS);
}
}
//=================================
@ -113,7 +107,6 @@ void task_fans( void * pvParameters ){
//=================================
//initialize spi flash filesystem (used for webserver)
void init_spiffs(){
ESP_LOGI(TAG, "init spiffs");
esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = {
.base_path = "/spiffs",
.partition_label = NULL,
@ -131,29 +124,54 @@ void init_spiffs(){
//==================================
//======== define loglevels ========
//==================================
void setLoglevels(void){
//set loglevel for all tags:
esp_log_level_set("*", ESP_LOG_WARN);
//--- set loglevel for individual tags ---
esp_log_level_set("main", ESP_LOG_INFO);
//esp_log_level_set("buzzer", ESP_LOG_INFO);
//esp_log_level_set("motordriver", ESP_LOG_DEBUG);
//esp_log_level_set("motor-control", ESP_LOG_INFO);
//esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
//esp_log_level_set("joystickCommands", ESP_LOG_DEBUG);
esp_log_level_set("button", ESP_LOG_INFO);
esp_log_level_set("control", ESP_LOG_INFO);
//esp_log_level_set("fan-control", ESP_LOG_INFO);
esp_log_level_set("wifi", ESP_LOG_INFO);
esp_log_level_set("http", ESP_LOG_INFO);
//esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG);
esp_log_level_set("display", ESP_LOG_INFO);
//esp_log_level_set("current-sensors", ESP_LOG_INFO);
//esp_log_level_set("speedSensor", ESP_LOG_INFO);
//=================================
//========= createObjects =========
//=================================
//create all shared objects
//their references can be passed to the tasks that need access in main
//Note: the configuration structures (e.g. configMotorControlLeft) are outsourced to file 'config.cpp'
void createObjects()
{
// create sabertooth motor driver instance
// sabertooth2x60a sabertoothDriver(sabertoothConfig);
// with configuration above
//sabertoothDriver = new sabertooth2x60a(sabertoothConfig);
// create speedsensor instances
// with configurations from config.cpp
speedLeft = new speedSensor(speedLeft_config);
speedRight = new speedSensor(speedRight_config);
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle, speedLeft, &motorRight); //note: ptr to ptr of controlledMotor since it isnt defined yet
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle, speedRight, &motorLeft);
// create joystick instance (joystick.hpp)
joystick = new evaluatedJoystick(configJoystick, &nvsHandle);
// create httpJoystick object (http.hpp)
httpJoystickMain = new httpJoystick(configHttpJoystickMain);
http_init_server(on_joystick_url);
// create buzzer object on pin 12 with gap between queued events of 1ms
buzzer = new buzzer_t(GPIO_NUM_12, 1);
// create objects for controlling the chair position
// gpio_up, gpio_down, name
legRest = new cControlledRest(GPIO_NUM_2, GPIO_NUM_15, "legRest");
backRest = new cControlledRest(GPIO_NUM_16, GPIO_NUM_4, "backRest");
// create control object (control.hpp)
// with configuration from config.cpp
control = new controlledArmchair(configControl, buzzer, motorLeft, motorRight, joystick, &joystickGenerateCommands_config, httpJoystickMain, automatedArmchair, legRest, backRest, &nvsHandle);
// create automatedArmchair_c object (for auto-mode) (auto.hpp)
automatedArmchair = new automatedArmchair_c(motorLeft, motorRight);
}
@ -163,70 +181,119 @@ void setLoglevels(void){
//=========== app_main ============
//=================================
extern "C" void app_main(void) {
//enable 5V volate regulator
ESP_LOGW(TAG, "===== BOOT (pre main) Completed =====\n");
ESP_LOGW(TAG, "===== INITIALIZING COMPONENTS =====");
//--- define log levels ---
setLoglevels();
//--- enable 5V volate regulator ---
ESP_LOGW(TAG, "enabling 5V regulator...");
gpio_pad_select_gpio(GPIO_NUM_17);
gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT);
gpio_set_level(GPIO_NUM_17, 1);
//---- define log levels ----
setLoglevels();
//--- initialize nvs-flash and netif ---
ESP_LOGW(TAG,"initializing NVS...");
wifi_initNvs(); //needed for wifi and persistent config variables
ESP_LOGW(TAG,"initializing NETIF...");
wifi_initNetif(); // needed for wifi
//--- initialize spiffs ---
ESP_LOGW(TAG, "initializing SPIFFS...");
init_spiffs(); // used by httpd server
//--- initialize and start wifi ---
// Note: now started only when switching to HTTP mode in control.cpp
// ESP_LOGW(TAG,"starting wifi...");
// wifi_start_client(); //connect to existing wifi (dropped)
// wifi_start_ap(); //start access point
//--- initialize encoder ---
const QueueHandle_t encoderQueue = encoder_init(&encoder_config);
//--- open nvs-flash ---
// note: nvs already initialized in wifi_initNvs()
ESP_LOGW(TAG, "opening NVS-handle...");
esp_err_t err = nvs_open("storage", NVS_READWRITE, &nvsHandle); // this handle is passed to all tasks for accessing nvs
if (err != ESP_OK)
ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
printf("\n");
//--- create all objects ---
ESP_LOGW(TAG, "===== CREATING SHARED OBJECTS =====");
//initialize sabertooth object in STACK (due to performance issues in heap)
///sabertoothDriver = static_cast<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 ---
//----------------------------------------------
//task that receives commands, handles ramp and current limit and executes commands using the motordriver function
xTaskCreate(&task_motorctl, "task_motor-control", 2*4096, NULL, 6, NULL);
//task for each motor that handles to following:
//receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr)
xTaskCreate(&task_motorctl, "task_ctl-left-motor", 2*4096, motorLeft, 6, NULL);
xTaskCreate(&task_motorctl, "task_ctl-right-motor", 2*4096, motorRight, 6, NULL);
//------------------------------
//--- create task for buzzer ---
//------------------------------
xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL);
//task that processes queued beeps
//note: pointer to shard object 'buzzer' is passed as task parameter:
xTaskCreate(&task_buzzer, "task_buzzer", 2048, buzzer, 2, NULL);
//-------------------------------
//--- create task for control ---
//-------------------------------
//task that generates motor commands depending on the current mode and sends those to motorctl task
xTaskCreate(&task_control, "task_control", 4096, NULL, 5, NULL);
//note: pointer to shared object 'control' is passed as task parameter:
xTaskCreate(&task_control, "task_control", 4096, control, 5, NULL);
//------------------------------
//--- create task for button ---
//------------------------------
//task that evaluates and processes the button input and runs the configured commands
xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL);
//task that handles button/encoder events in any mode except 'MENU_SETTINGS' and 'MENU_MODE_SELECT' (e.g. switch modes by pressing certain count)
task_button_parameters_t button_param = {control, joystick, encoderQueue, motorLeft, motorRight, buzzer};
xTaskCreate(&task_button, "task_button", 4096, &button_param, 3, NULL);
//-----------------------------------
//--- create task for fan control ---
//-----------------------------------
//task that evaluates and processes the button input and runs the configured commands
xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL);
//task that controls cooling fans of the motor driver
task_fans_parameters_t fans_param = {configFans, motorLeft, motorRight};
xTaskCreate(&task_fans, "task_fans", 2048, &fans_param, 1, NULL);
//-----------------------------------
//----- create task for display -----
//-----------------------------------
//task that handles the display
xTaskCreate(&display_task, "display_task", 3*2048, NULL, 1, NULL);
//task that handles the display (show stats, handle menu in 'MENU_SETTINGS' and 'MENU_MODE_SELECT' mode)
display_task_parameters_t display_param = {display_config, control, joystick, encoderQueue, motorLeft, motorRight, speedLeft, speedRight, buzzer, &nvsHandle};
xTaskCreate(&display_task, "display_task", 3*2048, &display_param, 3, NULL);
vTaskDelay(200 / portTICK_PERIOD_MS); //wait for all tasks to finish initializing
printf("\n");
//beep at startup
buzzer.beep(3, 70, 50);
//--- initialize nvs-flash and netif (needed for wifi) ---
wifi_initNvs_initNetif();
//--- initialize spiffs ---
init_spiffs();
//--- initialize and start wifi ---
//FIXME: run wifi_init_client or wifi_init_ap as intended from control.cpp when switching state
//currently commented out because of error "assert failed: xQueueSemaphoreTake queue.c:1549 (pxQueue->uxItemSize == 0)" when calling control->changeMode from button.cpp
//when calling control.changeMode(http) from main.cpp it worked without error for some reason?
ESP_LOGI(TAG,"starting wifi...");
//wifi_init_client(); //connect to existing wifi
wifi_init_ap(); //start access point
ESP_LOGI(TAG,"done starting wifi");
//--- startup finished ---
ESP_LOGW(TAG, "===== STARTUP FINISHED =====\n");
buzzer->beep(3, 70, 50);
//--- testing encoder ---
//xTaskCreate(&task_encoderExample, "task_buzzer", 2048, encoderQueue, 2, NULL);
//--- testing http server ---
// wifi_init_client(); //connect to existing wifi
@ -235,15 +302,17 @@ extern "C" void app_main(void) {
// http_init_server();
//--- testing force http mode after startup ---
//control.changeMode(controlMode_t::HTTP);
//--- testing force specific mode after startup ---
//control->changeMode(controlMode_t::MENU_SETTINGS);
//--- main loop ---
//does nothing except for testing things
while(1){
vTaskDelay(5000 / portTICK_PERIOD_MS);
vTaskDelay(portMAX_DELAY);
//vTaskDelay(5000 / portTICK_PERIOD_MS);
//---------------------------------
//-------- TESTING section --------
//---------------------------------

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_SSD1306_128x32 is not set
CONFIG_SSD1306_128x64=y
CONFIG_OFFSETX=0
CONFIG_OFFSETX=2
# CONFIG_FLIP is not set
CONFIG_SCL_GPIO=22
CONFIG_SDA_GPIO=21
CONFIG_SDA_GPIO=23
CONFIG_RESET_GPIO=15
CONFIG_I2C_PORT_0=y
# CONFIG_I2C_PORT_1 is not set
@ -1246,6 +1246,17 @@ CONFIG_WPA_MBEDTLS_CRYPTO=y
# CONFIG_WPA_MBO_SUPPORT is not set
# CONFIG_WPA_DPP_SUPPORT is not set
# end of Supplicant
#
# Rotary encoders
#
CONFIG_RE_MAX=1
CONFIG_RE_INTERVAL_US=1000
CONFIG_RE_BTN_DEAD_TIME_US=40000
CONFIG_RE_BTN_PRESSED_LEVEL_0=y
# CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set
CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000
# end of Rotary encoders
# end of Component config
#

1
common/CMakeLists.txt
View File

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

29
common/buzzer.cpp
View File

@ -2,6 +2,19 @@
static const char *TAG_BUZZER = "buzzer";
//======================================
//============ buzzer task =============
//======================================
// Task that repeatedly handles the buzzer object (process Queued beeps)
void task_buzzer(void * param_buzzerObject){
ESP_LOGI("task_buzzer", "Start of buzzer task...");
buzzer_t * buzzer = (buzzer_t *)param_buzzerObject;
//run function that waits for a beep events to arrive in the queue
//and processes them
buzzer->processQueue();
}
//============================
//========== init ============
//============================
@ -33,12 +46,18 @@ buzzer_t::buzzer_t(gpio_num_t gpio_pin_f, uint16_t msGap_f){
//=========== beep ===========
//============================
//function to add a beep command to the queue
//use default/configured gap when no custom pause duration is given:
void buzzer_t::beep(uint8_t count, uint16_t msOn, uint16_t msOff){
beep(count, msOn, msOff, msGap);
}
void buzzer_t::beep(uint8_t count, uint16_t msOn, uint16_t msOff, uint16_t msDelayFinished){
//create entry struct with provided data
struct beepEntry entryInsert = {
count = count,
msOn = msOn,
msOff = msOff
count,
msOn,
msOff,
msDelayFinished
};
// Send a pointer to a struct AMessage object. Don't block if the
@ -69,7 +88,7 @@ void buzzer_t::processQueue(){
// otherwise waits for at least 7 weeks
if( xQueueReceive( beepQueue, &entryRead, portMAX_DELAY ) )
{
ESP_LOGW(TAG_BUZZER, "Read entry from queue: count=%d, msOn=%d, msOff=%d", entryRead.count, entryRead.msOn, entryRead.msOff);
ESP_LOGI(TAG_BUZZER, "Read entry from queue: count=%d, msOn=%d, msOff=%d", entryRead.count, entryRead.msOn, entryRead.msOff);
//beep requested count with requested delays
for (int i = entryRead.count; i--;){
@ -83,7 +102,7 @@ void buzzer_t::processQueue(){
vTaskDelay(entryRead.msOff / portTICK_PERIOD_MS);
}
//wait for minimum gap between beep events
vTaskDelay(msGap / portTICK_PERIOD_MS);
vTaskDelay(entryRead.msDelay / portTICK_PERIOD_MS);
}
}else{ //wait for queue to become available
vTaskDelay(50 / portTICK_PERIOD_MS);

12
common/buzzer.hpp
View File

@ -27,24 +27,27 @@ class buzzer_t {
//--- functions ---
void processQueue(); //has to be run once in a separate task, waits for and processes queued events
//add entry to queue processing beeps, last parameter is optional to delay the next entry
void beep(uint8_t count, uint16_t msOn, uint16_t msOff, uint16_t msDelayFinished);
void beep(uint8_t count, uint16_t msOn, uint16_t msOff);
//void clear(); (TODO - not implemented yet)
//void createTask(); (TODO - not implemented yet)
//--- variables ---
uint16_t msGap; //gap between beep entries (when multiple queued)
private:
//--- functions ---
void init();
//--- variables ---
uint16_t msGap; //gap between beep entries (when multiple queued)
gpio_num_t gpio_pin;
struct beepEntry {
uint8_t count;
uint16_t msOn;
uint16_t msOff;
uint16_t msDelay;
};
//queue for queueing up multiple events while one is still processing
@ -53,4 +56,9 @@ class buzzer_t {
};
//======================================
//============ buzzer task =============
//======================================
// Task that repeatedly handles the buzzer object (process Queued beeps)
// Note: pointer to globally initialized buzzer object has to be passed as task-parameter
void task_buzzer(void * param_buzzerObject);

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

4
common/currentsensor.hpp
View File

@ -7,12 +7,14 @@
class currentSensor{
public:
currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent);
currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent, float snapToZeroThreshold, bool inverted = false);
void calibrateZeroAmpere(void); //set current voltage to voltage representing 0A
float read(void); //get current ampere
private:
adc1_channel_t adcChannel;
float ratedCurrent;
bool isInverted;
float snapToZeroThreshold;
uint32_t measure;
float voltage;
float current;

41
common/http.cpp
View File

@ -178,50 +178,39 @@ esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){
//-------------------
//----- getData -----
//-------------------
//wait for and return joystick data from queue, if timeout return NULL
//wait for and return joystick data from queue, return last data if nothing received within 500ms, return center data when timeout exceeded
joystickData_t httpJoystick::getData(){
//--- get joystick data from queue ---
if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(config.timeoutMs) ) ) {
if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(500) ) ) { //dont wait longer than 500ms to not block the control loop for too long
ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f",
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
timeLastData = esp_log_timestamp();
}
//--- timeout ---
//no new data received within configured timeout
// send error message when last received data did NOT result in CENTER position and timeout exceeded
else {
//send error message when last received data did NOT result in CENTER position
if (dataRead.position != joystickPos_t::CENTER) {
if (dataRead.position != joystickPos_t::CENTER && (esp_log_timestamp() - timeLastData) > config.timeoutMs) {
//change data to "joystick center" data to stop the motors
dataRead = dataCenter;
ESP_LOGE(TAG, "TIMEOUT - no data received for 3s -> set to center");
ESP_LOGE(TAG, "TIMEOUT - no data received for %dms -> set to center", config.timeoutMs);
}
}
return dataRead;
}
//--------------------------------------------
//--- receiveHttpData for httpJoystickMain ---
//--------------------------------------------
//function that wraps pointer to member function of httpJoystickMain instance in a "normal" function which the webserver can run on joystick URL
//declare pointer to receiveHttpData method of httpJoystick class
esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData;
esp_err_t on_joystick_url(httpd_req_t *req){
//run pointer to receiveHttpData function of httpJoystickMain instance
return (httpJoystickMain.*pointerToReceiveFunc)(req);
}
//============================
//===== init http server =====
//============================
//function that initializes http server and configures available urls
void http_init_server()
//function that initializes http server and configures available url's
//parameter: provide pointer to function that handle incomming joystick data (for configuring the url)
//TODO add handle functions to future additional endpoints/urls here too
void http_init_server(http_handler_t onJoystickUrl)
{
ESP_LOGI(TAG, "initializing HTTP-Server...");
//---- configure webserver ----
httpd_config_t config = HTTPD_DEFAULT_CONFIG();
@ -236,7 +225,7 @@ void http_init_server()
httpd_uri_t joystick_url = {
.uri = "/api/joystick",
.method = HTTP_POST,
.handler = on_joystick_url,
.handler = onJoystickUrl,
};
httpd_register_uri_handler(server, &joystick_url);
@ -265,8 +254,8 @@ void http_init_server()
//function that destroys the http server
void http_stop_server()
{
printf("stopping http\n");
httpd_stop(server);
ESP_LOGW(TAG, "stopping HTTP-Server");
httpd_stop(server);
}

29
common/http.hpp
View File

@ -13,7 +13,18 @@ extern "C"
//===== init http server =====
//============================
//function that initializes http server and configures available urls
void http_init_server();
//parameter: provide pointer to function that handles incomming joystick data (for configuring the url)
//TODO add handle functions to future additional endpoints/urls here too
typedef esp_err_t (*http_handler_t)(httpd_req_t *req);
void http_init_server(http_handler_t onJoystickUrl);
//example with lambda function to pass method of a class instance:
//esp_err_t (httpJoystick::*pointerToReceiveFunc)(httpd_req_t *req) = &httpJoystick::receiveHttpData;
//esp_err_t on_joystick_url(httpd_req_t *req){
// //run pointer to receiveHttpData function of httpJoystickMain instance
// return (httpJoystickMain->*pointerToReceiveFunc)(req);
//}
//http_init_server(on_joystick_url);
//==============================
@ -27,7 +38,7 @@ void start_mdns_service();
//===== stop http server =====
//============================
//function that destroys the http server
void http_stop_server();
void http_stop_server(httpd_handle_t * httpServer);
//==============================
@ -47,7 +58,7 @@ typedef struct httpJoystick_config_t {
class httpJoystick{
public:
//--- constructor ---
httpJoystick( httpJoystick_config_t config_f );
httpJoystick(httpJoystick_config_t config_f);
//--- functions ---
joystickData_t getData(); //wait for and return joystick data from queue, if timeout return CENTER
@ -59,7 +70,7 @@ class httpJoystick{
httpJoystick_config_t config;
QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) );
//struct for receiving data from http function, and storing data of last update
joystickData_t dataRead;
uint32_t timeLastData = 0;
const joystickData_t dataCenter = {
.position = joystickPos_t::CENTER,
.x = 0,
@ -67,11 +78,5 @@ class httpJoystick{
.radius = 0,
.angle = 0
};
};
//===== global object =====
//create global instance of httpJoystick
//note: is constructed/configured in config.cpp
extern httpJoystick httpJoystickMain;
joystickData_t dataRead = dataCenter;
};

342
common/joystick.cpp
View File

@ -19,8 +19,9 @@ static const char * TAG_CMD = "joystickCommands";
//-------- constructor --------
//-----------------------------
//copy provided struct with all configuration and run init function
evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){
evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f, nvs_handle_t * nvsHandle_f){
config = config_f;
nvsHandle = nvsHandle_f;
init();
}
@ -30,7 +31,7 @@ evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){
//---------- init ------------
//----------------------------
void evaluatedJoystick::init(){
ESP_LOGI(TAG, "initializing joystick");
ESP_LOGW(TAG, "initializing ADC's and loading calibration...");
//initialize adc
adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096
@ -41,6 +42,12 @@ void evaluatedJoystick::init(){
adc1_config_channel_atten(config.adc_x, ADC_ATTEN_DB_11); //max voltage
adc1_config_channel_atten(config.adc_y, ADC_ATTEN_DB_11); //max voltage
//load stored calibration values (if not found loads defaults from config)
loadCalibration(X_MIN);
loadCalibration(X_MAX);
loadCalibration(Y_MIN);
loadCalibration(Y_MAX);
//define joystick center from current position
defineCenter(); //define joystick center from current position
}
@ -81,17 +88,17 @@ joystickData_t evaluatedJoystick::getData() {
ESP_LOGV(TAG, "getting X coodrdinate...");
uint32_t adcRead;
adcRead = readAdc(config.adc_x, config.x_inverted);
float x = scaleCoordinate(readAdc(config.adc_x, config.x_inverted), config.x_min, config.x_max, x_center, config.tolerance_zeroX_per, config.tolerance_end_per);
float x = scaleCoordinate(readAdc(config.adc_x, config.x_inverted), x_min, x_max, x_center, config.tolerance_zeroX_per, config.tolerance_end_per);
data.x = x;
ESP_LOGD(TAG, "X: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => x=%.3f",
adc1_get_raw(config.adc_x), adcRead, config.x_min, config.x_max, x_center, config.x_inverted, x);
adc1_get_raw(config.adc_x), adcRead, x_min, x_max, x_center, config.x_inverted, x);
ESP_LOGV(TAG, "getting Y coodrinate...");
adcRead = readAdc(config.adc_y, config.y_inverted);
float y = scaleCoordinate(adcRead, config.y_min, config.y_max, y_center, config.tolerance_zeroY_per, config.tolerance_end_per);
float y = scaleCoordinate(adcRead, y_min, y_max, y_center, config.tolerance_zeroY_per, config.tolerance_end_per);
data.y = y;
ESP_LOGD(TAG, "Y: adc-raw=%d \tadc-conv=%d \tmin=%d \t max=%d \tcenter=%d \tinverted=%d => y=%.3lf",
adc1_get_raw(config.adc_y), adcRead, config.y_min, config.y_max, y_center, config.y_inverted, y);
adc1_get_raw(config.adc_y), adcRead, y_min, y_max, y_center, config.y_inverted, y);
//calculate radius
data.radius = sqrt(pow(data.x,2) + pow(data.y,2));
@ -297,37 +304,43 @@ joystickPos_t joystick_evaluatePosition(float x, float y){
//========= joystick_CommandsDriving =========
//============================================
//function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altStickMapping){
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){
//struct with current data of the joystick
//typedef struct joystickData_t {
// joystickPos_t position;
// float x;
// float y;
// float radius;
// float angle;
//} joystickData_t;
//--- variables ---
motorCommands_t commands;
float dutyMax = 100; //TODO add this to config, make changeable during runtime
float dutyOffset = 5; //immediately starts with this duty, TODO add this to config
float dutyRange = dutyMax - dutyOffset;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
//--- interpret config parameters ---
float dutyOffset = config->dutyOffset; // immediately starts with this duty
float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
// calculate configured boost duty (added when turning)
float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
// limit to maximum possible duty
float dutyAvailable = 100 - config->maxDutyStraight;
if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable;
//--- snap ratio to max at angle threshold ---
//(-> more joystick area where inner wheel is off when turning)
/*
//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
float ratioClipThreshold = 0.3;
if (ratio < ratioClipThreshold) ratio = 0;
else if (ratio > 1-ratioClipThreshold) ratio = 1;
//TODO subtract this clip threshold from available joystick range at ratio usage
*/
//--- calculate paramaters with current data ---
motorCommands_t commands; // store new motor commands
// -- calculate ratio --
// get current ratio from stick angle
float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
ratioActual = 1 - ratioActual; // invert ratio
// scale and clip ratio according to configured tolerance
// to have some joystick area at max ratio before reaching X-Axis-full-turn-mode
float ratio = ratioActual / (config->ratioSnapToOneThreshold); //0->0 threshold->1
// limit to 1 when above threshold (inside area max ratio)
if (ratio > 1) ratio = 1; // >threshold -> 1
// -- calculate outer tire boost --
#define BOOST_RATIO_MANIPULATION_SCALE 1.05 // >1 to apply boost slightly faster, this slightly compensates that available boost is most times less than reduction of inner duty, so for small turns the total speed feels more equal
float boostAmountOuter = data.radius*dutyBoost* ratio *BOOST_RATIO_MANIPULATION_SCALE;
// limit to max amount
if (boostAmountOuter > dutyBoost) boostAmountOuter = dutyBoost;
// -- calculate inner tire reduction --
float reductionAmountInner = (data.radius * dutyRange + dutyOffset) * ratio;
//--- experimental alternative control mode ---
if (altStickMapping == true){
if (config->altStickMapping == true){
//swap BOTTOM_LEFT and BOTTOM_RIGHT
if (data.position == joystickPos_t::BOTTOM_LEFT){
data.position = joystickPos_t::BOTTOM_RIGHT;
@ -375,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altSt
case joystickPos_t::TOP_RIGHT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break;
case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset;
commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break;
case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break;
case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset;
commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break;
}
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
// log input data
ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
// log generation details
ESP_LOGI(TAG_CMD, "left=%.2f, right=%.2f -- BoostOuter=%.1f, ReductionInner=%.1f, maxDuty=%.0f, maxBoost=%.0f, dutyOffset=%.0f",
commands.left.duty, commands.right.duty,
boostAmountOuter, reductionAmountInner,
config->maxDutyStraight, dutyBoost, dutyOffset);
// log generated motor commands
ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGD(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
return commands;
}
@ -418,13 +438,21 @@ uint32_t shake_timestamp_turnedOn = 0;
uint32_t shake_timestamp_turnedOff = 0;
bool shake_state = false;
joystickPos_t lastStickPos = joystickPos_t::CENTER;
//stick position quadrant only with "X_AXIS and Y_AXIS" as hysteresis
joystickPos_t stickQuadrant = joystickPos_t::CENTER;
//--- configure shake mode --- TODO: move this to config
uint32_t shake_msOffMax = 80;
uint32_t shake_msOffMax = 60;
uint32_t shake_msOnMax = 120;
float dutyShake = 60;
uint32_t shake_minDelay = 20; //min time in ms motor stays on/off
float dutyShakeMax = 30;
float dutyShakeMin = 5;
inline void invertMotorDirection(motorstate_t *state)
{
if (*state == motorstate_t::FWD)
*state = motorstate_t::REV;
else
*state = motorstate_t::FWD;
}
//function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
@ -432,25 +460,29 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
//--- handle pulsing shake variable ---
//TODO remove this, make individual per mode?
//TODO only run this when not CENTER anyways?
motorCommands_t commands;
static motorCommands_t commands;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
static uint32_t cycleCount = 0;
//calculate on/off duration
uint32_t msOn = shake_msOnMax * data.radius;
uint32_t msOff = shake_msOffMax * data.radius;
float msOn = (shake_msOnMax - shake_minDelay) * data.radius + shake_minDelay;
float msOff = (shake_msOffMax - shake_minDelay) * data.radius + shake_minDelay;
float dutyShake = (dutyShakeMax - dutyShakeMin) * ratio + dutyShakeMin;
//evaluate state (on/off)
//evaluate state (motors on/off)
if (data.radius > 0 ){
//currently off
//currently off:
if (shake_state == false){
//off long enough
if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) {
//turn on
cycleCount++;
shake_state = true;
shake_timestamp_turnedOn = esp_log_timestamp();
ESP_LOGD(TAG_CMD, "shake: cycleCount=%d, msOn=%f, msOff=%f, radius=%f, shakeDuty=%f", cycleCount, msOn, msOff, data.radius, dutyShake);
}
}
//currently on
//currently on:
else {
//on long enough
if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) {
@ -475,79 +507,49 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
// float angle;
//} joystickData_t;
//--- evaluate stick position ---
//4 quadrants and center only - with X and Y axis as hysteresis
switch (data.position){
case joystickPos_t::CENTER:
//immediately set to center at center
stickQuadrant = joystickPos_t::CENTER;
break;
case joystickPos_t::Y_AXIS:
//when moving from center to axis initially start in a certain quadrant
if (stickQuadrant == joystickPos_t::CENTER) {
if (data.y > 0){
stickQuadrant = joystickPos_t::TOP_RIGHT;
} else {
stickQuadrant = joystickPos_t::BOTTOM_RIGHT;
}
}
break;
case joystickPos_t::X_AXIS:
//when moving from center to axis initially start in a certain quadrant
if (stickQuadrant == joystickPos_t::CENTER) {
if (data.x > 0){
stickQuadrant = joystickPos_t::TOP_RIGHT;
} else {
stickQuadrant = joystickPos_t::TOP_LEFT;
}
}
break;
case joystickPos_t::TOP_RIGHT:
case joystickPos_t::TOP_LEFT:
case joystickPos_t::BOTTOM_LEFT:
case joystickPos_t::BOTTOM_RIGHT:
//update/change evaluated pos when in one of the 4 quadrants
stickQuadrant = data.position;
//TODO: maybe beep when switching mode? (difficult because beep object has to be passed to function)
break;
// force off when stick pos changes - TODO: is this necessary?
static joystickPos_t stickPosPrev = joystickPos_t::CENTER;
if (data.position != stickPosPrev) {
ESP_LOGW(TAG, "massage: stick quadrant changed, stopping for one cycle");
shake_state = false;
shake_timestamp_turnedOff = esp_log_timestamp();
}
stickPosPrev = data.position; // update last position
//--- handle different modes (joystick in any of 4 quadrants) ---
switch (stickQuadrant){
switch (data.position){
// idle
case joystickPos_t::CENTER:
case joystickPos_t::X_AXIS: //never true
case joystickPos_t::Y_AXIS: //never true
commands.left.state = motorstate_t::IDLE;
commands.right.state = motorstate_t::IDLE;
commands.left.duty = 0;
commands.right.duty = 0;
ESP_LOGI(TAG_CMD, "generate shake commands: CENTER -> idle");
ESP_LOGD(TAG_CMD, "generate shake commands: CENTER -> idle");
return commands;
break;
//4 different modes
// shake forward/reverse
case joystickPos_t::X_AXIS:
case joystickPos_t::Y_AXIS:
case joystickPos_t::TOP_RIGHT:
case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
break;
case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
break;
// shake left right
case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::FWD;
break;
case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::REV;
break;
}
// change direction every second on cycle in any mode
//(to not start driving on average)
if (cycleCount % 2 == 0)
{
invertMotorDirection(&commands.left.state);
invertMotorDirection(&commands.right.state);
}
//--- turn motors on/off depending on pulsing shake variable ---
if (shake_state == true){
@ -562,11 +564,127 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
commands.right.duty = 0;
}
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f, cycleCount=%d, msOn=%f, msOff=%f", motorstateStr[(int)commands.left.state], commands.left.duty, cycleCount, msOn, msOff);
return commands;
}
// corresponding storage key strings to each joystickCalibratenMode variable
const char *calibrationStorageKeys[] = {"stick_x-min", "stick_x-max", "stick_y-min", "stick_y-max", "", ""};
//-------------------------------
//------- loadCalibration -------
//-------------------------------
// loads selected calibration value from nvs or default values from config if no data stored
void evaluatedJoystick::loadCalibration(joystickCalibrationMode_t mode)
{
// determine desired variables
int *configValue, *usedValue;
switch (mode)
{
case X_MIN:
configValue = &(config.x_min);
usedValue = &x_min;
break;
case X_MAX:
configValue = &(config.x_max);
usedValue = &x_max;
break;
case Y_MIN:
configValue = &(config.y_min);
usedValue = &y_min;
break;
case Y_MAX:
configValue = &(config.y_max);
usedValue = &y_max;
break;
case X_CENTER:
case Y_CENTER:
default:
// center position is not stored in nvs, it gets defined at startup or during calibration
ESP_LOGE(TAG, "loadCalibration: 'center_x' and 'center_y' are not stored in nvs -> not assigning anything");
// defineCenter();
return;
}
// read from nvs
int16_t valueRead;
esp_err_t err = nvs_get_i16(*nvsHandle, calibrationStorageKeys[(int)mode], &valueRead);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", calibrationStorageKeys[(int)mode], *configValue, valueRead);
*usedValue = (int)valueRead;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, loading default value %d", calibrationStorageKeys[(int)mode], *configValue);
*usedValue = *configValue;
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
*usedValue = *configValue;
}
}
//-------------------------------
//------- loadCalibration -------
//-------------------------------
// loads selected calibration value from nvs or default values from config if no data stored
void evaluatedJoystick::writeCalibration(joystickCalibrationMode_t mode, int newValue)
{
// determine desired variables
int *configValue, *usedValue;
switch (mode)
{
case X_MIN:
configValue = &(config.x_min);
usedValue = &x_min;
break;
case X_MAX:
configValue = &(config.x_max);
usedValue = &x_max;
break;
case Y_MIN:
configValue = &(config.y_min);
usedValue = &y_min;
break;
case Y_MAX:
configValue = &(config.y_max);
usedValue = &y_max;
break;
case X_CENTER:
x_center = newValue;
ESP_LOGW(TAG, "writeCalibration: 'center_x' or 'center_y' are not stored in nvs -> loading only");
return;
case Y_CENTER:
y_center = newValue;
ESP_LOGW(TAG, "writeCalibration: 'center_x' or 'center_y' are not stored in nvs -> loading only");
default:
return;
}
// check if unchanged
if (*usedValue == newValue)
{
ESP_LOGW(TAG, "writeCalibration: value '%s' unchanged at %d, not writing to nvs", calibrationStorageKeys[(int)mode], newValue);
return;
}
// update nvs value
ESP_LOGW(TAG, "writeCalibration: updating nvs value '%s' from %d to %d", calibrationStorageKeys[(int)mode], *usedValue, newValue);
esp_err_t err = nvs_set_i16(*nvsHandle, calibrationStorageKeys[(int)mode], newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
*usedValue = newValue;
}

59
common/joystick.hpp
View File

@ -8,6 +8,9 @@ extern "C"
#include "driver/adc.h"
#include "esp_log.h"
#include "esp_err.h"
#include "nvs_flash.h"
#include "nvs.h"
#include <stdbool.h>
}
#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};
extern const char* joystickPosStr[7];
typedef enum joystickCalibrationMode_t { X_MIN = 0, X_MAX, Y_MIN, Y_MAX, X_CENTER, Y_CENTER } joystickCalibrationMode_t;
//struct with current data of the joystick
typedef struct joystickData_t {
@ -65,36 +69,59 @@ typedef struct joystickData_t {
float angle;
} joystickData_t;
// struct with parameters provided to joystick_GenerateCommandsDriving()
typedef struct joystickGenerateCommands_config_t
{
float maxDutyStraight; // max duty applied when driving with ratio=1 (when turning it might increase by Boost)
float maxRelativeBoostPercentOfMaxDuty; // max duty percent added to outer tire when turning (max actual is 100-maxDutyStraight) - set 0 to disable
// note: to be able to reduce the overall driving speed boost has to be limited as well otherwise outer tire when turning would always be 100% no matter of maxDuty
float dutyOffset; // motors immediately start with this duty (duty movement starts)
float ratioSnapToOneThreshold; // have some area around X-Axis where inner tire is completely off - set 1 to disable
bool altStickMapping; // swap reverse direction
} joystickGenerateCommands_config_t;
//------------------------------------
//----- evaluatedJoystick class -----
//------------------------------------
class evaluatedJoystick {
public:
//--- constructor ---
evaluatedJoystick(joystick_config_t config_f);
class evaluatedJoystick
{
public:
//--- constructor ---
evaluatedJoystick(joystick_config_t config_f, nvs_handle_t * nvsHandle);
//--- functions ---
joystickData_t getData(); //read joystick, calculate values and return the data in a struct
void defineCenter(); //define joystick center from current position
//--- functions ---
joystickData_t getData(); // read joystick, calculate values and return the data in a struct
// get raw adc value (inversion applied)
int getRawX() { return readAdc(config.adc_x, config.x_inverted); }
int getRawY() { return readAdc(config.adc_y, config.y_inverted); }
void defineCenter(); // define joystick center from current position
void writeCalibration(joystickCalibrationMode_t mode, int newValue); // load certain new calibration value and store it in nvs
private:
//--- functions ---
//initialize adc inputs, define center
void init();
//read adc while making multiple samples with option to invert the result
int readAdc(adc1_channel_t adc_channel, bool inverted = false);
private:
//--- functions ---
// initialize adc inputs, define center
void init();
// loads selected calibration value from nvs or default values from config if no data stored
void loadCalibration(joystickCalibrationMode_t mode);
// read adc while making multiple samples with option to invert the result
int readAdc(adc1_channel_t adc_channel, bool inverted = false);
//--- variables ---
// handle for using the nvs flash (persistent config variables)
nvs_handle_t *nvsHandle;
joystick_config_t config;
int x_min;
int x_max;
int y_min;
int y_max;
int x_center;
int y_center;
joystickData_t data;
float x;
float y;
};
};
@ -103,7 +130,7 @@ class evaluatedJoystick {
//============================================
//function that generates commands for both motors from the joystick data
//motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick);
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altStickMapping = false);
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config);

651
common/motorctl.cpp
View File

@ -5,25 +5,47 @@
//tag for logging
static const char * TAG = "motor-control";
#define TIMEOUT_IDLE_WHEN_NO_COMMAND 8000
#define TIMEOUT_IDLE_WHEN_NO_COMMAND 15000 // turn motor off when still on and no new command received within that time
#define TIMEOUT_QUEUE_WHEN_AT_TARGET 5000 // time waited for new command when motors at target duty (e.g. IDLE) (no need to handle fading in fast loop)
//====================================
//========== motorctl task ===========
//====================================
//task for handling the motors (ramp, current limit, driver)
void task_motorctl( void * ptrControlledMotor ){
//get pointer to controlledMotor instance from task parameter
controlledMotor * motor = (controlledMotor *)ptrControlledMotor;
ESP_LOGW(TAG, "Task-motorctl [%s]: starting handle loop...", motor->getName());
while(1){
motor->handle();
vTaskDelay(20 / portTICK_PERIOD_MS);
}
}
//=============================
//======== constructor ========
//=============================
//constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':')
controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control):
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) {
controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f):
//create current sensor
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted),
configDefault(config_control){
//copy parameters for controlling the motor
config = config_control;
log = config.loggingEnabled;
//pointer to update motot dury method
motorSetCommand = setCommandFunc;
//copy configured default fading durations to actually used variables
msFadeAccel = config.msFadeAccel;
msFadeDecel = config.msFadeDecel;
//pointer to nvs handle
nvsHandle = nvsHandle_f;
//pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values
init();
//TODO: add currentsensor object here
//currentSensor cSensor(config.currentSensor_adc, config.currentSensor_ratedCurrent);
}
@ -33,7 +55,19 @@ controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl
//============================
void controlledMotor::init(){
commandQueue = xQueueCreate( 1, sizeof( struct motorCommand_t ) );
//cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle?
if (commandQueue == NULL)
ESP_LOGE(TAG, "Failed to create command-queue");
else
ESP_LOGI(TAG, "[%s] Initialized command-queue", config.name);
// load config values from nvs, otherwise use default from config object
loadAccelDuration();
loadDecelDuration();
// turn motor off initially
motorSetCommand({motorstate_t::IDLE, 0.00});
//cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle?
}
@ -78,83 +112,271 @@ void controlledMotor::handle(){
//TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
//--- receive commands from queue ---
if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) )
//--- RECEIVE DATA FROM QUEUE ---
if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already
{
ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty);
if(log) ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state;
dutyTarget = commandReceive.duty;
receiveTimeout = false;
timestamp_commandReceived = esp_log_timestamp();
}
// ----- EXPERIMENTAL, DIFFERENT MODES -----
// define target duty differently depending on current contro-mode
//declare variables used inside switch
float ampereNow, ampereTarget, ampereDiff;
float speedDiff;
switch (mode)
{
case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
//--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate
//this value is more suitable for the fading algorithm
switch(commandReceive.state){
case motorstate_t::BRAKE:
//update state
state = motorstate_t::BRAKE;
//dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = - fabs(commandReceive.duty);
break;
// define target duty (-100 to 100) from provided duty and motorstate
// this value is more suitable for t
// todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm
switch (commandReceive.state)
{
case motorstate_t::BRAKE:
// update state
state = motorstate_t::BRAKE;
// dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = -fabs(commandReceive.duty);
break;
}
}
break;
//--- timeout, no data ---
//turn motors off if no data received for long time (e.g. no uart data / control offline)
//TODO no timeout when braking?
if ((esp_log_timestamp() - timestamp_commandReceived) > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout){
receiveTimeout = true;
state = motorstate_t::IDLE;
dutyTarget = 0;
ESP_LOGE(TAG, "TIMEOUT, no target data received for more than %ds -> switch to IDLE", TIMEOUT_IDLE_WHEN_NO_COMMAND/1000);
}
#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 1 //difference from target where no change is made yet
#define CURRENT_CONTROL_MIN_AMPERE 0.7 //current where motor is turned off
//TODO define different, fixed fading configuration in current mode, fade down can be significantly less (500/500ms fade up worked fine)
case motorControlMode_t::CURRENT: // regulate to desired current flow
ampereNow = cSensor.read();
ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config
if (commandReceive.state == motorstate_t::REV) ampereTarget = - ampereTarget; //target is negative when driving reverse
ampereDiff = ampereTarget - ampereNow;
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", config.name, ampereNow, ampereTarget, ampereDiff); // todo handle brake
//--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
if (msFadeAccel > 0){
dutyIncrementAccel = ( usPassed / ((float)msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
//--- when IDLE to keep the current at target zero motor needs to be on for some duty (to compensate generator current)
if (commandReceive.duty == 0 && fabs(ampereNow) < CURRENT_CONTROL_MIN_AMPERE){ //stop motors completely when current is very low already
dutyTarget = 0;
}
else if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF || commandReceive.duty == 0) //#### BOOST BY 1 A
{
if (ampereDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase current
{
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
}
else if (ampereDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase current (more negative)
{
dutyTarget = -100;
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: set target to %.0f%%", config.name, dutyTarget);
}
else
{
dutyTarget = dutyNow; // target current reached
if(log) ESP_LOGD("TESTING", "[%s] CURRENT-CONTROL: target current %.3f reached", config.name, dutyTarget);
}
break;
#define SPEED_CONTROL_MAX_SPEED_KMH 10
#define SPEED_CONTROL_ALLOWED_KMH_DIFF 0.6
#define SPEED_CONTROL_MIN_SPEED 0.7 //" start from standstill" always accelerate to this speed, ignoring speedsensor data
case motorControlMode_t::SPEED: // regulate to desired speed
speedNow = sSensor->getKmph();
//caculate target speed from input
speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config
// target speed negative when driving reverse
if (commandReceive.state == motorstate_t::REV)
speedTarget = -speedTarget;
if (sSensor->getTimeLastUpdate() != timestamp_speedLastUpdate ){ //only modify duty when new speed data available
timestamp_speedLastUpdate = sSensor->getTimeLastUpdate(); //TODO get time only once
speedDiff = speedTarget - speedNow;
} else {
dutyIncrementAccel = 100;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: no new speed data, not changing duty", config.name);
speedDiff = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
//stop when target is 0
if (commandReceive.duty == 0) { //TODO add IDLE, BRAKE state
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: OFF, target is 0... current-speed=%.2f, diff=%.3f", config.name, speedNow, speedDiff);
dutyTarget = 0;
}
else if (fabs(speedNow) < SPEED_CONTROL_MIN_SPEED){ //start from standstill or too slow (not enough speedsensor data)
if (log)
ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: starting from standstill -> increase duty... target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
if (commandReceive.state == motorstate_t::FWD)
dutyTarget = 100;
else if (commandReceive.state == motorstate_t::REV)
dutyTarget = -100;
}
else if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF) //speed too fast/slow
{
if (speedDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase speed
{
// TODO retain max duty here
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (fwd), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
else if (speedDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase speed (more negative)
{
dutyTarget = -100;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (rev), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to high, diff=%.2f, decreasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
}
else
{
dutyTarget = dutyNow; // target speed reached
if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget);
}
break;
}
//calculate increment for fading DOWN with passed time since last run and configured fade time
if (msFadeDecel > 0){
dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
} else {
dutyIncrementDecel = 100;
//--- TIMEOUT NO DATA ---
// turn motors off if no data received for a long time (e.g. no uart data or control task offline)
if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
{
if(log) ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget);
receiveTimeout = true;
// set target and last command to IDLE
state = motorstate_t::IDLE;
commandReceive.state = motorstate_t::IDLE;
dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
commandReceive.duty = 0;
}
//--- CALCULATE DUTY-DIFF ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//negative: need to decrease
//--- DETECT ALREADY AT TARGET ---
// when already at exact target duty there is no need to run very fast to handle fading
//-> slow down loop by waiting significantly longer for new commands to arrive
if (mode != motorControlMode_t::CURRENT //dont slow down when in CURRENT mode at all
&& ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled && mode != motorControlMode_t::SPEED) //when neither of current-limit, tractioncontrol or speed-mode is enabled slow down when target reached
|| (dutyTarget == 0 && dutyNow == 0))) //otherwise only slow down when when actually off
{
//increase queue timeout when duty is the same (once)
if (timeoutWaitForCommand == 0)
{ // TODO verify if state matches too?
if(log) ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget);
timeoutWaitForCommand = TIMEOUT_QUEUE_WHEN_AT_TARGET; // wait in queue very long, for new command to arrive
}
vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed
}
//reset timeout when duty differs again (once)
else if (timeoutWaitForCommand != 0)
{
timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast
if(log) ESP_LOGI(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget);
// adjust lastRun timestamp to not mess up fading, due to much time passed but with no actual duty change
timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay
}
//TODO skip rest of the handle function below using return? Some regular driver updates sound useful though
//--- BRAKE ---
//brake immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){
ESP_LOGD(TAG, "braking - skip fading");
if(log) ESP_LOGD(TAG, "braking - skip fading");
motorSetCommand({motorstate_t::BRAKE, dutyTarget});
ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow);
if(log) ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//dutyNow = 0;
return; //no need to run the fade algorithm
}
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//negative: need to decrease
//----- FADING -----
//fade duty to target (up and down)
//calculate passed time since last run
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
//--- calculate increment (acceleration) ---
//calculate increment for fading UP with passed time since last run and configured fade time
//- traction control -
if (tcs_isExceeded) // disable acceleration when slippage is currently detected
dutyIncrementAccel = 0;
//- recent braking -
//FIXME reset timeout when duty less
else if (isBraking && (esp_log_timestamp() - timestampBrakeStart) < config.brakePauseBeforeResume) // prevent immediate direction change when currently braking with timeout (eventually currently sliding)
{
if (log) ESP_LOGI(TAG, "pause after brake... -> accel = 0");
dutyIncrementAccel = 0;
}
//- normal accel -
else if (config.msFadeAccel > 0)
dutyIncrementAccel = (usPassed / ((float)config.msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
//- sport mode -
else //no accel limit (immediately set to 100)
dutyIncrementAccel = 100;
//--- calculate increment (deceleration) ---
//- sport mode -
if (config.msFadeDecel == 0){ //no decel limit (immediately reduce to 0)
dutyIncrementDecel = 100;
}
//- brake -
//detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast)
#define NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY 10 //TODO test/adjust this - dont brake when slow already (avoids starting full dead time)
else if (commandReceive.state != state && // direction differs
fabs(dutyNow) > NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY && // not very slow already
fabs(dutyTarget) > brakeStartThreshold) // joystick above threshold
{
// set braking state and track start time (both for disabling acceleration for some time)
if (!isBraking) {
if (log) ESP_LOGW(TAG, "started braking...");
timestampBrakeStart = esp_log_timestamp();
isBraking = true;
}
// use brake deceleration instead of normal deceleration
dutyIncrementDecel = (usPassed / ((float)config.brakeDecel * 1000)) * 100;
if(log) ESP_LOGI(TAG, "braking (target duty >%.0f%% in other direction) -> using deceleration %dms", brakeStartThreshold, config.brakeDecel);
}
//- normal deceleration -
else {
// normal deceleration according to configured time
dutyIncrementDecel = (usPassed / ((float)config.msFadeDecel * 1000)) * 100;
}
// reset braking state when start condition is no longer met (stick below threshold again)
if (isBraking &&
(fabs(dutyTarget) < brakeStartThreshold || commandReceive.state == state))
{
ESP_LOGW(TAG, "brake condition no longer met");
isBraking = false;
}
//--- fade duty to target (up and down) ---
//TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
if (dutyNow < 0) { //reverse, decelerating
@ -174,25 +396,103 @@ void controlledMotor::handle(){
}
//----- CURRENT LIMIT -----
//----- CURRENT LIMIT -----
currentNow = cSensor.read();
if ((config.currentLimitEnabled) && (dutyDelta != 0)){
if (fabs(currentNow) > config.currentMax){
float dutyOld = dutyNow;
//adaptive decrement:
//Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values
dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)msFadeDecel * 1500) ) * 100;
dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)config.msFadeDecel * 1500) ) * 100;
float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0
if (dutyNow < -currentLimitDecrement) {
dutyNow += currentLimitDecrement;
} else if (dutyNow > currentLimitDecrement) {
dutyNow -= currentLimitDecrement;
}
ESP_LOGW(TAG, "current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", currentNow, config.currentMax, dutyOld, dutyNow);
if(log) ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow);
}
}
//----- TRACTION CONTROL -----
//reduce duty when turning faster than expected
//TODO only run this when speed sensors actually updated
//handle tcs when enabled and new speed sensor data is available TODO: currently assumes here that speed sensor data of other motor updated as well
#define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down
#define TCS_NO_SPEED_DATA_TIMEOUT_US 200*1000
#define TCS_MIN_SPEED_KMH 1 //must be at least that fast for TCS to be enabled
//TODO rework this: clearer structure (less nested if statements)
if (config.tractionControlSystemEnabled && mode == motorControlMode_t::SPEED && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate && (esp_timer_get_time() - tcs_timestampLastRun < TCS_NO_SPEED_DATA_TIMEOUT_US)){
//update last speed update received
tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate
//calculate time passed since last run
uint32_t tcs_usPassed = esp_timer_get_time() - tcs_timestampLastRun; // passed time since last time handled
tcs_timestampLastRun = esp_timer_get_time();
//get motor stats
float speedNowThis = sSensor->getKmph();
float speedNowOther = (*ppOtherMotor)->getCurrentSpeed();
float speedTargetThis = speedTarget;
float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
float dutyTargetThis = dutyTarget;
float dutyNowOther = (*ppOtherMotor)->getDuty();
float dutyNowThis = dutyNow;
//calculate expected ratio
float ratioSpeedTarget = speedTargetThis / speedTargetOther;
//calculate current ratio of actual measured rotational speed
float ratioSpeedNow = speedNowThis / speedNowOther;
//calculate current duty ration (logging only)
float ratioDutyNow = dutyNowThis / dutyNowOther;
//calculate unexpected difference
float ratioDiff = ratioSpeedNow - ratioSpeedTarget;
if(log) ESP_LOGD("TESTING", "[%s] TCS: speedThis=%.3f, speedOther=%.3f, ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, speedNowThis, speedNowOther, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff);
//-- handle rotating faster than expected --
//TODO also increase duty when other motor is slipping? (diff negative)
if (speedNowThis < TCS_MIN_SPEED_KMH) { //disable / turn off TCS when currently too slow (danger of deadlock)
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
else if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF ) // motor turns too fast compared to expected target ratio
{
if (!tcs_isExceeded) // just started being too fast
{
tcs_timestampBeginExceeded = esp_timer_get_time();
tcs_isExceeded = true; //also blocks further acceleration (fade)
if(log) ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF);
}
else
{ // too fast for more than 2 cycles already
tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already
if(log) ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff);
// calculate amount duty gets decreased
float dutyDecrement = (tcs_usPassed / ((float)config.msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded
// decrease duty
if(log) ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement);
fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0
}
}
else
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
}
else // TCS mode not active or timed out
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
//--- define new motorstate --- (-100 to 100 => direction)
state=getStateFromDuty(dutyNow);
@ -202,25 +502,27 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only?
if ( //not enough time between last direction state
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs))
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs))
){
ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
if (!deadTimeWaiting){ //log start
deadTimeWaiting = true;
ESP_LOGW(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
}
//force IDLE state during wait
state = motorstate_t::IDLE;
dutyNow = 0;
} else {
if (deadTimeWaiting){ //log end
deadTimeWaiting = false;
ESP_LOGW(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]);
}
ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
if (config.deadTimeMs > 0) { //deadTime is enabled
if ( //not enough time between last direction state
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs))
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs))
){
if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
if (!deadTimeWaiting){ //log start
deadTimeWaiting = true;
if(log) ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
}
//force IDLE state during wait
state = motorstate_t::IDLE;
dutyNow = 0;
} else {
if (deadTimeWaiting){ //log end
deadTimeWaiting = false;
if(log) ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]);
}
if(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
}
//--- save current actual motorstate and timestamp ---
@ -232,7 +534,7 @@ void controlledMotor::handle(){
//--- apply new target to motor ---
motorSetCommand({state, (float)fabs(dutyNow)});
ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow);
if(log) ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//note: BRAKE state is handled earlier
@ -247,17 +549,18 @@ void controlledMotor::handle(){
//===============================
//function to set the target mode and duty of a motor
//puts the provided command in a queue for the handle function running in another task
void controlledMotor::setTarget(motorstate_t state_f, float duty_f){
commandSend = {
.state = state_f,
.duty = duty_f
};
ESP_LOGD(TAG, "Inserted command to queue: state=%s, duty=%.2f", motorstateStr[(int)commandSend.state], commandSend.duty);
void controlledMotor::setTarget(motorCommand_t commandSend){
if(log) ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty);
//send command to queue (overwrite if an old command is still in the queue and not processed)
xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 );
if(log) ESP_LOGD(TAG, "finished inserting new command");
}
// accept target state and duty as separate agrguments:
void controlledMotor::setTarget(motorstate_t state_f, float duty_f){
// create motorCommand struct from the separate parameters, and run the method to insert that new command
setTarget({state_f, duty_f});
}
@ -277,20 +580,63 @@ motorCommand_t controlledMotor::getStatus(){
//===============================
//=========== getFade ===========
//===============================
//return currently configured accel / decel time
uint32_t controlledMotor::getFade(fadeType_t fadeType){
switch(fadeType){
case fadeType_t::ACCEL:
return config.msFadeAccel;
break;
case fadeType_t::DECEL:
return config.msFadeDecel;
break;
}
return 0;
}
//==============================
//======= getFadeDefault =======
//==============================
//return default accel / decel time (from config)
uint32_t controlledMotor::getFadeDefault(fadeType_t fadeType){
switch(fadeType){
case fadeType_t::ACCEL:
return configDefault.msFadeAccel;
break;
case fadeType_t::DECEL:
return configDefault.msFadeDecel;
break;
}
return 0;
}
//===============================
//=========== setFade ===========
//===============================
//function for editing or enabling the fading/ramp of the motor control
//set/update fading duration/amount
void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew){
void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew, bool writeToNvs){
//TODO: mutex for msFade variable also used in handle function
switch(fadeType){
case fadeType_t::ACCEL:
msFadeAccel = msFadeNew;
ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, config.msFadeAccel, msFadeNew);
if (writeToNvs)
writeAccelDuration(msFadeNew);
else
config.msFadeAccel = msFadeNew;
break;
case fadeType_t::DECEL:
msFadeDecel = msFadeNew;
ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, config.msFadeDecel, msFadeNew);
// write new value to nvs and update the variable
if (writeToNvs)
writeDecelDuration(msFadeNew);
else
config.msFadeDecel = msFadeNew;
break;
}
}
@ -324,16 +670,16 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
bool enabled = false;
switch(fadeType){
case fadeType_t::ACCEL:
if (msFadeAccel == 0){
msFadeNew = config.msFadeAccel;
if (config.msFadeAccel == 0){
msFadeNew = configDefault.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
}
break;
case fadeType_t::DECEL:
if (msFadeDecel == 0){
msFadeNew = config.msFadeAccel;
if (config.msFadeDecel == 0){
msFadeNew = configDefault.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
@ -347,3 +693,114 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
return enabled;
}
//-----------------------------
//----- loadAccelDuration -----
//-----------------------------
// load stored value from nvs if not successfull uses config default value
void controlledMotor::loadAccelDuration(void)
{
// read from nvs
uint32_t valueNew;
char key[15];
snprintf(key, 15, "m-%s-accel", config.name);
esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, configDefault.msFadeAccel, valueNew);
config.msFadeAccel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeAccel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
}
}
//-----------------------------
//----- loadDecelDuration -----
//-----------------------------
void controlledMotor::loadDecelDuration(void)
{
// read from nvs
uint32_t valueNew;
char key[15];
snprintf(key, 15, "m-%s-decel", config.name);
esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeDecel, valueNew);
config.msFadeDecel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeDecel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
}
}
//------------------------------
//----- writeAccelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local config
void controlledMotor::writeAccelDuration(uint32_t newValue)
{
// check if unchanged
if(config.msFadeAccel == newValue){
ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue);
return;
}
// generate nvs storage key
char key[15];
snprintf(key, 15, "m-%s-accel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeAccel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
config.msFadeAccel = newValue;
}
//------------------------------
//----- writeDecelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local config
// TODO: reduce duplicate code
void controlledMotor::writeDecelDuration(uint32_t newValue)
{
// check if unchanged
if(config.msFadeDecel == newValue){
ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue);
return;
}
// generate nvs storage key
char key[15];
snprintf(key, 15, "m-%s-decel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeDecel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
config.msFadeDecel = newValue;
}

81
common/motorctl.hpp
View File

@ -7,10 +7,13 @@ extern "C"
#include "freertos/queue.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "nvs_flash.h"
#include "nvs.h"
}
#include "motordrivers.hpp"
#include "currentsensor.hpp"
#include "speedsensor.hpp"
//=======================================
@ -21,6 +24,7 @@ extern "C"
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//===================================
//====== controlledMotor class ======
@ -28,49 +32,88 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor {
public:
//--- functions ---
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
//TODO move speedsensor object creation in this class to (pass through / wrap methods)
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle, speedSensor * speedSensor, controlledMotor ** otherMotor); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task)
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command);
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
float getDuty() {return dutyNow;};
float getTargetDuty() {return dutyTarget;};
float getTargetSpeed() {return speedTarget;};
float getCurrentSpeed() {return sSensor->getKmph();};
void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
void setControlMode(motorControlMode_t newMode) {mode = newMode;};
void setBrakeStartThresholdDuty(float duty) {brakeStartThreshold = duty;};
void setBrakeDecel(uint32_t msFadeBrake) {config.brakeDecel = msFadeBrake;};
uint32_t getBrakeDecel() {return config.brakeDecel;}; //todo store and load from nvs
uint32_t getBrakeDecelDefault() {return configDefault.brakeDecel;};
uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time
uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config
void setFade(fadeType_t fadeType, bool enabled); //enable/disable acceleration or deceleration fading
void setFade(fadeType_t fadeType, uint32_t msFadeNew); //set acceleration or deceleration fade time
void setFade(fadeType_t fadeType, uint32_t msFadeNew, bool writeToNvs = true); //set acceleration or deceleration fade time and write it to nvs by default
bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
float getCurrentA() {return cSensor.read();}; //read current-sensor of this motor (Ampere)
char * getName() const {return config.name;};
//TODO set current limit method
private:
//--- functions ---
void init(); //creates currentsensor objects, motordriver objects and queue
void init(); // creates command-queue and initializes config values
void loadAccelDuration(void); // load stored value for msFadeAccel from nvs
void loadDecelDuration(void);
void writeAccelDuration(uint32_t newValue); // write value to nvs and update local variable
void writeDecelDuration(uint32_t newValue);
//--- objects ---
//queue for sending commands to the separate task running the handle() function very fast
QueueHandle_t commandQueue = NULL;
//current sensor
currentSensor cSensor;
//speed sensor
speedSensor * sSensor;
//other motor (needed for traction control)
controlledMotor ** ppOtherMotor; //ptr to ptr of controlledMotor (because not created at initialization yet)
//function pointer that sets motor duty (driver)
motorSetCommandFunc_t motorSetCommand;
//--- variables ---
//TODO add name for logging?
//struct for storing control specific parameters
motorctl_config_t config;
const motorctl_config_t configDefault; //backup default configuration (unchanged)
bool log = false;
motorstate_t state = motorstate_t::IDLE;
motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode
//handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle;
float currentMax;
float currentNow;
float dutyTarget;
float dutyNow;
//speed mode
float speedTarget = 0;
float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0;
float dutyNow = 0;
float dutyIncrementAccel;
float dutyIncrementDecel;
float dutyDelta;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
uint32_t timeoutWaitForCommand = 0;
uint32_t ramp;
int64_t timestampLastRunUs;
int64_t timestampLastRunUs = 0;
bool deadTimeWaiting = false;
uint32_t timestampsModeLastActive[4] = {};
@ -81,4 +124,24 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0;
bool receiveTimeout = false;
//traction control system
uint32_t tcs_timestampLastSpeedUpdate = 0; //track speedsensor update
int64_t tcs_timestampBeginExceeded = 0; //track start of event
uint32_t tcs_usExceeded = 0; //sum up time
bool tcs_isExceeded = false; //is currently too fast
int64_t tcs_timestampLastRun = 0;
//brake (decel boost)
uint32_t timestampBrakeStart = 0;
bool isBraking = false;
float brakeStartThreshold = 60;
};
//====================================
//========== motorctl task ===========
//====================================
// note: pointer to a 'controlledMotor' object has to be provided as task-parameter
// runs handle method of certain motor repeatedly:
// receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr)
void task_motorctl( void * controlledMotor );

167
common/speedsensor.cpp
View File

@ -7,6 +7,9 @@
static const char* TAG = "speedSensor";
//initialize ISR only once (for multiple instances)
bool speedSensor::isrIsInitialized = false;
uint32_t min(uint32_t a, uint32_t b){
if (a>b) return b;
@ -16,63 +19,82 @@ uint32_t min(uint32_t a, uint32_t b){
//=========================================
//========== ISR onEncoderChange ==========
//========== ISR onEncoderRising ==========
//=========================================
//handle gpio edge event
//handle gpio rising edge event
//determines direction and rotational speed with a speedSensor object
void IRAM_ATTR onEncoderChange(void* arg) {
speedSensor* sensor = (speedSensor*)arg;
void IRAM_ATTR onEncoderRising(void *arg)
{
speedSensor *sensor = (speedSensor *)arg;
int currentState = gpio_get_level(sensor->config.gpioPin);
//detect rising edge LOW->HIGH (reached end of gap in encoder disk)
if (currentState == 1 && sensor->prevState == 0) {
//time since last edge in us
uint32_t currentTime = esp_timer_get_time();
uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
sensor->lastEdgeTime = currentTime; //update last edge time
// time since last edge in us
uint32_t currentTime = esp_timer_get_time();
uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
sensor->lastEdgeTime = currentTime; // update last edge time
//store duration of last pulse
sensor->pulseDurations[sensor->pulseCounter] = timeElapsed;
sensor->pulseCounter++;
// store duration of last pulse
sensor->pulseDurations[sensor->pulseCounter] = timeElapsed;
sensor->pulseCounter++;
//check if 3rd pulse has occoured
if (sensor->pulseCounter >= 3) {
sensor->pulseCounter = 0; //reset counter
// check if 3rd pulse has occoured (one sequence recorded)
if (sensor->pulseCounter >= 3)
{
sensor->pulseCounter = 0; // reset count
//simplify variable names
uint32_t pulse1 = sensor->pulseDurations[0];
uint32_t pulse2 = sensor->pulseDurations[1];
uint32_t pulse3 = sensor->pulseDurations[2];
// simplify variable names
uint32_t pulse1 = sensor->pulseDurations[0];
uint32_t pulse2 = sensor->pulseDurations[1];
uint32_t pulse3 = sensor->pulseDurations[2];
//find shortest pulse
uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3));
// save all recored pulses of this sequence (for logging only)
sensor->pulse1 = pulse1;
sensor->pulse2 = pulse2;
sensor->pulse3 = pulse3;
//Determine direction based on pulse order
int directionNew = 0;
if (shortestPulse == pulse1) { //short-medium-long...
directionNew = 1; //fwd
} else if (shortestPulse == pulse3) { //long-medium-short...
directionNew = -1; //rev
} else if (shortestPulse == pulse2) {
if (pulse1 < pulse3){ //medium short long-medium-short long...
directionNew = -1; //rev
} else { //long short-medium-long short-medium-long...
directionNew = 1; //fwd
}
}
// find shortest pulse
sensor->shortestPulse = min(pulse1, min(pulse2, pulse3));
//save and invert direction if necessay
//TODO mutex?
if (sensor->config.directionInverted) sensor->direction = -directionNew;
else sensor->direction = directionNew;
//calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
// ignore this pulse sequence if one pulse is too short (possible noise)
if (sensor->shortestPulse < sensor->config.minPulseDurationUs)
{
sensor->debug_countIgnoredSequencesTooShort++;
return;
}
//--- Determine direction based on pulse order ---
int direction = 0;
if (sensor->shortestPulse == pulse1) // short...
{
if (pulse2 < pulse3) // short-medium-long -->
direction = 1;
else // short-long-medium <--
direction = -1;
}
else if (sensor->shortestPulse == pulse3) //...short
{
if (pulse1 > pulse2) // long-medium-short <--
direction = -1;
else // medium-long-short -->
direction = 1;
}
else if (sensor->shortestPulse == pulse2) //...short...
{
if (pulse1 < pulse3) // medium-short-long
direction = -1;
else // long-short-medium
direction = 1;
}
// save and invert direction if necessay
if (sensor->config.directionInverted)
direction = -direction;
// calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
sensor->timeLastUpdate = currentTime;
}
//store current pin state for next edge detection
sensor->prevState = currentState;
}
@ -84,11 +106,8 @@ void IRAM_ATTR onEncoderChange(void* arg) {
speedSensor::speedSensor(speedSensor_config_t config_f){
//copy config
config = config_f;
//note: currently gets initialized at first method call
//this prevents crash due to too early initialization at boot
//TODO: create global objects later after boot
//init gpio and ISR
//init();
init();
}
@ -102,15 +121,16 @@ void speedSensor::init() {
gpio_pad_select_gpio(config.gpioPin);
gpio_set_direction(config.gpioPin, GPIO_MODE_INPUT);
gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY);
ESP_LOGW(TAG, "%s, configured gpio-pin %d", config.logName, (int)config.gpioPin);
//configure interrupt
gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE);
gpio_install_isr_service(0);
gpio_isr_handler_add(config.gpioPin, onEncoderChange, this);
ESP_LOGW(TAG, "%s, configured interrupt", config.logName);
isInitialized = true;
gpio_set_intr_type(config.gpioPin, GPIO_INTR_POSEDGE);
if (!isrIsInitialized) {
gpio_install_isr_service(0);
isrIsInitialized = true;
ESP_LOGW(TAG, "Initialized ISR service");
}
gpio_isr_handler_add(config.gpioPin, onEncoderRising, this);
ESP_LOGW(TAG, "[%s], configured gpio-pin %d and interrupt routine", config.logName, (int)config.gpioPin);
}
@ -121,41 +141,38 @@ void speedSensor::init() {
//==========================
//get rotational speed in revolutions per minute
float speedSensor::getRpm(){
//check if initialized
if (!isInitialized) init();
uint32_t timeElapsed = esp_timer_get_time() - lastEdgeTime;
//timeout (standstill)
//TODO variable timeout considering config.degreePerGroup
if ((currentRpm != 0) && (esp_timer_get_time() - lastEdgeTime) > TIMEOUT_NO_ROTATION*1000){
ESP_LOGW(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0",
ESP_LOGI(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0",
config.logName, TIMEOUT_NO_ROTATION, timeElapsed/1000);
currentRpm = 0;
}
//debug output (also log variables when this function is called)
ESP_LOGI(TAG, "%s - getRpm: returning stored rpm=%.3f", config.logName, currentRpm);
ESP_LOGV(TAG, "%s - rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d lastEdgetime=%d",
config.logName,
currentRpm,
direction,
pulseCounter,
(int)pulseDurations[0]/1000,
(int)pulseDurations[1]/1000,
(int)pulseDurations[2]/1000,
(int)lastEdgeTime);
ESP_LOGD(TAG, "[%s] getRpm: returning stored rpm=%.3f", config.logName, currentRpm);
ESP_LOGV(TAG, "[%s] rpm=%f, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, totalTooShortCount=%d",
config.logName,
currentRpm,
pulseCounter,
pulse1 / 1000,
pulse2 / 1000,
pulse3 / 1000,
shortestPulse / 1000,
debug_countIgnoredSequencesTooShort);
//return currently stored rpm
return currentRpm;
}
//==========================
//===========================
//========= getKmph =========
//==========================
//===========================
//get speed in kilometers per hour
float speedSensor::getKmph(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000;
ESP_LOGI(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed);
ESP_LOGD(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed);
return currentSpeed;
}
@ -165,7 +182,7 @@ float speedSensor::getKmph(){
//==========================
//get speed in meters per second
float speedSensor::getMps(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter;
ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60;
ESP_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
return currentSpeed;
}

26
common/speedsensor.hpp
View File

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

8
common/types.hpp
View File

@ -12,6 +12,7 @@ extern "C"
//====== struct/type declarations ======
//=======================================
//global structs and types that need to be available for all boards
//this file is necessary to prevent dependency loop between motordrivers.hpp and motorctl.hpp since
//===============================
@ -40,13 +41,20 @@ typedef struct motorCommands_t {
//struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t {
char * name; //name for unique nvs storage-key prefix and logging
bool loggingEnabled; //enable/disable ALL log output (mostly better to debug only one instance)
uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%)
uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
bool currentLimitEnabled;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent;
float currentMax;
bool currentInverted;
float currentSnapToZeroThreshold;
uint32_t deadTimeMs; //time motor stays in IDLE before direction change
uint32_t brakePauseBeforeResume;
uint32_t brakeDecel;
} motorctl_config_t;
//enum fade type (acceleration, deceleration)

84
common/wifi.c
View File

@ -21,26 +21,45 @@ static const char *TAG = "wifi";
static esp_event_handler_instance_t instance_any_id;
//============================================
//============ init nvs and netif ============
//============================================
//initialize nvs-flash and netif (needed for both AP and CLIENT)
//##########################################
//############ common functions ############
//##########################################
//============================
//========= init nvs =========
//============================
//initialize nvs-flash (needed for both AP and CLIENT)
//has to be run once at startup
void wifi_initNvs_initNetif(){
void wifi_initNvs(){
//Initialize NVS (needed for wifi)
esp_err_t ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
ret = nvs_flash_init();
}
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_LOGE(TAG, "NVS truncated -> deleting flash");
// Retry nvs_flash_init
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
ESP_ERROR_CHECK(err);
}
//==============================
//========= init netif =========
//==============================
//initialize netif (needed for both AP and CLIENT)
//has to be run once at startup
void wifi_initNetif(){
ESP_ERROR_CHECK(esp_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default());
}
//===========================================
//============ init access point ============
//===========================================
//############################################
//############### access point ###############
//############################################
//--------------------------------------------
//------ configuration / declarations --------
@ -66,10 +85,12 @@ static void wifi_event_handler_ap(void* arg, esp_event_base_t event_base,
}
}
//-----------------------
//------ init ap --------
//-----------------------
void wifi_init_ap(void)
//========================
//====== start AP ========
//========================
void wifi_start_ap(void)
{
ap = esp_netif_create_default_wifi_ap();
@ -107,9 +128,9 @@ void wifi_init_ap(void)
//=============================
//========= deinit AP =========
//========== stop AP ==========
//=============================
void wifi_deinit_ap(void)
void wifi_stop_ap(void)
{
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
@ -123,9 +144,9 @@ void wifi_deinit_ap(void)
//===========================================
//=============== init client ===============
//===========================================
//##########################################
//################# client #################
//##########################################
//--------------------------------------------
//------ configuration / declarations --------
@ -168,10 +189,13 @@ static void event_handler(void* arg, esp_event_base_t event_base,
xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
}
}
//---------------------------
//------ init client --------
//---------------------------
void wifi_init_client(void)
//===========================
//====== init client ========
//===========================
void wifi_start_client(void)
{
s_wifi_event_group = xEventGroupCreate();
sta = esp_netif_create_default_wifi_sta();
@ -249,10 +273,10 @@ void wifi_init_client(void)
//=================================
//========= deinit client =========
//=================================
void wifi_deinit_client(void)
//===============================
//========= stop client =========
//===============================
void wifi_stop_client(void)
{
/* The event will not be processed after unregister */
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));

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?
//initialize nvs-flash and netif (needed for both AP and CLIENT)
//has to be run once at startup
//Note: this cant be put in wifi_init functions because this may not be in deinit functions
void wifi_initNvs_initNetif();
//both functions have to be run once at startup
void wifi_initNvs();
void wifi_initNetif();
//function to start an access point
void wifi_init_ap(void);
//function to disable/deinit access point
void wifi_deinit_ap(void);
//function to start an access point (config in wifi.c)
void wifi_start_ap(void);
//function to disable/stop access point
void wifi_stop_ap(void);
//function to connect to existing wifi network
void wifi_init_client(void);
//function to connect to existing wifi network (config in wifi.c)
void wifi_start_client(void);
//function to disable/deinit client
void wifi_deinit_client(void);
void wifi_stop_client(void);

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];
} PACK8 out_column_t;
void ssd1306_init(SSD1306_t * dev, int width, int height)
//void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX) //original
void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX)
{
dev->_offsetX = offsetX;
if (dev->_address == SPIAddress) {
spi_init(dev, width, height);
} else {

4
components/ssd1306/ssd1306.h
View File

@ -98,6 +98,7 @@ typedef struct {
int _scDirection;
PAGE_t _page[8];
bool _flip;
int _offsetX; //added offset here instead of using macro variable
} SSD1306_t;
#ifdef __cplusplus
@ -105,7 +106,7 @@ extern "C"
{
#endif
void ssd1306_init(SSD1306_t * dev, int width, int height);
void ssd1306_init(SSD1306_t * dev, int width, int height, int offsetX);
int ssd1306_get_width(SSD1306_t * dev);
int ssd1306_get_height(SSD1306_t * dev);
int ssd1306_get_pages(SSD1306_t * dev);
@ -128,6 +129,7 @@ void _ssd1306_pixel(SSD1306_t * dev, int xpos, int ypos, bool invert);
void _ssd1306_line(SSD1306_t * dev, int x1, int y1, int x2, int y2, bool invert);
void ssd1306_invert(uint8_t *buf, size_t blen);
void ssd1306_flip(uint8_t *buf, size_t blen);
void ssd1306_setOffset(SSD1306_t * dev, int offset);
uint8_t ssd1306_copy_bit(uint8_t src, int srcBits, uint8_t dst, int dstBits);
uint8_t ssd1306_rotate_byte(uint8_t ch1);
void ssd1306_fadeout(SSD1306_t * dev);

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 (seg >= dev->_width) return;
int _seg = seg + CONFIG_OFFSETX;
//int _seg = seg + CONFIG_OFFSETX; //original
int _seg = seg + dev->_offsetX;
uint8_t columLow = _seg & 0x0F;
uint8_t columHigh = (_seg >> 4) & 0x0F;

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 (seg >= dev->_width) return;
int _seg = seg + CONFIG_OFFSETX;
//int _seg = seg + CONFIG_OFFSETX; //original
int _seg = seg + dev->_offsetX;
uint8_t columLow = _seg & 0x0F;
uint8_t columHigh = (_seg >> 4) & 0x0F;

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.