#include "SPI.h"
#include "Adafruit_GFX.h"
#include <ESP32Servo.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>

Servo Servo1;
Servo Servo2;
Adafruit_MPU6050 Acc1;
int S1value;
int S2value;
int DRS;// no active aero
int AB; // air brake
int LC; // left corner
int RC; // right corner
int i;
int pos;
int SB; // slight braking 
int on = HIGH;
int off = LOW;
unsigned long previousMillis;
const unsigned long interval = 10;
int count700ms = 0;
const unsigned long interval1 = 100;

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define LEDR 18
#define LEDL 19
#define DRSB 33
#define SBB 25
#define ABB 32
int DEMOButton = 26;
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3c ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 OLED(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// draw primitives
  void drawPixel(uint16_t x, uint16_t y, uint16_t color);
  void drawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color);
  void drawFastVLine(uint16_t x0, uint16_t y0, uint16_t length, uint16_t color);
  void drawFastHLine(uint8_t x0, uint8_t y0, uint8_t length, uint16_t color);
  void drawRect(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint16_t color);
  void fillRect(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint16_t color);
  void drawCircle(uint16_t x0, uint16_t y0, uint16_t r, uint16_t color);
  void fillCircle(uint16_t x0, uint16_t y0, uint16_t r, uint16_t color);
  void drawRoundRect(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint16_t radius, uint16_t color);
  void fillRoundRect(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint16_t radius, uint16_t color);
  void drawTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color);
  void fillTriangle(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color);
  void drawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg, uint8_t size);
  void setCursor(int16_t x0, int16_t y0);
  void setTextColor(uint16_t color);
  void setTextColor(uint16_t color, uint16_t backgroundcolor);
  void setTextSize(uint8_t size);
  void setTextWrap(boolean w);

