#include <Arduino.h>
#include <Wire.h>
#include <OBD.h>
#include "MPU6050.h"
#include <SoftwareSerial.h>
#include <Servo.h>

int servoPin1=13;
int servoPin2=12;

Servo servo1;
Servo servo2;

COBDI2C obd;
SoftwareSerial lcd(2,3);

void clearDisplay() {
  lcd.write(0xFE);
  lcd.write(0x01);
}

void setLCDCursor(byte cursor_position) {
  lcd.write(0xFE);
  lcd.write(0x80);
  lcd.write(cursor_position);
}

void Show() {
  setLCDCursor(0);
  lcd.print("Test");
}

void setup() {
  lcd.begin(9600);
  clearDisplay();
  setLCDCursor(0);
  lcd.print("OBD");

  pinMode(servoPin1,OUTPUT);
  pinMode(servoPin2,OUTPUT);
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo1.write(0);
  delay(200);
  servo2.write(100);
  delay(200);

  obd.begin();
  conn=obd.init(PROTO_AUTO);
  lcd.print("Connected");

  Wire.begin();
}

void loop() {
  conn=obd.readPID(PID_RPM,rpm);
  if (condition) {
     servo2.write(50);
    delay(500);
    servo1.write(100);
    delay(500);
  }
  Show();
  delay(1000);
}