nthsthing/graphing.py

import threading
import pygame
import sys
import dobluetooth
import math

# Initialize Pygame
pygame.init()
pygame.font.init()

from collections import deque

# Rolling history buffers
graph_length = 200
# Gyroscope history
gyro_mag_history = deque(maxlen=graph_length)
gyro_x_history = deque(maxlen=graph_length)
gyro_y_history = deque(maxlen=graph_length)
gyro_z_history = deque(maxlen=graph_length)

# Accelerometer history
acc_x_history = deque(maxlen=graph_length)
acc_y_history = deque(maxlen=graph_length)
acc_z_history = deque(maxlen=graph_length)

# New values
gyro_mag_history = deque(maxlen=graph_length)
accel_mag_history = deque(maxlen=graph_length)
pitch_history = deque(maxlen=graph_length)
roll_history = deque(maxlen=graph_length)
temp_history = deque(maxlen=graph_length)

# Set up the display
width, height = 1000, 750
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption("MPU BLE Visualizer")

# Fonts
font = pygame.font.SysFont("Arial", 20)
warning_font = pygame.font.SysFont("Arial", 32, bold=True)

# Colors
WHITE = (255, 255, 255)
GRAY = (180, 180, 180)
BLACK = (0, 0, 0)
GYRO_COLORS = [(0, 255, 255), (255, 0, 255), (255, 255, 0)]  # rotX, rotY, rotZ
ACCEL_COLORS = [(255, 0, 0), (0, 255, 0), (0, 0, 255)]        # accX, accY, accZ

# Circle setup
padding = 50
circle_spacing = (width - 2 * padding) // 6
circle_radius = 40
circle_positions = [(padding + i * circle_spacing, 150) for i in range(6)]

# Axis labels
axis_labels = ['rotX', 'rotY', 'rotZ', 'accX', 'accY', 'accZ']

# Force threshold
HIGH_FORCE_THRESHOLD = 3.00

# Start BLE thread
def ble_thread_func():
    import asyncio
    loop = asyncio.new_event_loop()
    asyncio.set_event_loop(loop)
    loop.run_until_complete(dobluetooth.getData())

ble_thread = threading.Thread(target=ble_thread_func, daemon=True)
ble_thread.start()

def draw_graph(surface, history, color, y_base, height, height_scale=1.0):
    if len(history) < 2:
        return
    points = []
    for i, val in enumerate(history):
        x = i + 50
        y = y_base + height // 2 - val * height_scale
        y = max(y_base, min(y_base + height, y))  # Clamp inside graph area
        points.append((x, y))
    pygame.draw.lines(surface, color, False, points, 2)


# Main loop
running = True
clock = pygame.time.Clock()

