Outsource http, joystick to common/ and react-app to root
Same reason as before commit Note: some changes to http were necessary due to global object - untested! Also remove unneded duplicate components folder
This commit is contained in:
jonny_jr9
@@ -7,8 +7,10 @@ idf_component_register(
|
||||
"motordrivers.cpp"
|
||||
"motorctl.cpp"
|
||||
"currentsensor.cpp"
|
||||
"joystick.cpp"
|
||||
"http.cpp"
|
||||
INCLUDE_DIRS
|
||||
"."
|
||||
PRIV_REQUIRES nvs_flash
|
||||
PRIV_REQUIRES nvs_flash mdns json spiffs esp_http_server
|
||||
)
|
||||
|
||||
|
||||
273
common/http.cpp
Normal file
273
common/http.cpp
Normal file
@@ -0,0 +1,273 @@
|
||||
extern "C"
|
||||
{
|
||||
#include <stdio.h>
|
||||
#include "mdns.h"
|
||||
#include "cJSON.h"
|
||||
#include "esp_spiffs.h"
|
||||
#include "esp_wifi.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
#include "freertos/queue.h"
|
||||
|
||||
}
|
||||
|
||||
#include "http.hpp"
|
||||
//#include "config.hpp"
|
||||
|
||||
|
||||
//tag for logging
|
||||
static const char * TAG = "http";
|
||||
static httpd_handle_t server = NULL;
|
||||
|
||||
|
||||
|
||||
//==============================
|
||||
//===== start mdns service =====
|
||||
//==============================
|
||||
//TODO: test this, not working?
|
||||
//function that initializes and starts mdns server for host discovery
|
||||
void start_mdns_service()
|
||||
{
|
||||
//init queue for sending joystickdata from http endpoint to control task
|
||||
mdns_init();
|
||||
mdns_hostname_set("armchair");
|
||||
mdns_instance_name_set("electric armchair");
|
||||
}
|
||||
|
||||
|
||||
|
||||
//===========================
|
||||
//======= default url =======
|
||||
//===========================
|
||||
//serve requested files from spiffs
|
||||
static esp_err_t on_default_url(httpd_req_t *req)
|
||||
{
|
||||
ESP_LOGI(TAG, "Opening page for URL: %s", req->uri);
|
||||
|
||||
esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = {
|
||||
.base_path = "/spiffs",
|
||||
.partition_label = NULL,
|
||||
.max_files = 5,
|
||||
.format_if_mount_failed = true};
|
||||
esp_vfs_spiffs_register(&esp_vfs_spiffs_conf);
|
||||
|
||||
char path[600];
|
||||
if (strcmp(req->uri, "/") == 0)
|
||||
strcpy(path, "/spiffs/index.html");
|
||||
else
|
||||
sprintf(path, "/spiffs%s", req->uri);
|
||||
char *ext = strrchr(path, '.');
|
||||
if (ext == NULL || strncmp(ext, ".local", strlen(".local")) == 0)
|
||||
{
|
||||
httpd_resp_set_status(req, "301 Moved Permanently");
|
||||
httpd_resp_set_hdr(req, "Location", "/");
|
||||
httpd_resp_send(req, NULL, 0);
|
||||
return ESP_OK;
|
||||
}
|
||||
if (strcmp(ext, ".css") == 0)
|
||||
httpd_resp_set_type(req, "text/css");
|
||||
if (strcmp(ext, ".js") == 0)
|
||||
httpd_resp_set_type(req, "text/javascript");
|
||||
if (strcmp(ext, ".png") == 0)
|
||||
httpd_resp_set_type(req, "image/png");
|
||||
|
||||
FILE *file = fopen(path, "r");
|
||||
if (file == NULL)
|
||||
{
|
||||
httpd_resp_send_404(req);
|
||||
esp_vfs_spiffs_unregister(NULL);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
char lineRead[256];
|
||||
while (fgets(lineRead, sizeof(lineRead), file))
|
||||
{
|
||||
httpd_resp_sendstr_chunk(req, lineRead);
|
||||
}
|
||||
httpd_resp_sendstr_chunk(req, NULL);
|
||||
|
||||
esp_vfs_spiffs_unregister(NULL);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//==============================
|
||||
//===== httpJoystick class =====
|
||||
//==============================
|
||||
//-----------------------
|
||||
//----- constructor -----
|
||||
//-----------------------
|
||||
httpJoystick::httpJoystick( httpJoystick_config_t config_f ){
|
||||
//copy config struct
|
||||
config = config_f;
|
||||
}
|
||||
|
||||
|
||||
//--------------------------
|
||||
//---- receiveHttpData -----
|
||||
//--------------------------
|
||||
//joystick endpoint - function that is called when data is received with post request at /api/joystick
|
||||
esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){
|
||||
//--- add header ---
|
||||
//to allow cross origin (otherwise browser fails when app is running on another host)
|
||||
httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
|
||||
|
||||
//--- get data from http request ---
|
||||
char buffer[100];
|
||||
memset(&buffer, 0, sizeof(buffer));
|
||||
httpd_req_recv(req, buffer, req->content_len);
|
||||
ESP_LOGD(TAG, "/api/joystick: received data: %s", buffer);
|
||||
|
||||
//--- parse received json string to json object ---
|
||||
cJSON *payload = cJSON_Parse(buffer);
|
||||
ESP_LOGV(TAG, "parsed json: \n %s", cJSON_Print(payload));
|
||||
|
||||
//--- extract relevant items from json object ---
|
||||
cJSON *x_json = cJSON_GetObjectItem(payload, "x");
|
||||
cJSON *y_json = cJSON_GetObjectItem(payload, "y");
|
||||
|
||||
//--- save items to struct ---
|
||||
joystickData_t data = { };
|
||||
|
||||
//note cjson can only interpret values as numbers when there are no quotes around the values in json (are removed from json on client side)
|
||||
//convert json to double to float
|
||||
data.x = static_cast<float>(x_json->valuedouble);
|
||||
data.y = static_cast<float>(y_json->valuedouble);
|
||||
//log received and parsed values
|
||||
ESP_LOGI(TAG, "received values: x=%.3f y=%.3f",
|
||||
data.x, data.y);
|
||||
|
||||
// scaleCoordinate(input, min, max, center, tolerance_zero_per, tolerance_end_per)
|
||||
data.x = scaleCoordinate(data.x+1, 0, 2, 1, config.toleranceZeroX_Per, config.toleranceEndPer);
|
||||
data.y = scaleCoordinate(data.y+1, 0, 2, 1, config.toleranceZeroY_Per, config.toleranceEndPer);
|
||||
|
||||
//--- calculate radius with new/scaled coordinates ---
|
||||
data.radius = sqrt(pow(data.x,2) + pow(data.y,2));
|
||||
//TODO: radius tolerance? (as in original joystick func)
|
||||
//limit radius to 1
|
||||
if (data.radius > 1) {
|
||||
data.radius = 1;
|
||||
}
|
||||
//--- calculate angle ---
|
||||
data.angle = (atan(data.y/data.x) * 180) / 3.141;
|
||||
//--- evaluate position ---
|
||||
data.position = joystick_evaluatePosition(data.x, data.y);
|
||||
|
||||
//log processed values
|
||||
ESP_LOGI(TAG, "processed values: x=%.3f y=%.3f radius=%.3f angle=%.3f pos=%s",
|
||||
data.x, data.y, data.radius, data.angle, joystickPosStr[(int)data.position]);
|
||||
|
||||
//--- free memory ---
|
||||
cJSON_Delete(payload);
|
||||
|
||||
//--- send data to control task via queue ---
|
||||
//xQueueSend( joystickDataQueue, ( void * )&data, ( TickType_t ) 0 );
|
||||
//changed to length = 1 -> overwrite - older values are no longer relevant
|
||||
xQueueOverwrite( joystickDataQueue, ( void * )&data );
|
||||
|
||||
//--- return http response ---
|
||||
httpd_resp_set_status(req, "204 NO CONTENT");
|
||||
httpd_resp_send(req, NULL, 0);
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
|
||||
//-------------------
|
||||
//----- getData -----
|
||||
//-------------------
|
||||
//wait for and return joystick data from queue, if timeout return NULL
|
||||
joystickData_t httpJoystick::getData(){
|
||||
|
||||
//--- get joystick data from queue ---
|
||||
if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(config.timeoutMs) ) ) {
|
||||
|
||||
ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f",
|
||||
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
|
||||
}
|
||||
//--- timeout ---
|
||||
//no new data received within configured timeout
|
||||
else {
|
||||
//send error message when last received data did NOT result in CENTER position
|
||||
if (dataRead.position != joystickPos_t::CENTER) {
|
||||
//change data to "joystick center" data to stop the motors
|
||||
dataRead = dataCenter;
|
||||
ESP_LOGE(TAG, "TIMEOUT - no data received for 3s -> set to center");
|
||||
}
|
||||
}
|
||||
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()
|
||||
{
|
||||
|
||||
//---- configure webserver ----
|
||||
httpd_config_t config = HTTPD_DEFAULT_CONFIG();
|
||||
config.uri_match_fn = httpd_uri_match_wildcard;
|
||||
|
||||
//---- start webserver ----
|
||||
ESP_ERROR_CHECK(httpd_start(&server, &config));
|
||||
|
||||
|
||||
//----- define URLs -----
|
||||
//note: dont use separate assignment of elements because causes controller crash
|
||||
httpd_uri_t joystick_url = {
|
||||
.uri = "/api/joystick",
|
||||
.method = HTTP_POST,
|
||||
.handler = on_joystick_url,
|
||||
};
|
||||
httpd_register_uri_handler(server, &joystick_url);
|
||||
|
||||
httpd_uri_t default_url = {
|
||||
.uri = "/*",
|
||||
.method = HTTP_GET,
|
||||
.handler = on_default_url};
|
||||
httpd_register_uri_handler(server, &default_url);
|
||||
|
||||
|
||||
//previous approach with sockets:
|
||||
// httpd_uri_t socket_joystick_url = {
|
||||
// .uri = "/ws-api/joystick",
|
||||
// .method = HTTP_GET,
|
||||
// .handler = on_socket_joystick_url,
|
||||
// .is_websocket = true};
|
||||
// httpd_register_uri_handler(server, &socket_joystick_url);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//============================
|
||||
//===== stop http server =====
|
||||
//============================
|
||||
//function that destroys the http server
|
||||
void http_stop_server()
|
||||
{
|
||||
printf("stopping http\n");
|
||||
httpd_stop(server);
|
||||
}
|
||||
|
||||
|
||||
|
||||
77
common/http.hpp
Normal file
77
common/http.hpp
Normal file
@@ -0,0 +1,77 @@
|
||||
#pragma once
|
||||
|
||||
extern "C"
|
||||
{
|
||||
#include "esp_http_server.h"
|
||||
}
|
||||
|
||||
#include "joystick.hpp"
|
||||
|
||||
|
||||
|
||||
//============================
|
||||
//===== init http server =====
|
||||
//============================
|
||||
//function that initializes http server and configures available urls
|
||||
void http_init_server();
|
||||
|
||||
|
||||
//==============================
|
||||
//===== start mdns service =====
|
||||
//==============================
|
||||
//function that initializes and starts mdns server for host discovery
|
||||
void start_mdns_service();
|
||||
|
||||
|
||||
//============================
|
||||
//===== stop http server =====
|
||||
//============================
|
||||
//function that destroys the http server
|
||||
void http_stop_server();
|
||||
|
||||
|
||||
//==============================
|
||||
//===== httpJoystick class =====
|
||||
//==============================
|
||||
//class that receices that from a HTTP post request, generates and scales joystick data and provides the data in a queue
|
||||
|
||||
//struct with configuration parameters
|
||||
typedef struct httpJoystick_config_t {
|
||||
float toleranceZeroX_Per;//percentage around joystick axis the coordinate snaps to 0
|
||||
float toleranceZeroY_Per;
|
||||
float toleranceEndPer; //percentage before joystick end the coordinate snaps to 1/-1
|
||||
uint32_t timeoutMs; //time no new data was received before the motors get turned off
|
||||
} httpJoystick_config_t;
|
||||
|
||||
|
||||
class httpJoystick{
|
||||
public:
|
||||
//--- constructor ---
|
||||
httpJoystick( httpJoystick_config_t config_f );
|
||||
|
||||
//--- functions ---
|
||||
joystickData_t getData(); //wait for and return joystick data from queue, if timeout return CENTER
|
||||
|
||||
esp_err_t receiveHttpData(httpd_req_t *req); //function that is called when data is received with post request at /api/joystick
|
||||
|
||||
private:
|
||||
//--- variables ---
|
||||
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;
|
||||
const joystickData_t dataCenter = {
|
||||
.position = joystickPos_t::CENTER,
|
||||
.x = 0,
|
||||
.y = 0,
|
||||
.radius = 0,
|
||||
.angle = 0
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
|
||||
//===== global object =====
|
||||
//create global instance of httpJoystick
|
||||
//note: is constructed/configured in config.cpp
|
||||
extern httpJoystick httpJoystickMain;
|
||||
572
common/joystick.cpp
Normal file
572
common/joystick.cpp
Normal file
@@ -0,0 +1,572 @@
|
||||
extern "C" {
|
||||
#include "hal/timer_types.h"
|
||||
}
|
||||
|
||||
#include "joystick.hpp"
|
||||
|
||||
|
||||
//definition of string array to be able to convert state enum to readable string
|
||||
const char* joystickPosStr[7] = {"CENTER", "Y_AXIS", "X_AXIS", "TOP_RIGHT", "TOP_LEFT", "BOTTOM_LEFT", "BOTTOM_RIGHT"};
|
||||
|
||||
//tags for logging
|
||||
static const char * TAG = "evaluatedJoystick";
|
||||
static const char * TAG_CMD = "joystickCommands";
|
||||
|
||||
|
||||
|
||||
|
||||
//-----------------------------
|
||||
//-------- constructor --------
|
||||
//-----------------------------
|
||||
//copy provided struct with all configuration and run init function
|
||||
evaluatedJoystick::evaluatedJoystick(joystick_config_t config_f){
|
||||
config = config_f;
|
||||
init();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//----------------------------
|
||||
//---------- init ------------
|
||||
//----------------------------
|
||||
void evaluatedJoystick::init(){
|
||||
ESP_LOGI(TAG, "initializing joystick");
|
||||
//initialize adc
|
||||
adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096
|
||||
|
||||
//FIXME: the following two commands each throw error
|
||||
//"ADC: adc1_lock_release(419): adc1 lock release called before acquire"
|
||||
//note: also happens for each get_raw for first call of readAdc function
|
||||
//when run in main function that does not happen -> move init from constructor to be called in main
|
||||
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
|
||||
|
||||
//define joystick center from current position
|
||||
defineCenter(); //define joystick center from current position
|
||||
}
|
||||
|
||||
|
||||
|
||||
//-----------------------------
|
||||
//--------- readAdc -----------
|
||||
//-----------------------------
|
||||
//function for multisampling an anlog input
|
||||
int evaluatedJoystick::readAdc(adc1_channel_t adc_channel, bool inverted) {
|
||||
//make multiple measurements
|
||||
int adc_reading = 0;
|
||||
for (int i = 0; i < 16; i++) {
|
||||
adc_reading += adc1_get_raw(adc_channel);
|
||||
ets_delay_us(50);
|
||||
}
|
||||
adc_reading = adc_reading / 16;
|
||||
|
||||
//return original or inverted result
|
||||
if (inverted) {
|
||||
return 4095 - adc_reading;
|
||||
} else {
|
||||
return adc_reading;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//-------------------------------
|
||||
//---------- getData ------------
|
||||
//-------------------------------
|
||||
//function that reads the joystick, calculates values and returns a struct with current data
|
||||
joystickData_t evaluatedJoystick::getData() {
|
||||
//get coordinates
|
||||
//TODO individual tolerances for each axis? Otherwise some parameters can be removed
|
||||
//TODO duplicate code for each axis below:
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
//calculate radius
|
||||
data.radius = sqrt(pow(data.x,2) + pow(data.y,2));
|
||||
if (data.radius > 1-config.tolerance_radius) {
|
||||
data.radius = 1;
|
||||
}
|
||||
|
||||
//calculate angle
|
||||
data.angle = (atan(data.y/data.x) * 180) / 3.141;
|
||||
|
||||
//define position
|
||||
data.position = joystick_evaluatePosition(x, y);
|
||||
|
||||
ESP_LOGD(TAG, "X=%.2f Y=%.2f radius=%.2f angle=%.2f", data.x, data.y, data.radius, data.angle);
|
||||
return data;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//----------------------------
|
||||
//------ defineCenter --------
|
||||
//----------------------------
|
||||
//function that defines the current position of the joystick as center position
|
||||
void evaluatedJoystick::defineCenter(){
|
||||
//read voltage from adc
|
||||
x_center = readAdc(config.adc_x, config.x_inverted);
|
||||
y_center = readAdc(config.adc_y, config.y_inverted);
|
||||
|
||||
ESP_LOGW(TAG, "defined center to x=%d, y=%d", x_center, y_center);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//==============================
|
||||
//====== scaleCoordinate =======
|
||||
//==============================
|
||||
//function that scales an input value (e.g. from adc pin) to a value from -1 to 1 using the given thresholds and tolerances
|
||||
float scaleCoordinate(float input, float min, float max, float center, float tolerance_zero_per, float tolerance_end_per) {
|
||||
|
||||
float coordinate = 0;
|
||||
|
||||
//convert tolerance percentages to actual values of range
|
||||
double tolerance_zero = (max-min) * tolerance_zero_per / 100;
|
||||
double tolerance_end = (max-min) * tolerance_end_per / 100;
|
||||
|
||||
//define coordinate value considering the different tolerances
|
||||
//--- center ---
|
||||
if ((input < center+tolerance_zero) && (input > center-tolerance_zero) ) { //adc value is inside tolerance around center threshold
|
||||
coordinate = 0;
|
||||
}
|
||||
//--- maximum ---
|
||||
else if (input > max-tolerance_end) {
|
||||
coordinate = 1;
|
||||
}
|
||||
//--- minimum ---
|
||||
else if (input < min+tolerance_end) {
|
||||
coordinate = -1;
|
||||
}
|
||||
//--- positive area ---
|
||||
else if (input > center) {
|
||||
float range = max - center - tolerance_zero - tolerance_end;
|
||||
coordinate = (input - center - tolerance_zero) / range;
|
||||
}
|
||||
//--- negative area ---
|
||||
else if (input < center) {
|
||||
float range = (center - min - tolerance_zero - tolerance_end);
|
||||
coordinate = -(center-input - tolerance_zero) / range;
|
||||
}
|
||||
|
||||
ESP_LOGD(TAG, "scaling: in=%.3f coordinate=%.3f, tolZero=%.3f, tolEnd=%.3f", input, coordinate, tolerance_zero, tolerance_end);
|
||||
//return coordinate (-1 to 1)
|
||||
return coordinate;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//===========================================
|
||||
//====== joystick_scaleCoordinatesExp =======
|
||||
//===========================================
|
||||
//local function that scales the absolute value of a variable exponentionally
|
||||
float scaleExp(float value, float exponent){
|
||||
float result = powf(fabs(value), exponent);
|
||||
if (value >= 0) {
|
||||
return result;
|
||||
} else {
|
||||
return -result;
|
||||
}
|
||||
}
|
||||
//function that updates a joystickData object with exponentionally scaling applied to coordinates
|
||||
void joystick_scaleCoordinatesExp(joystickData_t * data, float exponent){
|
||||
//scale x and y coordinate
|
||||
data->x = scaleExp(data->x, exponent);
|
||||
data->y = scaleExp(data->y, exponent);
|
||||
//re-calculate radius
|
||||
data->radius = sqrt(pow(data->x,2) + pow(data->y,2));
|
||||
if (data->radius > 1-0.07) {//FIXME hardcoded radius tolerance
|
||||
data->radius = 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//==============================================
|
||||
//====== joystick_scaleCoordinatesLinear =======
|
||||
//==============================================
|
||||
//local function that scales value from -1-1 to -1-1 with two different slopes before and after a specified point
|
||||
//slope1: for value from 0 to pointX -> scale linear from 0 to pointY
|
||||
//slope2: for value from pointX to 1 -> scale linear from pointY to 1
|
||||
float scaleLinPoint(float value, float pointX, float pointY){
|
||||
float result;
|
||||
if (fabs(value) <= pointX) {
|
||||
//--- scale on line from 0 to point ---
|
||||
result = fabs(value) * (pointY/pointX);
|
||||
} else {
|
||||
//--- scale on line from point to 1 ---
|
||||
float m = (1-pointY) / (1-pointX);
|
||||
result = fabs(value) * m + (1 - m);
|
||||
}
|
||||
|
||||
//--- return result with same sign as input ---
|
||||
if (value >= 0) {
|
||||
return result;
|
||||
} else {
|
||||
return -result;
|
||||
}
|
||||
}
|
||||
|
||||
//function that updates a joystickData object with linear scaling applied to coordinates
|
||||
//e.g. use to use more joystick resolution for lower speeds
|
||||
//TODO rename this function to more general name (scales not only coordinates e.g. adjusts radius, in future angle...)
|
||||
void joystick_scaleCoordinatesLinear(joystickData_t * data, float pointX, float pointY){
|
||||
// --- scale x and y coordinate --- DISABLED
|
||||
/*
|
||||
data->x = scaleLinPoint(data->x, pointX, pointY);
|
||||
data->y = scaleLinPoint(data->y, pointX, pointY);
|
||||
//re-calculate radius
|
||||
data->radius = sqrt(pow(data->x,2) + pow(data->y,2));
|
||||
if (data->radius > 1-0.1) {//FIXME hardcoded radius tolerance
|
||||
data->radius = 1;
|
||||
}
|
||||
*/
|
||||
|
||||
//note: issue with scaling X, Y coordinates:
|
||||
// - messed up radius calculation - radius never gets 1 at diagonal positions
|
||||
//==> only scaling radius as only speed should be more acurate at low radius:
|
||||
//TODO make that clear and rename function, since it does not scale coordinates - just radius
|
||||
|
||||
//--- scale radius ---
|
||||
data-> radius = scaleLinPoint(data->radius, pointX, pointY);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//=============================================
|
||||
//========= joystick_evaluatePosition =========
|
||||
//=============================================
|
||||
//function that defines and returns enum joystickPos from x and y coordinates
|
||||
joystickPos_t joystick_evaluatePosition(float x, float y){
|
||||
//define position
|
||||
//--- center ---
|
||||
if((fabs(x) == 0) && (fabs(y) == 0)){
|
||||
return joystickPos_t::CENTER;
|
||||
}
|
||||
//--- x axis ---
|
||||
else if(fabs(y) == 0){
|
||||
return joystickPos_t::X_AXIS;
|
||||
}
|
||||
//--- y axis ---
|
||||
else if(fabs(x) == 0){
|
||||
return joystickPos_t::Y_AXIS;
|
||||
}
|
||||
//--- top right ---
|
||||
else if(x > 0 && y > 0){
|
||||
return joystickPos_t::TOP_RIGHT;
|
||||
}
|
||||
//--- top left ---
|
||||
else if(x < 0 && y > 0){
|
||||
return joystickPos_t::TOP_LEFT;
|
||||
}
|
||||
//--- bottom left ---
|
||||
else if(x < 0 && y < 0){
|
||||
return joystickPos_t::BOTTOM_LEFT;
|
||||
}
|
||||
//--- bottom right ---
|
||||
else if(x > 0 && y < 0){
|
||||
return joystickPos_t::BOTTOM_RIGHT;
|
||||
}
|
||||
//--- other ---
|
||||
else {
|
||||
return joystickPos_t::CENTER;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//============================================
|
||||
//========= joystick_CommandsDriving =========
|
||||
//============================================
|
||||
//function that generates commands for both motors from the joystick data
|
||||
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, bool altStickMapping){
|
||||
|
||||
//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 = 90; //TODO add this to config, make changeable during runtime
|
||||
|
||||
float dutyOffset = 5; //immediately starts with this duty, TODO add this to config
|
||||
float dutyRange = dutyMax - dutyOffset;
|
||||
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
|
||||
|
||||
//--- snap ratio to max at angle threshold ---
|
||||
//(-> more joystick area where inner wheel is off when turning)
|
||||
/*
|
||||
//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
|
||||
*/
|
||||
|
||||
//--- experimental alternative control mode ---
|
||||
if (altStickMapping == true){
|
||||
//swap BOTTOM_LEFT and BOTTOM_RIGHT
|
||||
if (data.position == joystickPos_t::BOTTOM_LEFT){
|
||||
data.position = joystickPos_t::BOTTOM_RIGHT;
|
||||
}
|
||||
else if (data.position == joystickPos_t::BOTTOM_RIGHT){
|
||||
data.position = joystickPos_t::BOTTOM_LEFT;
|
||||
}
|
||||
}
|
||||
|
||||
//--- handle all positions ---
|
||||
//define target direction and duty according to position
|
||||
switch (data.position){
|
||||
|
||||
case joystickPos_t::CENTER:
|
||||
commands.left.state = motorstate_t::IDLE;
|
||||
commands.right.state = motorstate_t::IDLE;
|
||||
commands.left.duty = 0;
|
||||
commands.right.duty = 0;
|
||||
break;
|
||||
|
||||
case joystickPos_t::Y_AXIS:
|
||||
if (data.y > 0){
|
||||
commands.left.state = motorstate_t::FWD;
|
||||
commands.right.state = motorstate_t::FWD;
|
||||
} else {
|
||||
commands.left.state = motorstate_t::REV;
|
||||
commands.right.state = motorstate_t::REV;
|
||||
}
|
||||
commands.left.duty = fabs(data.y) * dutyRange + dutyOffset;
|
||||
commands.right.duty = commands.left.duty;
|
||||
break;
|
||||
|
||||
case joystickPos_t::X_AXIS:
|
||||
if (data.x > 0) {
|
||||
commands.left.state = motorstate_t::FWD;
|
||||
commands.right.state = motorstate_t::REV;
|
||||
} else {
|
||||
commands.left.state = motorstate_t::REV;
|
||||
commands.right.state = motorstate_t::FWD;
|
||||
}
|
||||
commands.left.duty = fabs(data.x) * dutyRange + dutyOffset;
|
||||
commands.right.duty = commands.left.duty;
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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);
|
||||
return commands;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//============================================
|
||||
//========= joystick_CommandsShaking =========
|
||||
//============================================
|
||||
//--- variable declarations ---
|
||||
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_msOnMax = 120;
|
||||
float dutyShake = 60;
|
||||
|
||||
//function that generates commands for both motors from the joystick data
|
||||
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;
|
||||
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
|
||||
|
||||
//calculate on/off duration
|
||||
uint32_t msOn = shake_msOnMax * data.radius;
|
||||
uint32_t msOff = shake_msOffMax * data.radius;
|
||||
|
||||
//evaluate state (on/off)
|
||||
if (data.radius > 0 ){
|
||||
//currently off
|
||||
if (shake_state == false){
|
||||
//off long enough
|
||||
if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) {
|
||||
//turn on
|
||||
shake_state = true;
|
||||
shake_timestamp_turnedOn = esp_log_timestamp();
|
||||
}
|
||||
}
|
||||
//currently on
|
||||
else {
|
||||
//on long enough
|
||||
if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) {
|
||||
//turn off
|
||||
shake_state = false;
|
||||
shake_timestamp_turnedOff = esp_log_timestamp();
|
||||
}
|
||||
}
|
||||
}
|
||||
//joystick is at center
|
||||
else {
|
||||
shake_state = false;
|
||||
shake_timestamp_turnedOff = esp_log_timestamp();
|
||||
}
|
||||
|
||||
//struct with current data of the joystick
|
||||
//typedef struct joystickData_t {
|
||||
// joystickPos_t position;
|
||||
// float x;
|
||||
// float y;
|
||||
// float radius;
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
//--- handle different modes (joystick in any of 4 quadrants) ---
|
||||
switch (stickQuadrant){
|
||||
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");
|
||||
return commands;
|
||||
break;
|
||||
//4 different modes
|
||||
case joystickPos_t::TOP_RIGHT:
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
//--- turn motors on/off depending on pulsing shake variable ---
|
||||
if (shake_state == true){
|
||||
//set duty to shake
|
||||
commands.left.duty = dutyShake;
|
||||
commands.right.duty = dutyShake;
|
||||
//directions are defined above depending on mode
|
||||
} else {
|
||||
commands.left.state = motorstate_t::IDLE;
|
||||
commands.right.state = motorstate_t::IDLE;
|
||||
commands.left.duty = 0;
|
||||
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);
|
||||
|
||||
return commands;
|
||||
}
|
||||
153
common/joystick.hpp
Normal file
153
common/joystick.hpp
Normal file
@@ -0,0 +1,153 @@
|
||||
#pragma once
|
||||
|
||||
extern "C"
|
||||
{
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/adc.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_err.h"
|
||||
}
|
||||
|
||||
#include <cmath>
|
||||
#include "types.hpp"
|
||||
|
||||
|
||||
//======================================
|
||||
//========= evaluated Joystick =========
|
||||
//======================================
|
||||
//class which evaluates a joystick with 2 analog signals
|
||||
// - scales the adc input to coordinates with detailed tolerances
|
||||
// - calculates angle and radius
|
||||
// - defines an enum with position information
|
||||
|
||||
//--------------------------------------------
|
||||
//---- struct, enum, variable declarations ---
|
||||
//--------------------------------------------
|
||||
//struct with all required configuration parameters
|
||||
typedef struct joystick_config_t {
|
||||
//analog inputs the axis are connected
|
||||
adc1_channel_t adc_x;
|
||||
adc1_channel_t adc_y;
|
||||
|
||||
//percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
|
||||
int tolerance_zeroX_per;
|
||||
int tolerance_zeroY_per;
|
||||
//percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100)
|
||||
int tolerance_end_per;
|
||||
//threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
|
||||
float tolerance_radius;
|
||||
|
||||
//min and max adc values of each axis
|
||||
int x_min;
|
||||
int x_max;
|
||||
int y_min;
|
||||
int y_max;
|
||||
|
||||
//invert adc measurement (e.g. when moving joystick up results in a decreasing voltage)
|
||||
bool x_inverted;
|
||||
bool y_inverted;
|
||||
} joystick_config_t;
|
||||
|
||||
|
||||
//enum for describing the position of the joystick
|
||||
enum class joystickPos_t {CENTER, Y_AXIS, X_AXIS, TOP_RIGHT, TOP_LEFT, BOTTOM_LEFT, BOTTOM_RIGHT};
|
||||
extern const char* joystickPosStr[7];
|
||||
|
||||
|
||||
//struct with current data of the joystick
|
||||
typedef struct joystickData_t {
|
||||
joystickPos_t position;
|
||||
float x;
|
||||
float y;
|
||||
float radius;
|
||||
float angle;
|
||||
} joystickData_t;
|
||||
|
||||
|
||||
|
||||
//------------------------------------
|
||||
//----- evaluatedJoystick class -----
|
||||
//------------------------------------
|
||||
class evaluatedJoystick {
|
||||
public:
|
||||
//--- constructor ---
|
||||
evaluatedJoystick(joystick_config_t config_f);
|
||||
|
||||
//--- functions ---
|
||||
joystickData_t getData(); //read joystick, calculate values and return the data in a struct
|
||||
void defineCenter(); //define joystick center from current position
|
||||
|
||||
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);
|
||||
|
||||
//--- variables ---
|
||||
joystick_config_t config;
|
||||
int x_center;
|
||||
int y_center;
|
||||
|
||||
joystickData_t data;
|
||||
float x;
|
||||
float y;
|
||||
};
|
||||
|
||||
|
||||
|
||||
//============================================
|
||||
//========= joystick_CommandsDriving =========
|
||||
//============================================
|
||||
//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);
|
||||
|
||||
|
||||
|
||||
//============================================
|
||||
//========= joystick_CommandsShaking =========
|
||||
//============================================
|
||||
//function that generates commands for both motors from the joystick data
|
||||
//motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick);
|
||||
motorCommands_t joystick_generateCommandsShaking(joystickData_t data );
|
||||
|
||||
|
||||
|
||||
//==============================
|
||||
//====== scaleCoordinate =======
|
||||
//==============================
|
||||
//function that scales an input value (e.g. from adc pin) to a value from -1 to 1 using the giben thresholds and tolerances
|
||||
float scaleCoordinate(float input, float min, float max, float center, float tolerance_zero_per, float tolerance_end_per);
|
||||
|
||||
|
||||
|
||||
//===========================================
|
||||
//====== joystick_scaleCoordinatesExp =======
|
||||
//===========================================
|
||||
//function that updates a joystickData object with exponentionally scaling applied to coordinates
|
||||
//e.g. use to use more joystick resolution for lower speeds
|
||||
void joystick_scaleCoordinatesExp(joystickData_t * data, float exponent);
|
||||
|
||||
|
||||
|
||||
//==============================================
|
||||
//====== joystick_scaleCoordinatesLinear =======
|
||||
//==============================================
|
||||
//function that updates a joystickData object with linear scaling applied to coordinates
|
||||
//scales coordinates with two different slopes before and after a specified point
|
||||
//slope1: for value from 0 to pointX -> scale linear from 0 to pointY
|
||||
//slope2: for value from pointX to 1 -> scale linear from pointY to 1
|
||||
//=> best to draw the lines and point in a graph
|
||||
//e.g. use to use more joystick resolution for lower speeds
|
||||
void joystick_scaleCoordinatesLinear(joystickData_t * data, float pointX, float pointY);
|
||||
|
||||
|
||||
|
||||
//=============================================
|
||||
//========= joystick_evaluatePosition =========
|
||||
//=============================================
|
||||
//function that defines and returns enum joystickPos from x and y coordinates
|
||||
joystickPos_t joystick_evaluatePosition(float x, float y);
|
||||
Reference in New Issue
Block a user