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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


//H bridge, top left, bottom left, top right, bottom right
const int IN1 = 9;
const int IN2 = 6;
const int IN3 = 8;
const int IN4 = 7;

const int delayTime = 100;

//Buttons
const int LED1 = 4;
const int LED2 = 5;
const int Button1 = 2;
const int Button2 = 3;

int but1;
int but2;

//Solenoids
bool solenoid1;
bool solenoid2;

//Load Cell

int force;
int newforce;

//LED status light
  const int ledPin = 13;

//Display

int i;
int n;
int m;

// Bitmaps

#define PICO_OPEN_BMP_WIDTH 128
#define PICO_OPEN_BMP_HEIGHT 64
#define LOGOS_BMP_WIDTH 128
#define LOGOS_BMP_HEIGHT 64

// 'Logos', 128x64px
const unsigned char bitmap_LOGOS[] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0x7f, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0x1f, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0x0f, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0x83, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xe0, 0xf7, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xf8, 0x41, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xfc, 0x01, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0x01, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xc1, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xc0, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xc0, 0x3f, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xc0, 0x0f, 0xfe, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xe3, 0x83, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x00, 0x00, 0x03, 0x07, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xe1, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x00, 0x00, 0x03, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xf0, 0x7e, 0x7f, 
	0xff, 0xff, 0x82, 0x00, 0x00, 0x01, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xfc, 0x7e, 0x7f, 
	0xff, 0xff, 0x82, 0x00, 0x00, 0x01, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0x7e, 0x7f, 
	0xff, 0xff, 0x82, 0x0f, 0xff, 0xc1, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0f, 0xff, 0xe1, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0f, 0xff, 0xe1, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0f, 0xff, 0xe1, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0f, 0xff, 0xc1, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x08, 0x00, 0x01, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0c, 0x00, 0x01, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0c, 0x00, 0x03, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xfc, 0x00, 0x1f, 0xfe, 0x7f, 
	0xff, 0xff, 0x82, 0x0e, 0x00, 0x07, 0x87, 0xff, 0xff, 0xfe, 0x1f, 0xc0, 0x00, 0x01, 0xfe, 0x7f, 
	0xff, 0xff, 0x02, 0x0f, 0xff, 0xff, 0x83, 0xff, 0xff, 0xfe, 0x1e, 0x00, 0x00, 0x00, 0x3e, 0x7f, 
	0xfc, 0x00, 0x06, 0x0f, 0xff, 0xff, 0x80, 0x00, 0x7f, 0xfe, 0x18, 0x00, 0x00, 0x00, 0x0e, 0x7f, 
	0xf8, 0x00, 0x06, 0x0f, 0xff, 0xff, 0x80, 0x00, 0x7f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xf0, 0x00, 0x0e, 0x0f, 0xff, 0xff, 0xc0, 0x00, 0x3f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xf0, 0x00, 0x1e, 0x0f, 0xff, 0xff, 0xe0, 0x00, 0x1f, 0xfe, 0x00, 0x1e, 0x1e, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x3f, 0x3f, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x73, 0x33, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x63, 0x21, 0xb0, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x61, 0x31, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x70, 0x30, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x30, 0x38, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x38, 0x38, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x1c, 0x1c, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x0e, 0x0e, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x07, 0x07, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x03, 0x03, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x03, 0x81, 0xb0, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x61, 0xf1, 0xb0, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x61, 0xf1, 0xb0, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x73, 0x33, 0x30, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x3f, 0x3f, 0x3f, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x1e, 0x1e, 0x3f, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

// 'PICO OPEN', 128x64px
const unsigned char bitmap_PICO_OPEN [] 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, 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, 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, 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, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x03, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x07, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x1c, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x38, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x30, 0x78, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x61, 0xfe, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xc3, 0xff, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xc7, 0xff, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xc7, 0xff, 0x90, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xcf, 0xff, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0xcf, 0xff, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0x8f, 0xff, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0x8f, 0xff, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0x8f, 0xff, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0x8f, 0xff, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0x8f, 0xff, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xcf, 0xff, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x47, 0xff, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0xf1, 0xfe, 0x31, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x03, 0x88, 0xfc, 0x20, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x06, 0x04, 0x00, 0x60, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x18, 0x02, 0x00, 0xd1, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x3f, 0x01, 0x01, 0x96, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x70, 0xc0, 0x87, 0x18, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x60, 0x60, 0xfc, 0x1f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x30, 0x60, 0x7c, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x18, 0x20, 0xc1, 0xf8, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x18, 0x61, 0x8e, 0x0f, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x47, 0x0c, 0xe6, 0x18, 0xcc, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x47, 0x8f, 0x9e, 0x73, 0x19, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x44, 0xc6, 0x16, 0x64, 0x63, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x44, 0xc4, 0x16, 0x19, 0x8f, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x47, 0xc4, 0x13, 0xc4, 0x3c, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x47, 0x84, 0x10, 0xf3, 0xf1, 0xe4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x43, 0x04, 0x10, 0x78, 0x03, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x1f, 0xff, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x01, 0x7c, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x60, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x70, 0x04, 0x10, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x0e, 0x04, 0x1c, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x01, 0xc4, 0x62, 0x01, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x7f, 0x81, 0xc1, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x0c, 0x00, 0x31, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x0f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