// 'title', 120x15px
const unsigned char bitmap_title [] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x83, 0xcf, 0xe1, 
	0xff, 0xe3, 0x0f, 0x8f, 0xe3, 0xf3, 0xff, 0x01, 0xfe, 0xfe, 0x1e, 0x1d, 0xc7, 0xe3, 0x81, 0xc0, 
	0xe3, 0x1d, 0xce, 0x76, 0x38, 0xe3, 0x01, 0xc0, 0xe7, 0x1e, 0x18, 0xec, 0x73, 0x81, 0xc0, 0xe3, 
	0x1c, 0x0c, 0x76, 0x38, 0xe3, 0x01, 0xc0, 0xc7, 0x3c, 0x18, 0x6c, 0x73, 0x81, 0xf8, 0xf7, 0x1f, 
	0xcc, 0x76, 0x38, 0xe3, 0x01, 0xf8, 0xc7, 0x3c, 0x18, 0x6f, 0xf3, 0x81, 0xf8, 0xff, 0x0f, 0xef, 
	0xf6, 0x38, 0xe3, 0x01, 0xf8, 0xff, 0x30, 0x18, 0xef, 0xf3, 0x81, 0xc0, 0x7e, 0x00, 0x6f, 0xe6, 
	0x38, 0xe3, 0x01, 0xc0, 0xfc, 0x20, 0x1d, 0xcc, 0x73, 0x81, 0xc0, 0x3c, 0x1c, 0xec, 0x06, 0x38, 
	0xe3, 0x01, 0xc0, 0xde, 0x00, 0x1f, 0x8c, 0x73, 0x81, 0xff, 0x08, 0x0f, 0xcc, 0x03, 0xf3, 0xff, 
	0xf9, 0xfe, 0xcf, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00
};
// '25', 17x8px
const unsigned char bitmap_25 [] PROGMEM = {
	0x7e, 0x7f, 0x00, 0xe7, 0x60, 0x00, 0x0f, 0x7f, 0x00, 0x3f, 0x7f, 0x80, 0x7e, 0x03, 0x80, 0x7c, 
	0x03, 0x80, 0xf0, 0x63, 0x80, 0xff, 0x3f, 0x00
};
// '50', 17x8px
const unsigned char bitmap_50 [] PROGMEM = {
	0xfe, 0x1e, 0x00, 0xc0, 0x27, 0x00, 0xfe, 0x63, 0x80, 0xff, 0x63, 0x80, 0x03, 0x63, 0x80, 0x03, 
	0x63, 0x80, 0xe7, 0x3b, 0x00, 0x7e, 0x1e, 0x00
};
// '75', 17x8px
const unsigned char bitmap_75 [] PROGMEM = {
	0xff, 0x7f, 0x00, 0xe7, 0x60, 0x00, 0x0e, 0x7f, 0x00, 0x0e, 0x7f, 0x80, 0x3c, 0x03, 0x80, 0x38, 
	0x03, 0x80, 0x38, 0x63, 0x80, 0x38, 0x3f, 0x00
};
// '100', 25x8px
const unsigned char bitmap_100 [] PROGMEM = {
	0x38, 0x3c, 0x1c, 0x00, 0x78, 0x4e, 0x27, 0x00, 0x38, 0xc7, 0xe3, 0x80, 0x38, 0xc7, 0xe3, 0x80, 
	0x38, 0xc7, 0xe3, 0x80, 0x38, 0xc7, 0xe3, 0x80, 0x38, 0x76, 0x33, 0x00, 0xfe, 0x3c, 0x1c, 0x00
};
// 'Text', 25x47px
const unsigned char bitmap_Text [] PROGMEM = {
	0x38, 0x3c, 0x1c, 0x00, 0x78, 0x4e, 0x27, 0x00, 0x38, 0xc7, 0xe3, 0x80, 0x38, 0xc7, 0xe3, 0x80, 
	0x38, 0xc7, 0xe3, 0x80, 0x38, 0xc7, 0xe3, 0x80, 0x38, 0x76, 0x33, 0x00, 0xfe, 0x3c, 0x1c, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xf7, 0xf0, 0x00, 0x0e, 0x76, 0x00, 0x00, 0x00, 0xe7, 0xf0, 0x00, 
	0x00, 0xe7, 0xf8, 0x00, 0x03, 0xc0, 0x38, 0x00, 0x03, 0x80, 0x38, 0x00, 0x03, 0x86, 0x38, 0x00, 
	0x03, 0x83, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xe1, 0xe0, 0x00, 0x0c, 0x02, 0x70, 0x00, 
	0x0f, 0xe6, 0x38, 0x00, 0x0f, 0xf6, 0x38, 0x00, 0x00, 0x36, 0x38, 0x00, 0x00, 0x36, 0x38, 0x00, 
	0x0e, 0x73, 0xb0, 0x00, 0x07, 0xe1, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe7, 0xf0, 0x00, 
	0x0e, 0x76, 0x00, 0x00, 0x00, 0xf7, 0xf0, 0x00, 0x03, 0xf7, 0xf8, 0x00, 0x07, 0xe0, 0x38, 0x00, 
	0x07, 0xc0, 0x38, 0x00, 0x0f, 0x06, 0x38, 0x00, 0x0f, 0xf3, 0xf0, 0x00
};
// 'Bar_off', 32x8px
const unsigned char bitmap_Bar_off [] PROGMEM = {
	0x15, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 
	0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa, 0xaa, 0xaa, 0x55, 0x55, 0x55, 0x54, 0x2a, 0xaa, 0xaa, 0xa8
};
// 'Bar_on', 32x8px
const unsigned char bitmap_Bar_on [] PROGMEM = {
	0x3f, 0xff, 0xff, 0xfc, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xfc
};
// 'Ready', 50x8px
const unsigned char bitmap_Ready [] PROGMEM = {
	0xfe, 0x7f, 0x8e, 0x3f, 0x0c, 0xe1, 0xc0, 0xe7, 0x60, 0x1b, 0xb9, 0x8c, 0xe1, 0xc0, 0xc3, 0x60, 
	0x71, 0xf8, 0xec, 0xe7, 0x80, 0xc7, 0x7e, 0x71, 0xf8, 0xef, 0xc7, 0x80, 0xff, 0x7c, 0x7f, 0xf8, 
	0xe7, 0x87, 0x00, 0xfc, 0x60, 0x7f, 0xf8, 0xe3, 0x86, 0x00, 0xde, 0x70, 0x71, 0xf9, 0x83, 0x80, 
	0x00, 0xcf, 0x7f, 0xf1, 0xff, 0x03, 0x8c, 0x00
};
// 'Logo', 128x64px
const unsigned char bitmap_Logo [] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xf0, 0x39, 0xfe, 0x7e, 0xe3, 0xb8, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xfc, 0x79, 0xff, 0xfe, 0xe3, 0xbc, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x8e, 0x7c, 0x38, 0xe7, 0xe3, 0xbc, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x8e, 0x6c, 0x38, 0xe0, 0xe3, 0xbe, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x86, 0xee, 0x38, 0xfc, 0xe3, 0xb6, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x86, 0xce, 0x38, 0x3e, 0xe3, 0xb7, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x8e, 0xfe, 0x38, 0x0f, 0xe3, 0xb3, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0x8f, 0xff, 0x38, 0xe7, 0xe3, 0xb3, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xfd, 0x87, 0x38, 0xff, 0x7f, 0x33, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xfb, 0x83, 0x38, 0x7e, 0x3e, 0x31, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
// 'initialising', 111x8px
const unsigned char bitmap_initialising [] PROGMEM = {
	0xfe, 0xc7, 0x7f, 0xff, 0x1f, 0xc7, 0x9c, 0x07, 0xf3, 0xe1, 0xfd, 0x86, 0x3f, 0x1e, 0x38, 0xe7, 
	0x1c, 0x1c, 0x07, 0x0f, 0xdc, 0x01, 0xc7, 0x30, 0x71, 0xe6, 0x60, 0x1e, 0x38, 0xf7, 0x0c, 0x1c, 
	0x07, 0x1c, 0xfc, 0x00, 0xc7, 0x00, 0x71, 0xf6, 0xc0, 0x3c, 0x38, 0xff, 0x0c, 0x1c, 0x07, 0x18, 
	0x7c, 0x00, 0xc3, 0xf0, 0x71, 0xfe, 0xc7, 0x38, 0x38, 0xff, 0x0c, 0x1c, 0x07, 0x1f, 0xfc, 0x00, 
	0xc1, 0xf8, 0x71, 0xfe, 0xc7, 0x30, 0x38, 0xdf, 0x0c, 0x1c, 0x07, 0x1f, 0xfc, 0x00, 0xc0, 0x1c, 
	0x71, 0xbe, 0xc7, 0x20, 0x38, 0xcf, 0x1c, 0x1c, 0x07, 0x18, 0x7c, 0x01, 0xc7, 0x1c, 0x71, 0x9e, 
	0x77, 0x00, 0xfe, 0xc7, 0x7f, 0x1c, 0x1f, 0xd8, 0x7f, 0xc7, 0xf3, 0xf1, 0xfd, 0x86, 0x3f, 0x40
};
// 'Truck', 118x38px
const unsigned char bitmap_Truck [] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x80, 0x00, 0x26, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x01, 0x80, 0x00, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x03, 0x00, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 
	0x00, 0x00, 0x21, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 
	0x00, 0x20, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 
	0x20, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x20, 
	0x30, 0x00, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x20, 0x1c, 
	0x00, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 
	0x01, 0xc0, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x08, 0x00, 0x00, 0x03, 0xff, 0x00, 
	0x20, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x08, 0x00, 0x00, 0x02, 0x01, 0xff, 0xf0, 
	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x04, 0x20, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x1f, 0xf0, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x04, 0x20, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x1c, 0x80, 
	0x00, 0x00, 0x00, 0x00, 0x04, 0x20, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x80, 0x00, 
	0x00, 0x00, 0x00, 0x04, 0x20, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x80, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x80, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x08, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x80, 0x00, 0x00, 0x7f, 0x80, 0x00, 0x00, 
	0x08, 0x00, 0x00, 0x02, 0x00, 0x3f, 0xc0, 0x04, 0x80, 0x00, 0x00, 0xc0, 0x60, 0x00, 0x00, 0x08, 
	0x00, 0x00, 0x02, 0x00, 0xe0, 0x60, 0x04, 0x80, 0x00, 0x03, 0xff, 0x30, 0x00, 0x00, 0x08, 0x00, 
	0x00, 0x02, 0x01, 0xbf, 0xb0, 0x04, 0x80, 0x00, 0x06, 0xff, 0x88, 0x00, 0x00, 0x08, 0x00, 0x00, 
	0x02, 0x01, 0x7f, 0xd8, 0x04, 0x80, 0x00, 0x0d, 0xff, 0xc8, 0x00, 0x00, 0x08, 0x00, 0x00, 0x02, 
	0x07, 0xff, 0xec, 0x04, 0xf8, 0x00, 0x1b, 0xff, 0xe4, 0x00, 0x00, 0x08, 0x00, 0x00, 0x02, 0x0d, 
	0xff, 0xf4, 0x0c, 0x0f, 0x00, 0x13, 0xff, 0xe4, 0x00, 0x00, 0x08, 0x00, 0x00, 0x02, 0x09, 0xff, 
	0xf6, 0x08, 0x01, 0x80, 0x13, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf9, 0xff, 0xf3, 
	0xf8, 0x00, 0xff, 0xf3, 0xff, 0xe4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0xff, 0xf0, 0x00, 
	0x00, 0x00, 0x03, 0xff, 0xe7, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xf9, 0xff, 0xf0, 0x00, 0x00, 
	0x00, 0x03, 0xff, 0xe0, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x01, 0xff, 0xf0, 0x00, 0x00, 0x00, 
	0x03, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x01, 
	0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0xff, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7f, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x80, 0x00
};