while running:
    screen.fill(WHITE)

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

    try:
        data = dobluetooth.getDataArr()
        high_force : bool = False

        if len(data) >= 11:
            rotX, rotY, rotZ = data[0:3]
            accX, accY, accZ = data[3:6]
            gyroMag, accelMag = data[6:8]
            pitch, roll, temp = data[8:11]

            # Add to history
            gyro_x_history.append(rotX)
            gyro_y_history.append(rotY)
            gyro_z_history.append(rotZ)
            acc_x_history.append(accX)
            acc_y_history.append(accY)
            acc_z_history.append(accZ)
            gyro_mag_history.append(gyroMag)
            accel_mag_history.append(accelMag)
            pitch_history.append(pitch)
            roll_history.append(roll)
            temp_history.append(temp)

            if accelMag> HIGH_FORCE_THRESHOLD:
                high_force = True

            if gyroMag < 10:
                gyro_condition = "🟢 Stable"
                gyro_color = (0, 200, 0)
            elif gyroMag < 75:
                gyro_condition = "🟡 Moving"
                gyro_color = (255, 200, 0)
            else:
                gyro_condition = "🔴 High Rotation"
                gyro_color = (255, 50, 50)


            # Create the font surface
            gyro_status_surface = warning_font.render(f"Gyro Status: {gyro_condition}", True, gyro_color)

            # Position it — move it higher up on the screen
            gyro_status_rect = gyro_status_surface.get_rect(center=(width // 2, 250))

            # Blit it to the screen
            screen.blit(gyro_status_surface, gyro_status_rect)

            for i in range(6):
                val = abs(data[i])

                if i < 3:  # Gyroscope
                    scale = min(val / 5.0, 1.0)
                    base_color = GYRO_COLORS[i]
                else:  # Accelerometer
                    scale = min(val / 5.0, 1.0)  # ✅ Fixed scale
                    base_color = ACCEL_COLORS[i - 3]

                color = [int(c * scale) for c in base_color]
                pygame.draw.circle(screen, color, circle_positions[i], circle_radius)

                # Axis label
                label_surface = font.render(axis_labels[i], True, BLACK)
                label_rect = label_surface.get_rect(center=(circle_positions[i][0], circle_positions[i][1] + circle_radius + 10))
                screen.blit(label_surface, label_rect)

                # Value label
                val_surface = font.render(f"{val:.2f}", True, BLACK)
                val_rect = val_surface.get_rect(center=(circle_positions[i][0], circle_positions[i][1] - circle_radius - 10))
                screen.blit(val_surface, val_rect)

        else:
            for pos in circle_positions:
                pygame.draw.circle(screen, GRAY, pos, circle_radius)

    except Exception as e:
        print("Error parsing BLE data:", e)

    # Display "High Force" warning
    if high_force:
        warning_text = warning_font.render("⚠ High Force Detected!", True, (255, 0, 0))
        warning_rect = warning_text.get_rect(center=(width // 2, 50))
        screen.blit(warning_text, warning_rect)

    # === GRAPH VISUALIZATION ===
    # Where to place the graphs
    graph_area_top = 300# Leave top area for circles/status
    graph_spacing = 120   # Vertical space per graph

    # Graph heights
    graph_height = 80

    # Top Y positions for gyro and accel graphs
    gyro_top = graph_area_top
    accel_top = gyro_top + graph_spacing

    # Background
    pygame.draw.rect(screen, (245, 245, 245), (0, graph_area_top, width, 220))

    # Labels
    screen.blit(font.render("Gyro X/Y/Z (°/s)", True, (0, 0, 0)), (10, gyro_top - 15))
    screen.blit(font.render("Accel X/Y/Z (g)", True, (0, 0, 0)), (10, accel_top - 15))

    # Pitch and Roll
    pitch_top = accel_top + graph_spacing
    screen.blit(font.render("Pitch / Roll (°)", True, (0, 0, 0)), (10, pitch_top - 15))

    # Temp + Mags
    mags_top = pitch_top + graph_spacing
    screen.blit(font.render("Temp (°C), AccelMag (g), GyroMag (°/s)", True, (0, 0, 0)), (10, mags_top - 15))

    # Gyroscope graph (°/s)
    draw_graph(screen, gyro_x_history, (255, 0, 0), gyro_top, graph_height, height_scale=5)
    draw_graph(screen, gyro_y_history, (0, 255, 0), gyro_top, graph_height, height_scale=5)
    draw_graph(screen, gyro_z_history, (0, 0, 255), gyro_top, graph_height, height_scale=5)

    # Accelerometer graph (g)
    draw_graph(screen, acc_x_history, (255, 128, 0), accel_top, graph_height, height_scale=30)
    draw_graph(screen, acc_y_history, (0, 200, 200), accel_top, graph_height, height_scale=30)
    draw_graph(screen, acc_z_history, (128, 0, 255), accel_top, graph_height, height_scale=30)

    # Pitch and Roll (°)
    draw_graph(screen, pitch_history, (100, 100, 255), pitch_top, graph_height, height_scale=1.5)
    draw_graph(screen, roll_history, (255, 100, 100), pitch_top, graph_height, height_scale=1.5)

    # Temp + Mags
    draw_graph(screen, temp_history, (200, 0, 0), mags_top, graph_height, height_scale=3)
    draw_graph(screen, accel_mag_history, (0, 150, 255), mags_top, graph_height, height_scale=30)
    draw_graph(screen, gyro_mag_history, (100, 255, 100), mags_top, graph_height, height_scale=5)

    pygame.display.flip()
    clock.tick(60)

pygame.quit()
sys.exit()