joppe/BBOT
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Formatted and added bmi task

Browse Source
This commit is contained in:
Joppe Blondel
2026-03-19 19:27:06 +01:00
parent f5f47c26fd
commit 00e354c2b3
7 changed files with 284 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
main/bbot.c
View File

@@ -5,14 +5,13 @@
#include "esp_err.h" #include "esp_err.h"
#include "esp_log.h" #include "esp_log.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h" #include "freertos/task.h"
#include "motors.h" #include "motors.h"
#include "bmi160.h" #include "bmi160.h"
#define BBOT_INPUT_PIN GPIO_NUM_1 #define BBOT_DRDY_INPUT_PIN GPIO_NUM_1
#define BBOT_BLINK_PIN GPIO_NUM_8
#define BBOT_BLINK_DELAY_MS 500
#define BBOT_I2C_PORT I2C_NUM_0 #define BBOT_I2C_PORT I2C_NUM_0
#define BBOT_I2C_SCL_IO GPIO_NUM_3 #define BBOT_I2C_SCL_IO GPIO_NUM_3
@@ -20,33 +19,10 @@
#define BBOT_I2C_FREQ_HZ 100000 #define BBOT_I2C_FREQ_HZ 100000
static bmi160_t imu; static bmi160_t imu;
static QueueHandle_t imu_queue;
static void init_blink_gpio(){ static esp_err_t init_i2c()
const gpio_config_t io_conf = { {
.pin_bit_mask = (1ULL << BBOT_BLINK_PIN),
.mode = GPIO_MODE_OUTPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE,
};
gpio_config(&io_conf);
gpio_set_level(BBOT_BLINK_PIN, 0);
}
static void init_input_gpio(){
const gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << BBOT_INPUT_PIN),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE,
};
gpio_config(&io_conf);
}
static void init_i2c(){
const i2c_config_t i2c_conf = { const i2c_config_t i2c_conf = {
.mode = I2C_MODE_MASTER, .mode = I2C_MODE_MASTER,
.sda_io_num = BBOT_I2C_SDA_IO, .sda_io_num = BBOT_I2C_SDA_IO,
@@ -58,22 +34,21 @@ static void init_i2c(){
ESP_ERROR_CHECK(i2c_param_config(BBOT_I2C_PORT, &i2c_conf)); ESP_ERROR_CHECK(i2c_param_config(BBOT_I2C_PORT, &i2c_conf));
ESP_ERROR_CHECK(i2c_driver_install(BBOT_I2C_PORT, i2c_conf.mode, 0, 0, 0)); ESP_ERROR_CHECK(i2c_driver_install(BBOT_I2C_PORT, i2c_conf.mode, 0, 0, 0));
return ESP_OK;
} }
void app_main(void){ void app_main(void)
{
ESP_ERROR_CHECK(init_motors());
ESP_ERROR_CHECK(init_i2c());
ESP_ERROR_CHECK(imu_init(&imu, BBOT_I2C_PORT, BBOT_DRDY_INPUT_PIN, &imu_queue));
init_input_gpio(); while (1)
init_blink_gpio(); {
init_motors();
init_i2c();
imu_init(&imu, BBOT_I2C_PORT);
while (1) {
vTaskDelay(pdMS_TO_TICKS(10));
bmi160_value_t v; bmi160_value_t v;
imu_read(&imu, &v); while (xQueueReceive(imu_queue, &v, pdMS_TO_TICKS(1000)) == pdTRUE)
ESP_LOGI("[<IMU>]", "%d %d %d %d %d %d", v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z); {
ESP_LOGI("[<IMU>]", "%d %d %d %d %d %d %d", v.time, v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z);
}
} }
} }

230
main/bmi160.c
View File

@@ -1,23 +1,145 @@
#include "bmi160.h" #include "bmi160.h"
#include <stdbool.h>
#include <string.h> #include <string.h>
#include "driver/gpio.h"
#include "esp_log.h" #include "esp_log.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h" #include "freertos/task.h"
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){ #define BMI160_MAX_WRITE_LEN 32
#define BMI160_SAMPLE_QUEUE_LEN 16
#define BMI160_QUEUE_MIN_FREE_SLOTS 1
#define BMI160_READ_TASK_STACK_SIZE 4096
#define BMI160_READ_TASK_PRIORITY 5
static bool s_gpio_isr_service_installed = false;
static void IRAM_ATTR bmi160_drdy_isr(void *arg)
{
bmi160_t *dev = (bmi160_t *)arg;
BaseType_t higher_priority_task_woken = pdFALSE;
if (dev != NULL && dev->read_task != NULL)
{
vTaskNotifyGiveFromISR(dev->read_task, &higher_priority_task_woken);
}
if (higher_priority_task_woken == pdTRUE)
{
portYIELD_FROM_ISR();
}
}
static void bmi160_read_task(void *arg)
{
bmi160_t *dev = (bmi160_t *)arg;
for (;;)
{
bmi160_value_t value;
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
if (imu_read(dev, &value) != ESP_OK)
{
continue;
}
if (dev->sample_queue == NULL)
{
continue;
}
if (uxQueueSpacesAvailable(dev->sample_queue) <= BMI160_QUEUE_MIN_FREE_SLOTS)
{
continue;
}
xQueueSend(dev->sample_queue, &value, 0);
}
}
static esp_err_t bmi160_init_streaming(bmi160_t *dev, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
{
const gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << drdy_io),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_POSEDGE,
};
if (queue_handle == NULL)
{
return ESP_ERR_INVALID_ARG;
}
ESP_ERROR_CHECK(gpio_config(&io_conf));
if (!s_gpio_isr_service_installed)
{
esp_err_t err = gpio_install_isr_service(0);
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
{
return err;
}
s_gpio_isr_service_installed = true;
}
dev->sample_queue = xQueueCreate(BMI160_SAMPLE_QUEUE_LEN, sizeof(bmi160_value_t));
if (dev->sample_queue == NULL)
{
return ESP_ERR_NO_MEM;
}
*queue_handle = dev->sample_queue;
BaseType_t task_ok = xTaskCreate(
bmi160_read_task,
"bmi160_read",
BMI160_READ_TASK_STACK_SIZE,
dev,
BMI160_READ_TASK_PRIORITY,
&dev->read_task);
if (task_ok != pdPASS)
{
vQueueDelete(dev->sample_queue);
dev->sample_queue = NULL;
*queue_handle = NULL;
return ESP_ERR_NO_MEM;
}
ESP_ERROR_CHECK(gpio_isr_handler_add(drdy_io, bmi160_drdy_isr, dev));
return ESP_OK;
}
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
{
esp_err_t err; esp_err_t err;
err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_LOW); err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_LOW);
if (err != ESP_OK) { if (err != ESP_OK)
{
ESP_LOGW("bmi160", "BMI160 not found at 0x%02X: %s", BMI160_I2C_ADDRESS_LOW, esp_err_to_name(err)); ESP_LOGW("bmi160", "BMI160 not found at 0x%02X: %s", BMI160_I2C_ADDRESS_LOW, esp_err_to_name(err));
err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_HIGH); err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_HIGH);
} }
if (err != ESP_OK) { if (err != ESP_OK)
{
ESP_LOGE("bmi160", "BMI160 probe failed at 0x%02X and 0x%02X: %s", ESP_LOGE("bmi160", "BMI160 probe failed at 0x%02X and 0x%02X: %s",
BMI160_I2C_ADDRESS_LOW, BMI160_I2C_ADDRESS_LOW,
BMI160_I2C_ADDRESS_HIGH, BMI160_I2C_ADDRESS_HIGH,
esp_err_to_name(err)); esp_err_to_name(err));
return err;
}
dev->drdy_io = drdy_io;
dev->sample_queue = NULL;
dev->read_task = NULL;
if (queue_handle != NULL)
{
*queue_handle = NULL;
} }
// Soft reset // Soft reset
@@ -28,11 +150,12 @@ esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
uint8_t chip_id; uint8_t chip_id;
ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id)); ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id));
ESP_LOGI("bmi160", "BMI160 chip id: 0x%02x at 0x%02x", chip_id, dev->i2c_address); ESP_LOGI("bmi160", "BMI160 chip id: 0x%02x at 0x%02x", chip_id, dev->i2c_address);
if(chip_id != BMI160_CHIP_ID){ if (chip_id != BMI160_CHIP_ID)
{
ESP_LOGE("bmi160", "BMI160 chip id not right"); ESP_LOGE("bmi160", "BMI160 chip id not right");
return ESP_FAIL; return ESP_FAIL;
} }
// Configure ACC: 100Hz, normal mode filter, no undersampling // Configure ACC: 100Hz, normal mode filter, no undersampling
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_CONF, 0x28)); ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_CONF, 0x28));
// Configure ACC range: +-16g // Configure ACC range: +-16g
@@ -51,64 +174,88 @@ esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
// Poll until active // Poll until active
bool startup_complete = false; bool startup_complete = false;
for(int timeout=0; timeout<1000; timeout+=10){ for (int timeout = 0; timeout < 1000; timeout += 10)
{
uint8_t status; uint8_t status;
ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_PMU_STATUS, &status)); ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_PMU_STATUS, &status));
int acc_pmu_status = (status & 0b00110000)>>4; int acc_pmu_status = (status & 0b00110000) >> 4;
int gyr_pmu_status = (status & 0b00001100)>>2; int gyr_pmu_status = (status & 0b00001100) >> 2;
if(acc_pmu_status==0b01 && gyr_pmu_status==0b01){ if (acc_pmu_status == 0b01 && gyr_pmu_status == 0b01)
{
startup_complete = true; startup_complete = true;
break; break;
} }
vTaskDelay(pdMS_TO_TICKS(10)); vTaskDelay(pdMS_TO_TICKS(10));
} }
if(!startup_complete){ if (!startup_complete)
{
ESP_LOGE("bmi160", "Acc or gyr not set in normal mode"); ESP_LOGE("bmi160", "Acc or gyr not set in normal mode");
} }
// enable DRDY interrupt
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_EN1, 0x10));
// enable INT1, active high push pull
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_OUT_CTRL, 0x0a));
// Disable INT1 input, non-latched
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_LATCH, 0x00));
// Map DRDY to INT1
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_MAP1, 0x80));
// Clear data registers // Clear data registers
uint8_t data[20]; uint8_t data[20];
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA)); ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
if (drdy_io >= 0 && queue_handle != NULL)
{
return bmi160_init_streaming(dev, drdy_io, queue_handle);
}
return ESP_OK; return ESP_OK;
} }
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value){ esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value)
if (dev == NULL || value == NULL) { {
if (dev == NULL || value == NULL)
{
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
uint8_t data[20]; uint8_t data[20];
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA)); ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
value->acc.x = data[14] | (data[15]<<8); value->acc.x = data[14] | (data[15] << 8);
value->acc.y = data[16] | (data[17]<<8); value->acc.y = data[16] | (data[17] << 8);
value->acc.z = data[18] | (data[19]<<8); value->acc.z = data[18] | (data[19] << 8);
value->gyr.x = data[8] | (data[9]<<8); value->gyr.x = data[8] | (data[9] << 8);
value->gyr.y = data[10] | (data[11]<<8); value->gyr.y = data[10] | (data[11] << 8);
value->gyr.z = data[12] | (data[13]<<8); value->gyr.z = data[12] | (data[13] << 8);
uint8_t time[3];
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME));
value->time = (time[0] | (time[1]<<8) | (time[2]<<16)) >> 8; // shift right to set resolution
return ESP_OK; return ESP_OK;
} }
// ---------------------------------------------------- // ----------------------------------------------------
// Low level driver // Low level driver
#define BMI160_MAX_WRITE_LEN 32 static esp_err_t bmi160_check_dev(const bmi160_t *dev)
{
static esp_err_t bmi160_check_dev(const bmi160_t *dev) { if (dev == NULL)
if (dev == NULL) { {
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
return ESP_OK; return ESP_OK;
} }
esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address) { esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address)
{
uint8_t chip_id = 0; uint8_t chip_id = 0;
esp_err_t err; esp_err_t err;
if (dev == NULL) { if (dev == NULL)
{
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
@@ -117,32 +264,39 @@ esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address) {
dev->timeout_ticks = pdMS_TO_TICKS(BMI160_DEFAULT_TIMEOUT_MS); dev->timeout_ticks = pdMS_TO_TICKS(BMI160_DEFAULT_TIMEOUT_MS);
err = bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id); err = bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id);
if (err != ESP_OK) { if (err != ESP_OK)
{
return err; return err;
} }
if (chip_id != BMI160_CHIP_ID) { if (chip_id != BMI160_CHIP_ID)
{
return ESP_ERR_NOT_FOUND; return ESP_ERR_NOT_FOUND;
} }
return ESP_OK; return ESP_OK;
} }
esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value) { esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value)
if (value == NULL) { {
if (value == NULL)
{
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
return bmi160_read_registers(dev, reg, value, 1); return bmi160_read_registers(dev, reg, value, 1);
} }
esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len) { esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len)
{
esp_err_t err = bmi160_check_dev(dev); esp_err_t err = bmi160_check_dev(dev);
if (err != ESP_OK) { if (err != ESP_OK)
{
return err; return err;
} }
if (data == NULL || len == 0) { if (data == NULL || len == 0)
{
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
@@ -156,19 +310,23 @@ esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t
dev->timeout_ticks); dev->timeout_ticks);
} }
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value) { esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value)
{
return bmi160_write_registers(dev, reg, &value, 1); return bmi160_write_registers(dev, reg, &value, 1);
} }
esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len) { esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len)
{
uint8_t buffer[1 + BMI160_MAX_WRITE_LEN]; uint8_t buffer[1 + BMI160_MAX_WRITE_LEN];
esp_err_t err = bmi160_check_dev(dev); esp_err_t err = bmi160_check_dev(dev);
if (err != ESP_OK) { if (err != ESP_OK)
{
return err; return err;
} }
if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN) { if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN)
{
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }

