Untitled

#include <SoftwareSerial.h>
#include "Serial_CAN_Module.h"

#define CAN_TX_PIN 2
#define CAN_RX_PIN 3

SoftwareSerial canSerial(CAN_RX_PIN, CAN_TX_PIN);
Serial_CAN can;

uint16_t engineRPM = 0;
uint8_t vehicleSpeed = 0;
int8_t coolantTemp = 0;

unsigned long lastRequest = 0;
unsigned long lastPrint = 0;
uint8_t pidStep = 0;

void setup() {
  Serial.begin(115200);
  while (!Serial);

  Serial.println("OBD MODE - RPM / SPEED / COOLANT");

  canSerial.begin(9600);
  can.begin(canSerial, 9600);
}

void loop() {

  if (millis() - lastRequest >= 200) {
    lastRequest = millis();

    switch (pidStep) {
      case 0:
      case 1:
        requestPID(0x0C); // RPM (priority)
        break;

      case 2:
        requestPID(0x0D); // SPEED
        break;

      case 3:
        requestPID(0x05); // COOLANT TEMP
        break;
    }

    pidStep++;
    if (pidStep > 3) pidStep = 0;
  }

  processOBD();

  if (millis() - lastPrint >= 1000) {
    lastPrint = millis();

    Serial.print("RPM: ");
    Serial.print(engineRPM);

    Serial.print(" | SPD: ");
    Serial.print(vehicleSpeed);
    Serial.print(" km/h");

    Serial.print(" | CLT: ");
    Serial.print(coolantTemp);
    Serial.println(" C");
  }
}

void requestPID(uint8_t pid) {
  unsigned char buf[8] = {
    0x02, 0x01, pid,
    0x00, 0x00, 0x00, 0x00, 0x00
  };

  can.send(0x7DF, 0, 0, 8, buf);
}

void processOBD() {
  unsigned long id;
  unsigned char buf[8];

  while (can.recv(&id, buf)) {

    // Only OBD ECU responses
    if (id < 0x7E8 || id > 0x7EF) continue;

    if (buf[1] != 0x41) continue;

    uint8_t pid = buf[2];

    switch (pid) {
      case 0x0C: // RPM
        engineRPM = ((buf[3] << 8) | buf[4]) / 4;
        break;

      case 0x0D: // SPEED
        vehicleSpeed = buf[3];
        break;

      case 0x05: // COOLANT TEMP
        coolantTemp = buf[3] - 40;
        break;
    }
  }
}