Add speedSensor class - functional
successfully read rotational speed and direction using encoder disk and optical sensor custom implementation of incremental encoder using gpio edge interrupt
This commit is contained in:
jonny_jr9
parent
e3460a06ae
commit
a3257e89ee
@ -7,6 +7,7 @@ idf_component_register(
|
||||
"fan.cpp"
|
||||
"currentsensor.cpp"
|
||||
"uart.cpp"
|
||||
"speedsensor.cpp"
|
||||
INCLUDE_DIRS
|
||||
"."
|
||||
)
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
164
board_motorctl/main/speedsensor.cpp
Normal file
164
board_motorctl/main/speedsensor.cpp
Normal file
@ -0,0 +1,164 @@
|
||||
#include "speedsensor.hpp"
|
||||
#include "esp_timer.h"
|
||||
#include <ctime>
|
||||
|
||||
//===== 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;
|
||||
}
|
||||
53
board_motorctl/main/speedsensor.hpp
Normal file
53
board_motorctl/main/speedsensor.hpp
Normal file
@ -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:
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user