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