diff --git a/main/joystick.cpp b/main/joystick.cpp index 4a112fa..5415833 100644 --- a/main/joystick.cpp +++ b/main/joystick.cpp @@ -4,8 +4,9 @@ //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"}; -//tag for logging +//tags for logging static const char * TAG = "evaluatedJoystick"; +static const char * TAG_CMD = "joystickCommands"; @@ -58,9 +59,9 @@ int evaluatedJoystick::readAdc(adc1_channel_t adc_channel, bool inverted) { //return original or inverted result if (inverted) { - return adc_reading; - } else { return 4095 - adc_reading; + } else { + return adc_reading; } } @@ -186,3 +187,99 @@ void evaluatedJoystick::defineCenter(){ + + + +//============================================ +//========= joystick_CommandsDriving ========= +//============================================ +//function that generates commands for both motors from the joystick data +motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick){ + + + //struct with current data of the joystick + //typedef struct joystickData_t { + // joystickPos_t position; + // float x; + // float y; + // float radius; + // float angle; + //} joystickData_t; + + + joystickData_t data = joystick.getData(); + motorCommands_t commands; + float dutyMax = 60; //TODO add this to config, make changeable during runtime + + float dutyOffset = 5; //immedeately 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 + + 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*(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*(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*(1-ratio) + dutyOffset; //TODO remove offset? allow one motor only + 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*(1-ratio) + dutyOffset; //TODO remove offset? allow one motor only + 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; +} diff --git a/main/joystick.hpp b/main/joystick.hpp index 26c65ef..ea865e0 100644 --- a/main/joystick.hpp +++ b/main/joystick.hpp @@ -11,6 +11,7 @@ extern "C" } #include +#include "motorctl.hpp" //for deklaration of motorCommands_t struct //====================================== @@ -21,7 +22,6 @@ extern "C" // - calculates angle and radius // - defines an enum with position information - //-------------------------------------------- //---- struct, enum, variable deklarations --- //-------------------------------------------- @@ -99,3 +99,12 @@ class evaluatedJoystick { float y; }; + + + + +//============================================ +//========= joystick_CommandsDriving ========= +//============================================ +//function that generates commands for both motors from the joystick data +motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick); diff --git a/main/main.cpp b/main/main.cpp index 430baa1..d14a2c2 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -28,11 +28,12 @@ static const char * TAG = "main"; //==================================== //task for handling the motors (ramp, current limit, driver) void task_motorctl( void * pvParameters ){ - ESP_LOGI("motorctl-task", "starting handle loop..."); + ESP_LOGI(TAG, "starting handle loop..."); while(1){ + motorRight.handle(); motorLeft.handle(); //10khz -> T=100us - vTaskDelay(10 / portTICK_PERIOD_MS); + vTaskDelay(50 / portTICK_PERIOD_MS); } } @@ -42,17 +43,24 @@ void task_motorctl( void * pvParameters ){ //=========== app_main ============ //================================= extern "C" void app_main(void) { + //enable 5V volate regulator + gpio_pad_select_gpio(GPIO_NUM_17); + gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); + gpio_set_level(GPIO_NUM_17, 1); + //------------------------------- //---------- log level ---------- //------------------------------- //set loglevel for all tags: - esp_log_level_set("*", ESP_LOG_INFO); + esp_log_level_set("*", ESP_LOG_WARN); //set loglevel for individual tags: //esp_log_level_set("motordriver", ESP_LOG_DEBUG); //esp_log_level_set("motor-control", ESP_LOG_DEBUG); - esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); + //esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); + esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); + @@ -64,18 +72,27 @@ extern "C" void app_main(void) { while(1){ - vTaskDelay(100 / portTICK_PERIOD_MS); + vTaskDelay(400 / portTICK_PERIOD_MS); + + //--- testing joystick commands --- + motorCommands_t commands = joystick_generateCommandsDriving(joystick); + motorRight.setTarget(commands.right.state, commands.right.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly + motorLeft.setTarget(commands.left.state, commands.left.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly + //motorRight.setTarget(commands.right.state, commands.right.duty); + + + //--- testing joystick class --- - joystickData_t data = joystick.getData(); - ESP_LOGI(TAG, "position=%s, x=%.1f%%, y=%.1f%%, radius=%.1f%%, angle=%.2f", - joystickPosStr[(int)data.position], data.x*100, data.y*100, data.radius*100, data.angle); + //joystickData_t data = joystick.getData(); + //ESP_LOGI(TAG, "position=%s, x=%.1f%%, y=%.1f%%, radius=%.1f%%, angle=%.2f", + // joystickPosStr[(int)data.position], data.x*100, data.y*100, data.radius*100, data.angle); //--- testing the motor driver --- //fade up duty - forward // for (int duty=0; duty<=100; duty+=5) { // motorLeft.setTarget(motorstate_t::FWD, duty); - // vTaskDelay(100 / portTICK_PERIOD_MS); + // vTaskDelay(100 / portTICK_PERIOD_MS); // } diff --git a/main/motorctl.hpp b/main/motorctl.hpp index 6cdd7d6..ec5e159 100644 --- a/main/motorctl.hpp +++ b/main/motorctl.hpp @@ -12,8 +12,23 @@ extern "C" #include "motordrivers.hpp" +//------------------------------------- +//-------- struct declarations ------- +//------------------------------------- +//struct for sending command for one motor in the queue +struct motorCommand_t { + motorstate_t state; + float duty; +}; +//struct containing commands for two motors +typedef struct motorCommands_t { + motorCommand_t left; + motorCommand_t right; +} motorCommands_t; + +//struct with all config parameters for a motor regarding ramp and current limit typedef struct motorctl_config_t { uint32_t msFade; //acceleration of the motor (ms it should take from 0 to 100%) float currentMax; @@ -57,11 +72,6 @@ class controlledMotor { uint32_t ramp; int64_t timestampLastRunUs; - //structs for sending commands in the queue - struct motorCommand_t { - motorstate_t state; - float duty; - }; struct motorCommand_t commandReceive = {}; struct motorCommand_t commandSend = {};