diff --git a/board_motorctl/main/CMakeLists.txt b/board_motorctl/main/CMakeLists.txt index f6c8f85..d9a0937 100644 --- a/board_motorctl/main/CMakeLists.txt +++ b/board_motorctl/main/CMakeLists.txt @@ -4,6 +4,7 @@ idf_component_register( "config.cpp" "fan.cpp" "uart.cpp" + "speedsensor.cpp" INCLUDE_DIRS "." ) diff --git a/board_motorctl/main/main.cpp b/board_motorctl/main/main.cpp index 4b527c3..c3b6870 100644 --- a/board_motorctl/main/main.cpp +++ b/board_motorctl/main/main.cpp @@ -14,12 +14,20 @@ extern "C" #include "driver/ledc.h" -//custom C files + //custom C files #include "wifi.h" } //custom C++ files #include "config.hpp" #include "uart.hpp" +#include "speedsensor.hpp" + + +//============================ +//======= TESTING MODE ======= +//============================ +//do not start the actual tasks for controlling the armchair +#define TESTING_MODE //========================= //======= UART TEST ======= @@ -32,14 +40,18 @@ extern "C" //======= BRAKE TEST ======= //========================== //only run brake-test (ignore uart input) -#define BRAKE_TEST_ONLY +//#define BRAKE_TEST_ONLY +//====================-====== +//==== SPEED SENSOR TEST ==== +//======================-==== +//only run speed-sensor test +#define SPEED_SENSOR_TEST //tag for logging static const char * TAG = "main"; -#ifndef UART_TEST_ONLY //==================================== //========== motorctl task =========== //==================================== @@ -110,22 +122,25 @@ void setLoglevels(void){ esp_log_level_set("uart_common", ESP_LOG_INFO); esp_log_level_set("uart", ESP_LOG_INFO); //esp_log_level_set("current-sensors", ESP_LOG_INFO); + esp_log_level_set("speedSensor", ESP_LOG_WARN); } -#endif + //================================= //=========== app_main ============ //================================= extern "C" void app_main(void) { -#ifndef UART_TEST_ONLY + + //---- define log levels ---- + setLoglevels(); + +#ifndef TESTING_MODE //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); - //---- define log levels ---- - setLoglevels(); //---------------------------------------------- //--- create task for controlling the motors --- @@ -152,20 +167,42 @@ extern "C" void app_main(void) { +#ifndef BRAKE_TEST_ONLY //------------------------------------------- //--- create tasks for uart communication --- //------------------------------------------- -#ifndef BRAKE_TEST_ONLY uart_init(); xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL); xTaskCreate(task_uartSend, "task_uartSend", 4096, NULL, 10, NULL); #endif - //--- main loop --- + +#ifdef SPEED_SENSOR_TEST + speedSensor_config_t speedRight_config{ + .gpioPin = GPIO_NUM_18, + .degreePerGroup = 72, + .tireCircumferenceMeter = 0.69, + .directionInverted = false, + .logName = "speedRight", + }; + speedSensor speedRight (speedRight_config); +#endif + + + //--------------------------- + //-------- main loop -------- + //--------------------------- //does nothing except for testing things while(1){ - vTaskDelay(500 / portTICK_PERIOD_MS); + +#ifdef SPEED_SENSOR_TEST + vTaskDelay(100 / portTICK_PERIOD_MS); + //speedRight.getRpm(); + ESP_LOGI(TAG, "speedsensor-test: rpm=%fRPM, speed=%fkm/h dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d lastEdgetime=%d", speedRight.getRpm(), speedRight.getKmph(), speedRight.direction, speedRight.pulseCounter, (int)speedRight.pulseDurations[0]/1000, (int)speedRight.pulseDurations[1]/1000, (int)speedRight.pulseDurations[2]/1000,(int)speedRight.lastEdgeTime); +#endif + + #ifdef BRAKE_TEST_ONLY //test relays at standstill ESP_LOGW("brake-test", "test relays via motorctl"); @@ -198,7 +235,6 @@ extern "C" void app_main(void) { vTaskDelay(5000 / portTICK_PERIOD_MS); //reset to idle motorRight.setTarget(motorstate_t::IDLE, 0); - #endif diff --git a/board_motorctl/main/speedsensor.cpp b/board_motorctl/main/speedsensor.cpp new file mode 100644 index 0000000..7d717d7 --- /dev/null +++ b/board_motorctl/main/speedsensor.cpp @@ -0,0 +1,164 @@ +#include "speedsensor.hpp" +#include "esp_timer.h" +#include + +//===== config ===== +#define TIMEOUT_NO_ROTATION 1000 //RPM set to 0 when no pulses within that time (ms) +static const char* TAG = "speedSensor"; + + + +uint32_t min(uint32_t a, uint32_t b){ + if (a>b) return b; + else return a; +} + + + +//========================================= +//========== ISR onEncoderChange ========== +//========================================= +//handle gpio edge event +//determines direction and rotational speed with a speedSensor object +void IRAM_ATTR onEncoderChange(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 + + //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 + + //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)); + + //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 + } + } + + //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)); + } + } + //store current pin state for next edge detection + sensor->prevState = currentState; +} + + + + +//============================ +//======= constructor ======== +//============================ +speedSensor::speedSensor(speedSensor_config_t config_f){ + //copy config + config = config_f; + //init gpio and ISR + init(); +} + + + +//========================== +//========== init ========== +//========================== + //initializes gpio pin and configures interrupt +void speedSensor::init() { + //configure pin + 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); +} + + + + +//========================== +//========= getRpm ========= +//========================== +//get rotational speed in revolutions per minute +float speedSensor::getRpm(){ + 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", + 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); + + //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); + return currentSpeed; +} + + +//========================== +//========= getMps ========= +//========================== +//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); + return currentSpeed; +} diff --git a/board_motorctl/main/speedsensor.hpp b/board_motorctl/main/speedsensor.hpp new file mode 100644 index 0000000..eb4c97e --- /dev/null +++ b/board_motorctl/main/speedsensor.hpp @@ -0,0 +1,53 @@ +#pragma once +extern "C" { +#include "esp_log.h" +#include "hal/gpio_types.h" +#include "driver/gpio.h" +#include "esp_timer.h" +} + +//Encoder disk requirements: +//encoder disk has to have gaps in 3 differnt intervals (short, medium, long) +//that pattern can be repeated multiple times, see config option +typedef struct { + gpio_num_t gpioPin; + float degreePerGroup; //360 / [count of short,medium,long groups on encoder disk] + float tireCircumferenceMeter; + //positive direction is pulse order "short, medium, long" + bool directionInverted; + char* logName; +} speedSensor_config_t; + + +class speedSensor { + //TODO add count of revolutions/pulses if needed? (get(), reset() etc) +public: + //constructor + speedSensor(speedSensor_config_t config); + //initializes gpio pin and configures 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 + + //1=forward, -1=reverse + int direction; + + //variables for handling the encoder + speedSensor_config_t config; + int prevState = 0; + uint64_t pulseDurations[3] = {}; + uint64_t lastEdgeTime = 0; + uint8_t pulseCounter = 0; + int debugCount = 0; + double currentRpm = 0; + +private: + +}; + + +