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

Working IMU with polling

Browse Source
This commit is contained in:
Joppe Blondel
2026-03-19 18:41:32 +01:00
commit 182cc429bb
10 changed files with 2999 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
main/CMakeLists.txt Normal file
View File

@@ -0,0 +1,8 @@
idf_component_register(
SRCS
"bbot.c"
"bmi160.c"
"motors.c"
INCLUDE_DIRS
"."
)

79
main/bbot.c Normal file
View File

@@ -0,0 +1,79 @@
#include <stdio.h>
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "motors.h"
#include "bmi160.h"
#define BBOT_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_SCL_IO GPIO_NUM_3
#define BBOT_I2C_SDA_IO GPIO_NUM_2
#define BBOT_I2C_FREQ_HZ 100000
static bmi160_t imu;
static void init_blink_gpio(){
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 = {
.mode = I2C_MODE_MASTER,
.sda_io_num = BBOT_I2C_SDA_IO,
.scl_io_num = BBOT_I2C_SCL_IO,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
.master.clk_speed = BBOT_I2C_FREQ_HZ,
};
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));
}
void app_main(void){
init_input_gpio();
init_blink_gpio();
init_motors();
init_i2c();
imu_init(&imu, BBOT_I2C_PORT);
while (1) {
vTaskDelay(pdMS_TO_TICKS(10));
bmi160_value_t v;
imu_read(&imu, &v);
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);
}
}

184
main/bmi160.c Normal file
View File

@@ -0,0 +1,184 @@
#include "bmi160.h"
#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
esp_err_t err;
err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_LOW);
if (err != ESP_OK) {
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);
}
if (err != ESP_OK) {
ESP_LOGE("bmi160", "BMI160 probe failed at 0x%02X and 0x%02X: %s",
BMI160_I2C_ADDRESS_LOW,
BMI160_I2C_ADDRESS_HIGH,
esp_err_to_name(err));
}
// Soft reset
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0xB6));
vTaskDelay(pdMS_TO_TICKS(50));
// Check chip ID
uint8_t 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);
if(chip_id != BMI160_CHIP_ID){
ESP_LOGE("bmi160", "BMI160 chip id not right");
return ESP_FAIL;
}
// Configure ACC: 100Hz, normal mode filter, no undersampling
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_CONF, 0x28));
// Configure ACC range: +-16g
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_RANGE, 0x0C));
// Configure GYR: 100Hz, normal mode filter, no undersampling
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_CONF, 0x28));
// Configure GYR range: +-2000dps
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_RANGE, 0x00));
// Both sensors in normal mode
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x11));
vTaskDelay(pdMS_TO_TICKS(100));
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x15));
vTaskDelay(pdMS_TO_TICKS(100));
// Poll until active
bool startup_complete = false;
for(int timeout=0; timeout<1000; timeout+=10){
uint8_t status;
ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_PMU_STATUS, &status));
int acc_pmu_status = (status & 0b00110000)>>4;
int gyr_pmu_status = (status & 0b00001100)>>2;
if(acc_pmu_status==0b01 && gyr_pmu_status==0b01){
startup_complete = true;
break;
}
vTaskDelay(pdMS_TO_TICKS(10));
}
if(!startup_complete){
ESP_LOGE("bmi160", "Acc or gyr not set in normal mode");
}
// Clear data registers
uint8_t data[20];
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
return ESP_OK;
}
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value){
if (dev == NULL || value == NULL) {
return ESP_ERR_INVALID_ARG;
}
uint8_t data[20];
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.y = data[16] | (data[17]<<8);
value->acc.z = data[18] | (data[19]<<8);
value->gyr.x = data[8] | (data[9]<<8);
value->gyr.y = data[10] | (data[11]<<8);
value->gyr.z = data[12] | (data[13]<<8);
return ESP_OK;
}
// ----------------------------------------------------
// Low level driver
#define BMI160_MAX_WRITE_LEN 32
static esp_err_t bmi160_check_dev(const bmi160_t *dev) {
if (dev == NULL) {
return ESP_ERR_INVALID_ARG;
}
return ESP_OK;
}
esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address) {
uint8_t chip_id = 0;
esp_err_t err;
if (dev == NULL) {
return ESP_ERR_INVALID_ARG;
}
dev->i2c_port = i2c_port;
dev->i2c_address = i2c_address;
dev->timeout_ticks = pdMS_TO_TICKS(BMI160_DEFAULT_TIMEOUT_MS);
err = bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id);
if (err != ESP_OK) {
return err;
}
if (chip_id != BMI160_CHIP_ID) {
return ESP_ERR_NOT_FOUND;
}
return ESP_OK;
}
esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value) {
if (value == NULL) {
return ESP_ERR_INVALID_ARG;
}
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 err = bmi160_check_dev(dev);
if (err != ESP_OK) {
return err;
}
if (data == NULL || len == 0) {
return ESP_ERR_INVALID_ARG;
}
return i2c_master_write_read_device(
dev->i2c_port,
dev->i2c_address,
&start_reg,
1,
data,
len,
dev->timeout_ticks);
}
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value) {
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) {
uint8_t buffer[1 + BMI160_MAX_WRITE_LEN];
esp_err_t err = bmi160_check_dev(dev);
if (err != ESP_OK) {
return err;
}
if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN) {
return ESP_ERR_INVALID_ARG;
}
buffer[0] = start_reg;
memcpy(&buffer[1], data, len);
return i2c_master_write_to_device(
dev->i2c_port,
dev->i2c_address,
buffer,
len + 1,
dev->timeout_ticks);
}

