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

3
.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
*.old
__pycache__*
build/

6
CMakeLists.txt Normal file
View File

@@ -0,0 +1,6 @@
# The following five lines of boilerplate have to be in your project's
# CMakeLists in this exact order for cmake to work correctly
cmake_minimum_required(VERSION 3.16)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(bbot)

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);

327
plot.py Normal file
View File

@@ -0,0 +1,327 @@
#!/usr/bin/env python3
import argparse
import re
import sys
import threading
import time
from collections import deque
from queue import Empty, Queue
try:
import pygame
except ImportError as exc:
raise SystemExit(
"Missing dependency: pygame. Install with `pip install pygame pyserial`."
) from exc
try:
import serial
except ImportError as exc:
raise SystemExit(
"Missing dependency: pyserial. Install with `pip install pygame pyserial`."
) from exc
IMU_RE = re.compile(
r"\[<IMU>\]:\s*(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)\s+(-?\d+)"
)
BACKGROUND = (18, 20, 24)
GRID = (48, 54, 64)
AXIS = (140, 148, 160)
ACC_X_TRACE = (90, 170, 255)
ACC_Y_TRACE = (80, 220, 140)
ACC_Z_TRACE = (255, 200, 60)
GYR_X_TRACE = (240, 80, 80)
GYR_Y_TRACE = (220, 80, 220)
GYR_Z_TRACE = (240, 240, 240)
TEXT = (230, 235, 240)
ERROR = (255, 110, 110)
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="Read BMI160 IMU values from a serial port and draw them live."
)
parser.add_argument("port", help="Serial port, for example /dev/ttyUSB0 or COM3")
parser.add_argument(
"-b",
"--baudrate",
type=int,
default=115200,
help="Serial baudrate (default: 115200)",
)
parser.add_argument(
"--history-seconds",
type=float,
default=10.0,
help="Initial visible history window in seconds (default: 10)",
)
parser.add_argument(
"--max-samples",
type=int,
default=30000,
help="Maximum number of samples to keep in memory (default: 30000)",
)
parser.add_argument(
"--width",
type=int,
default=1200,
help="Window width in pixels (default: 1200)",
)
parser.add_argument(
"--height",
type=int,
default=700,
help="Window height in pixels (default: 700)",
)
return parser.parse_args()
def serial_reader(port: str, baudrate: int, output: Queue) -> None:
try:
with serial.Serial(port, baudrate=baudrate, timeout=1) as ser:
while True:
raw_line = ser.readline()
if not raw_line:
continue
line = raw_line.decode("utf-8", errors="replace").strip()
timestamp = time.monotonic()
match = IMU_RE.search(line)
if not match:
continue
values = [int(group) for group in match.groups()]
for stream_name, value in zip(
("acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"),
values,
):
output.put((stream_name, timestamp, value))
except serial.SerialException as exc:
output.put(("error", f"Serial error: {exc}"))
def drain_queue(
queue: Queue,
sample_sets: dict[str, deque[tuple[float, int]]],
) -> str | None:
error_message = None
while True:
try:
item = queue.get_nowait()
except Empty:
return error_message
if item[0] == "error":
error_message = item[1]
continue
stream_name, timestamp, value = item
sample_sets[stream_name].append((timestamp, value))
def draw_grid(
surface: pygame.Surface,
rect: pygame.Rect,
font: pygame.font.Font,
view_span: float,
y_min: float,
y_max: float,
title: str,
) -> None:
pygame.draw.rect(surface, GRID, rect, width=1)
for fraction in (0.0, 0.25, 0.5, 0.75, 1.0):
y = rect.top + round(fraction * rect.height)
pygame.draw.line(surface, GRID, (rect.left, y), (rect.right, y), 1)
value = y_max - fraction * (y_max - y_min)
label = f"{value:.0f}"
text = font.render(label, True, AXIS)
surface.blit(text, (10, y - text.get_height() // 2))
for fraction in (0.0, 0.25, 0.5, 0.75, 1.0):
x = rect.left + round(fraction * rect.width)
pygame.draw.line(surface, GRID, (x, rect.top), (x, rect.bottom), 1)
seconds_ago = view_span * (1.0 - fraction)
label = f"-{seconds_ago:.1f}s" if seconds_ago > 0.05 else "now"
text = font.render(label, True, AXIS)
surface.blit(text, (x - text.get_width() // 2, rect.bottom + 8))
surface.blit(font.render(title, True, TEXT), (rect.left + 8, rect.top + 8))
def draw_trace(
surface: pygame.Surface,
rect: pygame.Rect,
samples: deque[tuple[float, int]],
view_span: float,
now: float,
y_min: float,
y_max: float,
color: tuple[int, int, int],
) -> int | None:
visible_points = []
latest_value = None
y_span = max(y_max - y_min, 1e-6)
for timestamp, value in samples:
age = now - timestamp
if age < 0 or age > view_span:
continue
x = rect.right - (age / view_span) * rect.width
y = rect.bottom - ((value - y_min) / y_span) * rect.height
visible_points.append((round(x), round(y)))
latest_value = value
if len(visible_points) >= 2:
pygame.draw.lines(surface, color, False, visible_points, 2)
elif len(visible_points) == 1:
pygame.draw.circle(surface, color, visible_points[0], 2)
return latest_value
def visible_range(
sample_sets: dict[str, deque[tuple[float, int]]], stream_names: tuple[str, ...], now: float, view_span: float
) -> tuple[float, float]:
max_abs = 1.0
for stream_name in stream_names:
for timestamp, value in sample_sets[stream_name]:
age = now - timestamp
if 0 <= age <= view_span:
max_abs = max(max_abs, abs(value))
return -max_abs, max_abs
def zoom(view_span: float, direction: int, initial_span: float) -> float:
if direction > 0:
return max(1.0, view_span / 1.2)
return min(max(initial_span * 20.0, 60.0), view_span * 1.2)
def main() -> int:
args = parse_args()
sample_sets = {
"acc_x": deque(maxlen=args.max_samples),
"acc_y": deque(maxlen=args.max_samples),
"acc_z": deque(maxlen=args.max_samples),
"gyr_x": deque(maxlen=args.max_samples),
"gyr_y": deque(maxlen=args.max_samples),
"gyr_z": deque(maxlen=args.max_samples),
}
queue: Queue = Queue()
thread = threading.Thread(
target=serial_reader, args=(args.port, args.baudrate, queue), daemon=True
)
thread.start()
pygame.init()
pygame.display.set_caption(f"IMU stream: {args.port} @ {args.baudrate}")
screen = pygame.display.set_mode((args.width, args.height), pygame.RESIZABLE)
font = pygame.font.SysFont("monospace", 18)
small_font = pygame.font.SysFont("monospace", 14)
clock = pygame.time.Clock()
view_span = max(args.history_seconds, 1.0)
error_message = None
running = True
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.MOUSEWHEEL:
view_span = zoom(view_span, event.y, args.history_seconds)
elif event.type == pygame.MOUSEBUTTONDOWN:
if event.button == 4:
view_span = zoom(view_span, 1, args.history_seconds)
elif event.button == 5:
view_span = zoom(view_span, -1, args.history_seconds)
queued_error = drain_queue(queue, sample_sets)
if queued_error is not None:
error_message = queued_error
width, height = screen.get_size()
now = time.monotonic()
plot_width = max(100, width - 100)
panel_height = max(100, (height - 140) // 2)
acc_rect = pygame.Rect(70, 40, plot_width, panel_height)
gyr_rect = pygame.Rect(70, acc_rect.bottom + 40, plot_width, panel_height)
acc_y_min, acc_y_max = visible_range(
sample_sets, ("acc_x", "acc_y", "acc_z"), now, view_span
)
gyr_y_min, gyr_y_max = visible_range(
sample_sets, ("gyr_x", "gyr_y", "gyr_z"), now, view_span
)
screen.fill(BACKGROUND)
draw_grid(screen, acc_rect, small_font, view_span, acc_y_min, acc_y_max, "Accelerometer")
draw_grid(screen, gyr_rect, small_font, view_span, gyr_y_min, gyr_y_max, "Gyroscope")
latest_acc_x = draw_trace(
screen, acc_rect, sample_sets["acc_x"], view_span, now, acc_y_min, acc_y_max, ACC_X_TRACE
)
latest_acc_y = draw_trace(
screen, acc_rect, sample_sets["acc_y"], view_span, now, acc_y_min, acc_y_max, ACC_Y_TRACE
)
latest_acc_z = draw_trace(
screen, acc_rect, sample_sets["acc_z"], view_span, now, acc_y_min, acc_y_max, ACC_Z_TRACE
)
latest_gyr_x = draw_trace(
screen, gyr_rect, sample_sets["gyr_x"], view_span, now, gyr_y_min, gyr_y_max, GYR_X_TRACE
)
latest_gyr_y = draw_trace(
screen, gyr_rect, sample_sets["gyr_y"], view_span, now, gyr_y_min, gyr_y_max, GYR_Y_TRACE
)
latest_gyr_z = draw_trace(
screen, gyr_rect, sample_sets["gyr_z"], view_span, now, gyr_y_min, gyr_y_max, GYR_Z_TRACE
)
header = f"port={args.port} baud={args.baudrate} zoom={view_span:.1f}s"
if latest_acc_x is not None:
header += f" acc=({latest_acc_x}, {latest_acc_y}, {latest_acc_z})"
if latest_gyr_x is not None:
header += f" gyr=({latest_gyr_x}, {latest_gyr_y}, {latest_gyr_z})"
screen.blit(font.render(header, True, TEXT), (10, 8))
screen.blit(
small_font.render("mouse wheel: zoom horizontal axis", True, AXIS),
(10, height - 24),
)
legend_items = (
("ACC X", ACC_X_TRACE),
("ACC Y", ACC_Y_TRACE),
("ACC Z", ACC_Z_TRACE),
("GYR X", GYR_X_TRACE),
("GYR Y", GYR_Y_TRACE),
("GYR Z", GYR_Z_TRACE),
)
legend_x = 10
legend_y = 34
for label, color in legend_items:
pygame.draw.line(
screen,
color,
(legend_x, legend_y + 9),
(legend_x + 24, legend_y + 9),
3,
)
screen.blit(small_font.render(label, True, TEXT), (legend_x + 30, legend_y))
legend_x += 105
if error_message:
error_surface = font.render(error_message, True, ERROR)
screen.blit(error_surface, (10, 56))
pygame.display.flip()
clock.tick(60)
pygame.quit()
return 0
if __name__ == "__main__":
raise SystemExit(main())

2190
sdkconfig Normal file
View File

File diff suppressed because it is too large Load Diff