62
main/bmi160.h
View File

@@ -4,43 +4,59 @@
#include <stdint.h> #include <stdint.h>
#include "driver/i2c.h" #include "driver/i2c.h"
#include "driver/gpio.h"
#include "esp_err.h" #include "esp_err.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#ifdef __cplusplus #define BMI160_I2C_ADDRESS_LOW 0x68
extern "C" {
#endif
#define BMI160_I2C_ADDRESS_LOW 0x68
#define BMI160_I2C_ADDRESS_HIGH 0x69 #define BMI160_I2C_ADDRESS_HIGH 0x69
#define BMI160_REG_CHIP_ID 0x00 #define BMI160_REG_CHIP_ID 0x00
#define BMI160_REG_PMU_STATUS 0x03 #define BMI160_REG_PMU_STATUS 0x03
#define BMI160_REG_DATA 0x04 #define BMI160_REG_DATA 0x04
#define BMI160_SIZE_REG_DATA 20 #define BMI160_SIZE_REG_DATA 20
#define BMI160_REG_STATUS 0x1B #define BMI160_REG_SENSORTIME 0x18
#define BMI160_REG_ACC_CONF 0x40 #define BMI160_SIZE_SENSORTIME 3
#define BMI160_REG_ACC_RANGE 0x41 #define BMI160_REG_STATUS 0x1B
#define BMI160_REG_GYR_CONF 0x42 #define BMI160_REG_ACC_CONF 0x40
#define BMI160_REG_GYR_RANGE 0x43 #define BMI160_REG_ACC_RANGE 0x41
#define BMI160_REG_CMD 0x7e #define BMI160_REG_GYR_CONF 0x42
#define BMI160_REG_GYR_RANGE 0x43
#define BMI160_REG_INT_EN0 0x50
#define BMI160_REG_INT_EN1 0x51
#define BMI160_REG_INT_EN2 0x52
#define BMI160_REG_INT_OUT_CTRL 0x53
#define BMI160_REG_INT_LATCH 0x54
#define BMI160_REG_INT_MAP0 0x55
#define BMI160_REG_INT_MAP1 0x56
#define BMI160_REG_INT_MAP2 0x57
#define BMI160_REG_CMD 0x7e
#define BMI160_CHIP_ID 0xD1 #define BMI160_CHIP_ID 0xD1
#define BMI160_DEFAULT_TIMEOUT_MS 100 #define BMI160_DEFAULT_TIMEOUT_MS 100
typedef struct { typedef struct
{
i2c_port_t i2c_port; i2c_port_t i2c_port;
uint8_t i2c_address; uint8_t i2c_address;
TickType_t timeout_ticks; TickType_t timeout_ticks;
gpio_num_t drdy_io;
QueueHandle_t sample_queue;
TaskHandle_t read_task;
} bmi160_t; } bmi160_t;
typedef struct { typedef struct
struct { {
struct
{
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
} acc, gyr; } acc, gyr;
uint32_t time;
} bmi160_value_t; } bmi160_value_t;
esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address); esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address);
@@ -49,9 +65,5 @@ esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value); esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value);
esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len); esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len);
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port); esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle);
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value); esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value);
#ifdef __cplusplus
}
#endif