void setup() {
  Serial.begin(9600);
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  while (!Acc1.begin()) {
  Serial.println(F("MPU6050 not connected!"));
  delay(1000);
  }
  Serial.println(F("MPU6050 ready!"));

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!OLED.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  
  OLED.clearDisplay();
  OLED.drawBitmap(0,0,bitmap_Logo,128,64,SSD1306_WHITE);
  OLED.display();
  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  // OLED.display();
  delay(20); // Pause for  seconds
  Servo1.attach(14);
  Servo2.attach(27);
  pinMode(LEDR, OUTPUT);
  pinMode(LEDL, OUTPUT);
  pinMode(DRSB, INPUT_PULLUP);
  pinMode(SBB, INPUT_PULLUP);
  pinMode(SBB, INPUT_PULLUP);
  pinMode(DEMOButton, INPUT_PULLUP);
  Wire.begin();
  Servo1.write(0);
  Servo2.write(0);
	// Try to initialize!
  
  // Try to initialize!
  Serial.println(F("Start up Initialized!"));
  delay(2000);

  Acc1.setAccelerometerRange(MPU6050_RANGE_8_G);
  // set gyro range to +- 500 deg/s
  Acc1.setGyroRange(MPU6050_RANGE_500_DEG);
  // set filter bandwidth to 21 Hz
  Acc1.setFilterBandwidth(MPU6050_BAND_21_HZ);

  OLED.clearDisplay();
  OLED.drawBitmap(4,12,bitmap_Truck,118,38,SSD1306_WHITE);
  OLED.display();
  
  //for(i=0;i<3;i++)
  {
    digitalWrite(LEDL, on);
    for (pos=0; pos<=45; pos+=1);{
     Servo1.write(pos);
     delay(200);
     digitalWrite(LEDR, on);
     Servo2.write(pos);
      }
    for(pos=45;pos<=90; pos+=1); 
     Servo1.write(pos);
     delay(200);
    for(pos=90;pos<=135;pos+=1);
     Servo1.write(pos);
    for(pos=45;pos<=90;pos+=1);
     Servo2.write(pos);
     delay(200);
   for(pos=135;pos<=180;pos+=1);
     Servo1.write(pos);
   for(pos=90;pos<=135;pos+=1);
     (Servo2.write(pos));
     delay(200);
   for(pos=135;pos<=180;pos+=1);
     Servo2.write(pos);
     delay(200);
   for(pos=180; pos>=0; pos-=1);{
     Servo1.write(pos);
     digitalWrite(LEDL, off);
     Servo2.write(pos);
     digitalWrite(LEDR, off);
  }
  delay(500);
  }
  OLED.clearDisplay();
  OLED.drawBitmap(40,27,bitmap_Ready,50,8,SSD1306_WHITE);
  OLED.display();
  delay(2000);

  OLED.clearDisplay();
  OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
  OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
  OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
  OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
  OLED.drawBitmap(10,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(52,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(10,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(52,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(10,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(52,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(10,46,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.drawBitmap(52,46,bitmap_Bar_off,32,8,SSD1306_WHITE);
  OLED.display();
}

void loop() {
  delay(10); // this speeds up the simulation
  unsigned long currentMillis = millis();
    // Sensor Check
  sensors_event_t a, g, temp;
  Acc1.getEvent(&a, &g, &temp);
   if( currentMillis - previousMillis >= interval)
  {

  // Active Aero
  int DEMOMeasure=digitalRead(DEMOButton);
  int DRSMeasure = digitalRead(DRSB);
  int SBMeasure= digitalRead(SBB);
  int ABMeasure = digitalRead(ABB);
  if(DEMOMeasure == 1||a.acceleration.x!=0)
  {
    
      if	(a.acceleration.x>=-.5&&g.gyro.y<2.125&&g.gyro.y>-2.125){;
       {DRS = 0;
       Servo1.write(DRS);
       Servo2.write(DRS);
       }
       digitalWrite(LEDL, off);
       digitalWrite(LEDR, off);
       OLED.clearDisplay();
       OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
       OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
       OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
       OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
       OLED.drawBitmap(10,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(52,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(10,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(52,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(10,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(52,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(10,46,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.drawBitmap(52,46,bitmap_Bar_off,32,8,SSD1306_WHITE);
       OLED.display();
     }
     else{
       digitalWrite(LEDL, on);
       digitalWrite(LEDR, on);
     }
    
  {
  if	(a.acceleration.x<=-15){;
    {AB = 180;
     Servo1.write(AB);
     Servo2.write(AB);
     OLED.clearDisplay();
     OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
     OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
     OLED.drawBitmap(10,6,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,6,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,19,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,19,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.display();
   }
  }
  }

  {
  if	(a.acceleration.x<-0.5 && a.acceleration.x>-15&&(g.gyro.y<2.125||g.gyro.y>-2.125)){;
   {SB = 90;
     Servo1.write(SB);
     Servo2.write(SB);
     OLED.clearDisplay();
     OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
     OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
     OLED.drawBitmap(10,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.display();
   }
  }
  }

  {
  if (g.gyro.y>2.125 && a.acceleration.x<-7.5){;
  {
   RC=135;
   LC=45;
     Servo1.write(RC);
     Servo2.write(LC);
     OLED.clearDisplay();
     OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
     OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
     OLED.drawBitmap(10,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,19,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.display();
    }
  }
  }

  {
  if (g.gyro.y<-2.125 && a.acceleration.x<-7.5){;
  {
    RC=45;
    LC=135;
     Servo1.write(RC);
     Servo2.write(LC);
     OLED.clearDisplay();
     OLED.drawBitmap(99,44,bitmap_25,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,31,bitmap_50,17,8,SSD1306_WHITE);
     OLED.drawBitmap(99,19,bitmap_75,17,8,SSD1306_WHITE);
     OLED.drawBitmap(95,5,bitmap_100,25,8,SSD1306_WHITE);
     OLED.drawBitmap(10,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,6,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,19,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,19,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,32,bitmap_Bar_off,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,32,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(10,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.drawBitmap(52,46,bitmap_Bar_on,32,8,SSD1306_WHITE);
     OLED.display();
    }
  }
  }
     
  }
  else
   {
   if (currentMillis - previousMillis >= interval1) {
     // save the last time you blinked the LED
      previousMillis = currentMillis;

     // if the LED is off turn it on and vice-versa:
     if (off == LOW) {
       off = HIGH;
     } 
     else {
       off = LOW;
     }

     // set the LED with the ledState of the variable:
     digitalWrite(LEDL, off);
     digitalWrite(LEDR,off);
   }

   OLED.clearDisplay();
   OLED.drawBitmap(0,32,bitmap_title,120,15,SSD1306_WHITE);
   OLED.display();
   count700ms++;
    if( count700ms >= (80 / interval)){
      count700ms = 0;
      int angle1 = random( 0, 181);    // 0...180
      int angle2 = random( 0, 181);
      Servo1.write( angle1);
      Servo2.write(angle2);
   }

  }
  
 //Serial Print
  Serial.print(F("Angle: S1: "));
  Serial.print(Servo1.read());
  Serial.print(F("°, S2: "));
  Serial.print(Servo2.read());
  Serial.println(F("°"));
	/* Print out the values */
	Serial.print("Acceleration X: ");
	Serial.print(a.acceleration.x);// divide by 9.80665 to get gravity
	Serial.print(F(", Y: "));
	Serial.print(a.acceleration.y);// divide by 9.80665 to get gravity
	Serial.print(F(", Z: "));
	Serial.print(a.acceleration.z); // divide by 9.80665 to get gravity
	Serial.println(F(" m/s^2"));//G

	Serial.print(F("Rotation X: "));
	Serial.print(g.gyro.x);//*180/3.1416 to get degrees
	Serial.print(F(", Y: "));
	Serial.print(g.gyro.y);//*180/3.1416 to get degrees
	Serial.print(F(", Z: "));
	Serial.print(g.gyro.z);//*180/3.1416 to get degrees
	Serial.println(F(" rad/s"));//°

	delay(000);
  }
}