Add function 'joystick_generateCommandsDriving'
- add new function to joystick.hpp/cpp
- reads data from joystick and generates commands for driving in
"joystick" mode
- returns struct with commands for both motors
- main.cpp
- add code for testing the new function
- enable 5v regulator (needed for pullups AB left motor)
- add newly created motorRight to handle function
- add new struct with two motorcommands to motorctl.hpp
This commit is contained in:
jonny_ji7
parent
4109c6f239
commit
f7ce61c666
@ -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;
|
||||
}
|
||||
|
||||
@ -11,6 +11,7 @@ extern "C"
|
||||
}
|
||||
|
||||
#include <cmath>
|
||||
#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);
|
||||
|
||||
@ -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);
|
||||
// }
|
||||
|
||||
|
||||
|
||||
@ -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 = {};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user