33
main/motors.c
View File

@@ -13,7 +13,8 @@
#define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1) #define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1)
#define MOTOR_MIN_VALUE 60 #define MOTOR_MIN_VALUE 60
typedef struct { typedef struct
{
int fwd_pin; int fwd_pin;
int bak_pin; int bak_pin;
ledc_channel_t fwd_channel; ledc_channel_t fwd_channel;
@@ -39,13 +40,17 @@ static uint32_t percentile_to_duty(percentile_t percentile)
{ {
int clamped = percentile; int clamped = percentile;
const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100; const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100;
if (clamped > 100) { if (clamped > 100)
{
clamped = 100; clamped = 100;
} else if (clamped < -100) { }
else if (clamped < -100)
{
clamped = -100; clamped = -100;
} }
if (clamped == 0) { if (clamped == 0)
{
return 0; return 0;
} }
@@ -58,7 +63,7 @@ static void set_channel_duty(ledc_channel_t channel, uint32_t duty)
ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel)); ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel));
} }
void init_motors(void) esp_err_t init_motors(void)
{ {
const ledc_timer_config_t timer_config = { const ledc_timer_config_t timer_config = {
.speed_mode = MOTOR_PWM_MODE, .speed_mode = MOTOR_PWM_MODE,
@@ -70,7 +75,8 @@ void init_motors(void)
ESP_ERROR_CHECK(ledc_timer_config(&timer_config)); ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i) { for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i)
{
const motor_config_t *motor = &s_motor_configs[i]; const motor_config_t *motor = &s_motor_configs[i];
const ledc_channel_config_t fwd_channel_config = { const ledc_channel_config_t fwd_channel_config = {
.gpio_num = motor->fwd_pin, .gpio_num = motor->fwd_pin,
@@ -96,24 +102,31 @@ void init_motors(void)
} }
set_motors(0, 0); set_motors(0, 0);
return ESP_OK;
} }
void set_motor(motor_t motor, percentile_t percentile) void set_motor(motor_t motor, percentile_t percentile)
{ {
if (motor < MOTOR1 || motor > MOTOR2) { if (motor < MOTOR1 || motor > MOTOR2)
{
return; return;
} }
const motor_config_t *config = &s_motor_configs[motor]; const motor_config_t *config = &s_motor_configs[motor];
const uint32_t duty = percentile_to_duty(percentile); const uint32_t duty = percentile_to_duty(percentile);
if (percentile > 0) { if (percentile > 0)
{
set_channel_duty(config->bak_channel, 0); set_channel_duty(config->bak_channel, 0);
set_channel_duty(config->fwd_channel, duty); set_channel_duty(config->fwd_channel, duty);
} else if (percentile < 0) { }
else if (percentile < 0)
{
set_channel_duty(config->fwd_channel, 0); set_channel_duty(config->fwd_channel, 0);
set_channel_duty(config->bak_channel, duty); set_channel_duty(config->bak_channel, duty);
} else { }
else
{
set_channel_duty(config->fwd_channel, 0); set_channel_duty(config->fwd_channel, 0);
set_channel_duty(config->bak_channel, 0); set_channel_duty(config->bak_channel, 0);
} }

6
main/motors.h
View File

@@ -1,19 +1,21 @@
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>
#include "esp_err.h"
#define MOTOR1_FWD_PIN 4 #define MOTOR1_FWD_PIN 4
#define MOTOR1_BAK_PIN 5 #define MOTOR1_BAK_PIN 5
#define MOTOR2_FWD_PIN 6 #define MOTOR2_FWD_PIN 6
#define MOTOR2_BAK_PIN 7 #define MOTOR2_BAK_PIN 7
typedef enum { typedef enum
{
MOTOR1, MOTOR1,
MOTOR2, MOTOR2,
} motor_t; } motor_t;
typedef int8_t percentile_t; typedef int8_t percentile_t;
void init_motors(); esp_err_t init_motors();
void set_motor(motor_t motor, percentile_t percentile); void set_motor(motor_t motor, percentile_t percentile);
void set_motors(percentile_t left, percentile_t right); void set_motors(percentile_t left, percentile_t right);

37
plot.py
View File

@@ -30,36 +30,6 @@ TEXT = (230, 235, 240)
ERROR = (255, 110, 110) ERROR = (255, 110, 110)
LINE_RE = re.compile(r"\[<([^>]+)>\]:\s*(.*)") LINE_RE = re.compile(r"\[<([^>]+)>\]:\s*(.*)")
DEFAULT_CONFIG = {
"title": "IMU stream",
"history_seconds": 10.0,
"graphs": [
{
"title": "Accelerometer",
"streams": [
{"key": "acc_x", "label": "ACC X", "color": [90, 170, 255]},
{"key": "acc_y", "label": "ACC Y", "color": [80, 220, 140]},
{"key": "acc_z", "label": "ACC Z", "color": [255, 200, 60]},
],
},
{
"title": "Gyroscope",
"streams": [
{"key": "gyr_x", "label": "GYR X", "color": [240, 80, 80]},
{"key": "gyr_y", "label": "GYR Y", "color": [220, 80, 220]},
{"key": "gyr_z", "label": "GYR Z", "color": [240, 240, 240]},
],
},
],
"sources": [
{
"tag": "IMU",
"fields": ["acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"],
}
],
}
def parse_args() -> argparse.Namespace: def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Read tagged serial values and draw them live from a JSON config." description="Read tagged serial values and draw them live from a JSON config."
@@ -77,7 +47,7 @@ def parse_args() -> argparse.Namespace:
"--config", "--config",
type=Path, type=Path,
default=None, default=None,
help="Path to JSON plot config (default: built-in IMU config)", help="Path to JSON plot config (default: ./plot_config.json)",
) )
parser.add_argument( parser.add_argument(
"--history-seconds", "--history-seconds",
@@ -108,7 +78,10 @@ def parse_args() -> argparse.Namespace:
def load_config(config_path: Path | None) -> dict: def load_config(config_path: Path | None) -> dict:
if config_path is None: if config_path is None:
return DEFAULT_CONFIG config_path = Path("plot_config.json")
if not config_path.exists():
raise SystemExit(f"Config file not found: {config_path}")
with config_path.open("r", encoding="utf-8") as handle: with config_path.open("r", encoding="utf-8") as handle:
return json.load(handle) return json.load(handle)

8
plot_config.json
View File

@@ -4,7 +4,7 @@
"sources": [ "sources": [
{ {
"tag": "IMU", "tag": "IMU",
"fields": ["acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"] "fields": ["time", "acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"]
} }
], ],
"graphs": [ "graphs": [
@@ -19,9 +19,9 @@
{ {
"title": "Gyroscope", "title": "Gyroscope",
"streams": [ "streams": [
{ "key": "gyr_x", "label": "GYR X", "color": [240, 80, 80] }, { "key": "gyr_x", "label": "GYR X", "color": [90, 170, 255] },
{ "key": "gyr_y", "label": "GYR Y", "color": [220, 80, 220] }, { "key": "gyr_y", "label": "GYR Y", "color": [80, 220, 140] },
{ "key": "gyr_z", "label": "GYR Z", "color": [240, 240, 240] } { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60] }
] ]
} }
] ]