void setup() {
  Serial.begin(9600);
  //H Bridge
  pinMode (IN1, OUTPUT);
  pinMode (IN2, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN4, OUTPUT);
  
  // Buttons
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(Button1, INPUT);
  pinMode(Button2, INPUT);

  // Close both Solenoids
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  delay(50);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

  solenoid1 = false;
  solenoid2 = false;

  //LED status light
  pinMode(ledPin, OUTPUT);

  //OLED Display

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  display.begin(SSD1306_SWITCHCAPVCC, 0x3c);
  display.fillScreen(0);
  display.drawBitmap(0, 0, bitmap_LOGOS, 128, 64, WHITE);
  display.display();
  delay(2000);
  display.clearDisplay();
  display.drawBitmap(30, 0, bitmap_PICO_OPEN, 128, 64, WHITE);
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();
  display.startscrollleft(0x00, 0x0f);
  delay(2000);
  display.stopscroll();
  display.clearDisplay();
  display.drawBitmap(0, 0, bitmap_PICO_OPEN, 128, 64, WHITE);
  display.setCursor(10,0);
  display.print("PICO Controller");
  display.display();
  display.setTextSize(1);
  display.setCursor(48,20);
  delay(1000);
  display.print("Programmed by");
  display.setCursor(48,30);
  display.print(" Brandon Park");
  display.setCursor(48,40);
  display.print("    at SSL");
  display.display();
  delay(1250);

  i = 0;
  m = 15;
}

void loop() {
  // Buttons
  but1 = digitalRead(Button1);
  but2 = digitalRead(Button2);

//left button toggles solenoid 1

  if (but2 == HIGH){
    if (solenoid1 == false){
      solenoid1 = true;
    }
    else{
      solenoid1 = false;
    }
    if (solenoid1 == true){
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
      delay(50);
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, LOW);
    }
    else{
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      delay(50);
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, LOW);
    }
  delay(200);
  }

//right button toggles both solenoids

  if (but1 == HIGH){ 
    
    if (solenoid1 == false){
      solenoid1 = true;
    }
    else{
      solenoid1 = false;
    }

    if (solenoid2 == false){
      solenoid2 = true;
    }
    else{
      solenoid2 = false;
    }
    if (solenoid1 == true){
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
    }
    else{
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
    }

    if (solenoid2 == true){
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
    }
    else{
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
    }
    delay(50);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  
  //delay adjusts button press timing

  delay(200);
  }
  
  force = newforce;
  force += 1;
  newforce = force;
  delay(10);

  // OLED Display 
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print(force);
  display.setTextSize(1);
  display.print("lbs");
  
  display.setCursor(50, 2);
  display.print("      SSL|JPL");
  display.drawLine(53, 17, 53, 63, WHITE);
  display.drawLine(53, 40, 128, 40, WHITE);

  if (solenoid1 == true){
      digitalWrite(LED2, HIGH);
      display.setCursor(60,20);
      display.setTextSize(2);
      display.print("WASTE");
  }
  else{
      digitalWrite(LED2, LOW);
      display.setCursor(60,20);
      display.setTextSize(2);
      display.print("PICO");
  }

  if (solenoid2 == true){
      digitalWrite(LED1, HIGH);
      display.setCursor(60,45);
      display.setTextSize(2);
      display.print("WATER");
  }
  else{
      digitalWrite(LED1, LOW);
      display.setCursor(60,45);
      display.setTextSize(2);
      display.print("GAS");
  }
  
  display.drawBitmap(0, 0, bitmap_PICO_OPEN, 128, 64, WHITE);

  //particle animation
  n = m;
  n += 1;
  
  if(n>40){
    i += 1;
    n = 40;
  }
  if(i>25){
    n = 16;
    i = 0;
  }
  m = n;

  display.drawPixel(33, n+3, WHITE);
  display.drawPixel(31, n, WHITE);
  display.drawPixel(38, n+1, WHITE);
  display.drawPixel(29, n+5, WHITE);
  display.drawPixel(28, n+1, WHITE);
  display.drawPixel(40, n+6, WHITE);
  display.drawPixel(36, n+8, WHITE);
  display.drawPixel(27, n+9, WHITE);
  display.display();
}