#include <SPI.h>
#include <mcp_can.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define CAN_CS 10
MCP_CAN CAN(CAN_CS);

const byte NODE_ID = 0x01;   // Change to your Sevcon node ID

void setup() {
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("vitesse = ");
  lcd.print(" ");
  lcd.print(" km/h  ");

  lcd.setCursor(0, 1);
  lcd.print("ubat = ");
  lcd.print("  ");
  lcd.print(" V  ");

  Serial.begin(115200);

  while (CAN_OK != CAN.begin(MCP_ANY, CAN_500KBPS, MCP_8MHZ)) {
    Serial.println("CAN init failed, retrying...");
    delay(100);
  }

  CAN.setMode(MCP_NORMAL);
  Serial.println("CAN init OK");
  
}

void loop() {

  if (CAN.checkReceive() == CAN_MSGAVAIL) {

    unsigned long rxId;
    byte len = 0;
    byte buf[8];

    CAN.readMsgBuf(&rxId, &len, buf);

    switch (rxId) {
      case 0x382 : {
        int16_t rpm = buf[0] | (buf[1] << 8);
        int v = rpm * 0.017;
        Serial.print(v);
        Serial.println();
        lcd.setCursor(0, 0);
        lcd.print("vitesse = ");
        lcd.print(v);
        lcd.print(" km/h  ");
        delay(400);
      }
      case 0x209 : {
        int16_t ubat = buf[0] | (buf[1] << 8);
        int ubatr = ubat / 16;
        Serial.print(ubat);
        Serial.println();
        lcd.setCursor(0, 1);
        lcd.print("ubat = ");
        lcd.print(ubatr);
        lcd.print(" V  ");
        delay(400);
      }
    }
  }
}