Чтение CAN шины через модуль MCP2515 и вывод информации на OLED i2c дисплей

Arduino Nano (Arduino IDE), который читает данные с CAN-шины через модуль MCP2515 и выводит информацию на OLED I2C дисплей 128x64:

#include <SPI.h>
#include <mcp2515.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Настройки OLED дисплея
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Настройки MCP2515
#define CAN_CS 10
MCP2515 mcp2515(CAN_CS);

// Структура для хранения CAN-сообщения
struct can_frame canMsg;

void setup() {
  Serial.begin(9600);
  
  // Инициализация OLED дисплея
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED не найден!"));
    for(;;); // Бесконечный цикл при ошибке
  }
  
  // Очистка дисплея и отображение приветствия
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println(F("CAN Monitor"));
  display.println(F("Initializing..."));
  display.display();
  delay(2000);

  // Инициализация SPI и MCP2515
  SPI.begin();
  
  mcp2515.reset();
  mcp2515.setBitrate(CAN_500KBPS, MCP_8MHZ); // Скорость CAN 500 kbps, кварц 8MHz
  mcp2515.setNormalMode(); // Режим нормальной работы
  
  display.clearDisplay();
  display.setCursor(0,0);
  display.println(F("CAN Ready"));
  display.println(F("Waiting for data..."));
  display.display();
}

void loop() {
  if (mcp2515.readMessage(&canMsg) == MCP2515::ERROR_OK) {
    // Если получено CAN-сообщение
    displayCANMessage(canMsg);
    printToSerial(canMsg); // Также выводим в Serial для отладки
  }
  
  delay(100); // Небольшая задержка для стабильности
}

// Функция вывода CAN-сообщения на OLED дисплей
void displayCANMessage(struct can_frame frame) {
  display.clearDisplay();
  display.setCursor(0,0);
  
  // Вывод ID сообщения
  display.print(F("ID: 0x"));
  display.println(frame.can_id, HEX);
  
  // Вывод длины данных
  display.print(F("DLC: "));
  display.println(frame.can_dlc);
  
  // Вывод данных
  display.print(F("Data: "));
  for (int i = 0; i < frame.can_dlc; i++) {
    if (frame.data[i] < 0x10) {
      display.print("0"); // Добавляем ведущий ноль для байтов < 0x10
    }
    display.print(frame.data[i], HEX);
    display.print(" ");
  }
  
  display.display();
}

// Функция вывода в Serial Monitor для отладки
void printToSerial(struct can_frame frame) {
  Serial.print("ID: 0x");
  Serial.print(frame.can_id, HEX);
  Serial.print(" DLC: ");
  Serial.print(frame.can_dlc);
  Serial.print(" Data: ");
  
  for (int i = 0; i < frame.can_dlc; i++) {
    if (frame.data[i] < 0x10) {
      Serial.print("0");
    }
    Serial.print(frame.data[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}

// Функция для отображения ошибок CAN
void displayError() {
  display.clearDisplay();
  display.setCursor(0,0);
  display.println(F("CAN Error!"));
  display.display();
}