57
main/bmi160.h Normal file
View File

@@ -0,0 +1,57 @@
#pragma once
#include <stddef.h>
#include <stdint.h>
#include "driver/i2c.h"
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#ifdef __cplusplus
extern "C" {
#endif
#define BMI160_I2C_ADDRESS_LOW 0x68
#define BMI160_I2C_ADDRESS_HIGH 0x69
#define BMI160_REG_CHIP_ID 0x00
#define BMI160_REG_PMU_STATUS 0x03
#define BMI160_REG_DATA 0x04
#define BMI160_SIZE_REG_DATA 20
#define BMI160_REG_STATUS 0x1B
#define BMI160_REG_ACC_CONF 0x40
#define BMI160_REG_ACC_RANGE 0x41
#define BMI160_REG_GYR_CONF 0x42
#define BMI160_REG_GYR_RANGE 0x43
#define BMI160_REG_CMD 0x7e
#define BMI160_CHIP_ID 0xD1
#define BMI160_DEFAULT_TIMEOUT_MS 100
typedef struct {
i2c_port_t i2c_port;
uint8_t i2c_address;
TickType_t timeout_ticks;
} bmi160_t;
typedef struct {
struct {
int16_t x;
int16_t y;
int16_t z;
} acc, gyr;
} bmi160_value_t;
esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address);
esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value);
esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len);
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 imu_init(bmi160_t *dev, i2c_port_t port);
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value);
#ifdef __cplusplus
}
#endif

126
main/motors.c Normal file
View File

@@ -0,0 +1,126 @@
#include "motors.h"
#include <stdlib.h>
#include "driver/ledc.h"
#include "esp_err.h"
#include "esp_log.h"
#define MOTOR_PWM_MODE LEDC_LOW_SPEED_MODE
#define MOTOR_PWM_TIMER LEDC_TIMER_0
#define MOTOR_PWM_FREQ_HZ 20000
#define MOTOR_PWM_RESOLUTION LEDC_TIMER_10_BIT
#define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1)
#define MOTOR_MIN_VALUE 60
typedef struct {
int fwd_pin;
int bak_pin;
ledc_channel_t fwd_channel;
ledc_channel_t bak_channel;
} motor_config_t;
static const motor_config_t s_motor_configs[] = {
[MOTOR1] = {
.fwd_pin = MOTOR1_FWD_PIN,
.bak_pin = MOTOR1_BAK_PIN,
.fwd_channel = LEDC_CHANNEL_0,
.bak_channel = LEDC_CHANNEL_1,
},
[MOTOR2] = {
.fwd_pin = MOTOR2_FWD_PIN,
.bak_pin = MOTOR2_BAK_PIN,
.fwd_channel = LEDC_CHANNEL_2,
.bak_channel = LEDC_CHANNEL_3,
},
};
static uint32_t percentile_to_duty(percentile_t percentile)
{
int clamped = percentile;
const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100;
if (clamped > 100) {
clamped = 100;
} else if (clamped < -100) {
clamped = -100;
}
if (clamped == 0) {
return 0;
}
return min_duty + (((uint32_t)abs(clamped) * (MOTOR_PWM_MAX_DUTY - min_duty)) / 100);
}
static void set_channel_duty(ledc_channel_t channel, uint32_t duty)
{
ESP_ERROR_CHECK(ledc_set_duty(MOTOR_PWM_MODE, channel, duty));
ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel));
}
void init_motors(void)
{
const ledc_timer_config_t timer_config = {
.speed_mode = MOTOR_PWM_MODE,
.timer_num = MOTOR_PWM_TIMER,
.duty_resolution = MOTOR_PWM_RESOLUTION,
.freq_hz = MOTOR_PWM_FREQ_HZ,
.clk_cfg = LEDC_AUTO_CLK,
};
ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
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 ledc_channel_config_t fwd_channel_config = {
.gpio_num = motor->fwd_pin,
.speed_mode = MOTOR_PWM_MODE,
.channel = motor->fwd_channel,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = MOTOR_PWM_TIMER,
.duty = 0,
.hpoint = 0,
};
const ledc_channel_config_t bak_channel_config = {
.gpio_num = motor->bak_pin,
.speed_mode = MOTOR_PWM_MODE,
.channel = motor->bak_channel,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = MOTOR_PWM_TIMER,
.duty = 0,
.hpoint = 0,
};
ESP_ERROR_CHECK(ledc_channel_config(&fwd_channel_config));
ESP_ERROR_CHECK(ledc_channel_config(&bak_channel_config));
}
set_motors(0, 0);
}
void set_motor(motor_t motor, percentile_t percentile)
{
if (motor < MOTOR1 || motor > MOTOR2) {
return;
}
const motor_config_t *config = &s_motor_configs[motor];
const uint32_t duty = percentile_to_duty(percentile);
if (percentile > 0) {
set_channel_duty(config->bak_channel, 0);
set_channel_duty(config->fwd_channel, duty);
} else if (percentile < 0) {
set_channel_duty(config->fwd_channel, 0);
set_channel_duty(config->bak_channel, duty);
} else {
set_channel_duty(config->fwd_channel, 0);
set_channel_duty(config->bak_channel, 0);
}
}
void set_motors(percentile_t left, percentile_t right)
{
set_motor(MOTOR1, left);
set_motor(MOTOR2, right);
}

19
main/motors.h Normal file
View File

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