//Libraries needed
#include <VarSpeedServo.h>
#include <Adafruit_GFX.h> 
#include <Adafruit_SH1106.h> 
#include <QMC5883L.h>
#include <Wire.h>

//Ultrasonic sensor settings
#define SenseMetricSystem
#define triPin 4 
#define echoPin 3

//Magnometer(digital compass)ssttings
QMC5883L compass;


//OLED display settings
#define OLED_RESET 4
Adafruit_SH1106 display(OLED_RESET);

// OLED Display Hello World Message
const unsigned char HELLO_WORLD [] PROGMEM = {
  0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 
  0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 
  0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 
  0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
  0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 
  0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 
  0xf0, 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, 0x00, 0x00, 0x00, 0x07, 
  0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xfe, 0x1f, 0xc0, 0x07, 0xf0, 0x00, 0x07, 0xf0, 0x00, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xfe, 0x1f, 0xc0, 0x07, 0xf0, 0x00, 0x3f, 0xfe, 0x00, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xfe, 0x1f, 0xc0, 0x07, 0xf0, 0x00, 0xff, 0xff, 0x80, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xfc, 0x00, 0x1f, 0xc0, 0x07, 0xf0, 0x01, 0xfe, 0x3f, 0xc0, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xfc, 0x00, 0x1f, 0xc0, 0x07, 0xf0, 0x01, 0xf8, 0x0f, 0xc0, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xfc, 0x00, 0x1f, 0xc0, 0x07, 0xf0, 0x03, 0xf8, 0x0f, 0xe0, 0x00, 
  0x00, 0x00, 0xff, 0xff, 0xe1, 0xff, 0xfc, 0x1f, 0xc0, 0x07, 0xf0, 0x03, 0xf8, 0x0f, 0xe0, 0x00, 
  0x00, 0x00, 0xff, 0xff, 0xe1, 0xff, 0xfc, 0x1f, 0xc0, 0x07, 0xf0, 0x03, 0xf8, 0x0f, 0xe0, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xfc, 0x00, 0x1f, 0xc0, 0x07, 0xf0, 0x01, 0xf8, 0x0f, 0xc0, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xfc, 0x00, 0x1f, 0xc0, 0x07, 0xf0, 0x01, 0xfe, 0x3f, 0xc0, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xff, 0x1f, 0xff, 0xe7, 0xff, 0xf8, 0xff, 0xff, 0x80, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xff, 0x1f, 0xff, 0xe7, 0xff, 0xf8, 0x3f, 0xfe, 0x00, 0x00, 
  0x00, 0x00, 0xfe, 0x0f, 0xe1, 0xff, 0xff, 0x1f, 0xff, 0xe7, 0xff, 0xf8, 0x07, 0xf0, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x7f, 0x0f, 0xc7, 0xf0, 0x07, 0xf0, 0x03, 0xff, 0xf0, 0x0f, 0xe0, 0x03, 0xff, 0xe0, 0x00, 
  0x00, 0x3f, 0x0f, 0xc7, 0xe0, 0x7f, 0xff, 0x03, 0xff, 0xfe, 0x0f, 0xe0, 0x03, 0xff, 0xfc, 0x00, 
  0x00, 0x3f, 0x8f, 0xc7, 0xe0, 0xff, 0xff, 0x83, 0xff, 0xfe, 0x0f, 0xe0, 0x03, 0xff, 0xfe, 0x00, 
  0x00, 0x1f, 0x9f, 0xe7, 0xc1, 0xfc, 0x1f, 0xc3, 0xf8, 0x7f, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x00, 
  0x00, 0x1f, 0x9f, 0xe7, 0xc1, 0xf8, 0x0f, 0xc3, 0xf8, 0x7f, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x00, 
  0x00, 0x1f, 0x9f, 0xef, 0xc3, 0xf8, 0x0f, 0xe3, 0xf8, 0x7e, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x80, 
  0x00, 0x0f, 0xff, 0xff, 0x83, 0xf8, 0x0f, 0xe3, 0xff, 0xfc, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x80, 
  0x00, 0x0f, 0xfd, 0xff, 0x83, 0xf8, 0x0f, 0xe3, 0xff, 0xfc, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x80, 
  0x00, 0x07, 0xfc, 0xff, 0x81, 0xfc, 0x1f, 0xc3, 0xf8, 0x7f, 0x0f, 0xe0, 0x03, 0xf8, 0x3f, 0x00, 
  0x00, 0x07, 0xfc, 0xff, 0x01, 0xfe, 0x3f, 0xc3, 0xf8, 0x7f, 0x0f, 0xe0, 0x03, 0xf8, 0xff, 0x00, 
  0x00, 0x07, 0xf8, 0xff, 0x00, 0x7f, 0xff, 0x03, 0xf8, 0x7f, 0x0f, 0xff, 0xf3, 0xff, 0xfc, 0x00, 
  0x00, 0x03, 0xf8, 0x7f, 0x00, 0x3f, 0xfe, 0x03, 0xf8, 0x7f, 0x0f, 0xff, 0xf3, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
  0x80, 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, 0x03, 
  0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 
  0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 
  0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 
  0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 
  0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 
  0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 
  0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff
};

// OLED Display Smile Message
const unsigned char SMILE [] 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, 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, 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, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xf8, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 
  0xff, 0xe0, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0xff, 0xff, 
  0xff, 0xc0, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x3f, 0xff, 
  0xff, 0x80, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x1f, 0xff, 
  0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x07, 0xff, 
  0xff, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0x80, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0x80, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0xe0, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x3f, 0xff, 
  0xff, 0xf0, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x3f, 0xff, 
  0xff, 0xf0, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 
  0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 
  0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 
  0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 
  0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 
  0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 
  0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 
  0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 
  0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 
  0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 
  0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0x00, 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, 0x0f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x03, 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, 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, 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, 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, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

// OLED Display Frown Message
const unsigned char FROWN [] 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, 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, 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, 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, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 
  0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 
  0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 
  0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 
  0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 
  0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 
  0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 
  0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 
  0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 
  0xff, 0xf0, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 
  0xff, 0xf0, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x3f, 0xff, 
  0xff, 0xe0, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x3f, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0xc0, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0x80, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x1f, 0xff, 
  0xff, 0x80, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 
  0xfe, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x07, 0xff, 
  0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x07, 0xff, 
  0xff, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x07, 0xff, 
  0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x0f, 0xff, 
  0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x0f, 0xff, 
  0xff, 0x80, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x1f, 0xff, 
  0xff, 0xc0, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x3f, 0xff, 
  0xff, 0xe0, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0xff, 0xff, 
  0xff, 0xf8, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe7, 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, 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, 
  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, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

//Servo settings
VarSpeedServo myservoLEFTANKLE;
VarSpeedServo myservoRIGHTANKLE;
VarSpeedServo myservoLEFTLEG;
VarSpeedServo myservoRIGHTLEG;
VarSpeedServo myservoLEFTHIP;
VarSpeedServo myservoRIGHTHIP;
VarSpeedServo myservoLEFTROTATION;
VarSpeedServo myservoRIGHTROTATION;

//Array for storing Desired Orientation of BORIS when in autonomous mode
byte dataArray[1];          
int arrayIndex = 0;

//Buzzer settings
int BuzzerPin = 17;

//Buzzer notes
#define NOTE_B0  31
#define NOTE_C1  33
#define NOTE_CS1 35
#define NOTE_D1  37
#define NOTE_DS1 39
#define NOTE_E1  41
#define NOTE_F1  44
#define NOTE_FS1 46
#define NOTE_G1  49
#define NOTE_GS1 52
#define NOTE_A1  55
#define NOTE_AS1 58
#define NOTE_B1  62
#define NOTE_C2  65
#define NOTE_CS2 69
#define NOTE_D2  73
#define NOTE_DS2 78
#define NOTE_E2  82
#define NOTE_F2  87
#define NOTE_FS2 93
#define NOTE_G2  98
#define NOTE_GS2 104
#define NOTE_A2  110
#define NOTE_AS2 117
#define NOTE_B2  123
#define NOTE_C3  131
#define NOTE_CS3 139
#define NOTE_D3  147
#define NOTE_DS3 156
#define NOTE_E3  165
#define NOTE_F3  175
#define NOTE_FS3 185
#define NOTE_G3  196
#define NOTE_GS3 208
#define NOTE_A3  220
#define NOTE_AS3 233
#define NOTE_B3  247
#define NOTE_C4  262
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_DS4 311
#define NOTE_E4  330
#define NOTE_F4  349
#define NOTE_FS4 370
#define NOTE_G4  392
#define NOTE_GS4 415
#define NOTE_A4  440
#define NOTE_AS4 466
#define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
#define NOTE_F5  698
#define NOTE_FS5 740
#define NOTE_G5  784
#define NOTE_GS5 831
#define NOTE_A5  880
#define NOTE_AS5 932
#define NOTE_B5  988
#define NOTE_C6  1047
#define NOTE_CS6 1109
#define NOTE_D6  1175
#define NOTE_DS6 1245
#define NOTE_E6  1319
#define NOTE_F6  1397
#define NOTE_FS6 1480
#define NOTE_G6  1568
#define NOTE_GS6 1661
#define NOTE_A6  1760
#define NOTE_AS6 1865
#define NOTE_B6  1976
#define NOTE_C7  2093
#define NOTE_CS7 2217
#define NOTE_D7  2349
#define NOTE_DS7 2489
#define NOTE_E7  2637
#define NOTE_F7  2794
#define NOTE_FS7 2960
#define NOTE_G7  3136
#define NOTE_GS7 3322
#define NOTE_A7  3520
#define NOTE_AS7 3729
#define NOTE_B7  3951
#define NOTE_C8  4186
#define NOTE_CS8 4435
#define NOTE_D8  4699
#define NOTE_DS8 4978



void setup() 
{

  // Serial begin for debugging in the serial monitor 
  Serial.begin(9600);

  //OLED initialise
  display.begin(SH1106_SWITCHCAPVCC, 0x3C); //initialize with the I2C addr 0x3C (128x64)
  
  //OLED display welcome message
  display.clearDisplay();
  display.drawBitmap(0, 0, HELLO_WORLD, 128, 64, WHITE);
  display.display();
  delay(10);

  //Buzzer welcome message
  HappySound();
  delay(50);
  SadSound();
  delay(50);
  HappySound();
  delay(50);
  
  // Attach servos to pins 
  myservoLEFTANKLE.attach(5);          //4 
  myservoRIGHTANKLE.attach(16);        //9 
  myservoLEFTLEG.attach(6);            //5  
  myservoRIGHTLEG.attach(15);          //8 
  myservoLEFTHIP.attach(7);            //6
  myservoRIGHTHIP.attach(14);          //7
  
  //Home servos
  myservoLEFTANKLE.write(90,100);         
  myservoRIGHTANKLE.write(90,100);          
  myservoLEFTLEG.write(90,100);          
  myservoRIGHTLEG.write(90,100);
  myservoLEFTHIP.write(90,100);           
  myservoRIGHTHIP.write(90,100);
 
  delay(2000);

  // detach servos to avoid jitter 
  myservoLEFTANKLE.detach();          
  myservoRIGHTANKLE.detach();        
  myservoLEFTLEG.detach();          
  myservoRIGHTLEG.detach();               
  myservoLEFTHIP.detach();
  myservoRIGHTHIP.detach();
    
  // OLED display Smile message
  display.clearDisplay();
  display.drawBitmap(0, 0, SMILE, 128, 64, WHITE);
  display.display();
  delay(10);

  //Ultrasonic setup settings
  pinMode(triPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Magnometer (digital compass) setup settings
  Wire.begin();
  compass.init();
  compass.setSamplingRate(50);
 
  //Calibrate the compass for 10 seconds
  while (millis() < 10000)
    {
      int heading = compass.readHeading();
      Serial.print("Turn compass in all directions to calibrate.  ");
      Serial.print("heading: ");
      Serial.print(heading);
      Serial.println(" ");
    }
  
}

void loop()
{ 
      //Ultrasonic readings
      long duration, distance;
      digitalWrite(triPin, LOW);  //PULSE
      delayMicroseconds(2); 
      digitalWrite(triPin, HIGH); 
      delayMicroseconds(10); 
      digitalWrite(triPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;

      //Magnometer readings
      int Orientation = compass.readHeading();
      Orientation = map(Orientation, 0, 360, 0, 255);
      int DesiredOrientation = dataArray[arrayIndex];
      int OrientationError = DesiredOrientation - Orientation; 
      Serial.print(" Orientation: ");
      Serial.print(Orientation);
      Serial.print(" DesiredOrientation: ");
      Serial.print(DesiredOrientation);
      Serial.println(" "); 

      while (millis() < 15000)
      {
        int Orientation = compass.readHeading();
        Orientation = map(Orientation, 0, 360, 0, 255);
        int DesiredOrientation;
        dataArray[arrayIndex] = Orientation;
        DesiredOrientation = dataArray[arrayIndex];
        int OrientationError = DesiredOrientation - Orientation; 
        Serial.print(" Orientation: ");
        Serial.print(Orientation);
        Serial.print(" DesiredOrientation: ");
        Serial.print(DesiredOrientation);
        Serial.println(" "); 

      }

      if (OrientationError < -127.5)
      {      
          OrientationError += 255;
      }
      
      if (OrientationError > 127.5)
      {
          OrientationError -= 255;
      }
      
////////////////////////// THIS IS WHERE YOU CAN START MODIFYING THE PROGRAM ////////////////////////      

      //If Sensor detect wall
      if (distance > 2 && distance < 20)
      {
          Frown();
          SadSound();
          RobotBackward();
          RobotBackward();
          RobotLeft();
          RobotLeft();
          RobotLeft();
          RobotLeft();
          RobotForward();
          RobotForward();
          RobotForward();
      }

      //If Sensor detect no wall and the Orientation = Desired orientation +- 30 degrees 
      if (distance >= 20 && OrientationError <= 30 && OrientationError >= - 30)
      {
          Smile();
          HappySound(); 
          RobotForward();
          RobotForward();  
      }

      //If Sensor detect no wall and Orientation > Desired orientation +- 30 degrees
      if (distance >= 20 && OrientationError < 0)
      {
          Smile();
          RobotLeft();
      }

      //If Sensor detect no wall and Orientation < Desired orientation +- 30 degrees
      if (distance >= 20 && OrientationError > 0)
      {
          Smile();
          RobotRight();
      }
}   

void RobotStop()
{
    myservoLEFTANKLE.write(90);         
    myservoRIGHTANKLE.write(90);          
    myservoLEFTLEG.write(90);          
    myservoRIGHTLEG.write(90);
    myservoLEFTHIP.write(90);           
    myservoRIGHTHIP.write(90);

    delay(500);    
          
    myservoLEFTANKLE.detach();          
    myservoRIGHTANKLE.detach();         
    myservoLEFTHIP.detach();            
    myservoRIGHTHIP.detach();
    myservoLEFTLEG.detach();            
    myservoRIGHTLEG.detach();
    myservoLEFTROTATION.detach();   
    myservoRIGHTROTATION.detach();  
}

void RobotForward()
{
    myservoLEFTANKLE.attach(5);          //4 
    myservoRIGHTANKLE.attach(16);        //9 
    myservoLEFTLEG.attach(6);            //5  
    myservoRIGHTLEG.attach(15);          //8 
    myservoLEFTHIP.attach(7);            //6
    myservoRIGHTHIP.attach(14);          //7     

    myservoLEFTHIP.write(90);       
    myservoRIGHTHIP.write(90);       

    myservoLEFTANKLE.write(180,130);         
    myservoRIGHTANKLE.write(180,130);

    delay(300);  

    myservoLEFTLEG.write(110,40);          
    myservoRIGHTLEG.write(110,40);

    delay(300); 

    myservoLEFTANKLE.write(0,130);         
    myservoRIGHTANKLE.write(0,130);          

    delay(300);  

    myservoLEFTLEG.write(70,40);          
    myservoRIGHTLEG.write(70,40);

    delay(300);    
}

void RobotBackward()
{
    myservoLEFTANKLE.attach(5);          //4 
    myservoRIGHTANKLE.attach(16);        //9 
    myservoLEFTLEG.attach(6);            //5  
    myservoRIGHTLEG.attach(15);          //8 
    myservoLEFTHIP.attach(7);            //6
    myservoRIGHTHIP.attach(14);          //7
         
    myservoLEFTHIP.write(90);       
    myservoRIGHTHIP.write(90);       

    myservoLEFTANKLE.write(180,130);         
    myservoRIGHTANKLE.write(180,130);          
          
    delay(300);

    myservoLEFTLEG.write(70,40);          
    myservoRIGHTLEG.write(70,40);

    delay(300);

    myservoLEFTANKLE.write(0,130);         
    myservoRIGHTANKLE.write(0,130);          

    delay(300);

    myservoLEFTLEG.write(110,40);          
    myservoRIGHTLEG.write(110,40);

    delay(300);    
}  

void RobotLeft()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7
          
   myservoLEFTLEG.write(90);          
   myservoRIGHTLEG.write(90);

   myservoLEFTANKLE.write(180,130);         
   myservoRIGHTANKLE.write(180,130);          

   delay(400);

   myservoLEFTHIP.write(120,130);       
          
   delay(350);

   myservoLEFTANKLE.write(0,130);         
   myservoRIGHTANKLE.write(0,130);          

   delay(400);

   myservoLEFTHIP.write(90,130);

   delay(300); 
}

void RobotRight()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7
          
   myservoLEFTLEG.write(90);          
   myservoRIGHTLEG.write(90);

   myservoLEFTANKLE.write(0,130);         
   myservoRIGHTANKLE.write(0,130);         

   delay(400);

   myservoRIGHTHIP.write(60,130);       
          
   delay(350);

   myservoLEFTANKLE.write(180,130);         
   myservoRIGHTANKLE.write(180,130);          

   delay(400);

   myservoRIGHTHIP.write(90,130);

   delay(300);    
}

void RobotLeftKick()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7
         
   myservoLEFTANKLE.write(0,100);         
   myservoRIGHTANKLE.write(0,100);          
          
   delay(300);
          
   myservoLEFTLEG.write(0);
            
   delay(300); 
          
   myservoLEFTLEG.write(90);   

   delay(300);         
}   

void RobotRightKick()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7
          
   myservoLEFTANKLE.write(180,100);         
   myservoRIGHTANKLE.write(180,100);          
          
   delay(300);
          
   myservoRIGHTLEG.write(180); 

   delay(300);

   myservoRIGHTLEG.write(90); 

   delay(300);   
}  

void RobotDance1()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7

   myservoLEFTANKLE.write(180,150);         
   myservoRIGHTANKLE.write(180,150); 

   delay(400);

   myservoLEFTANKLE.write(0,150);         
   myservoRIGHTANKLE.write(0,150); 

   delay(400);

   myservoLEFTANKLE.write(180,150);         
   myservoRIGHTANKLE.write(180,150); 

   delay(400);

   myservoLEFTANKLE.write(0,150);         
   myservoRIGHTANKLE.write(0,150); 

   delay(400);

   myservoLEFTANKLE.write(180,150);         
   myservoRIGHTANKLE.write(180,150); 

   delay(600);

   myservoLEFTANKLE.write(0,150);         
   myservoRIGHTANKLE.write(0,150); 

   delay(400);

   myservoLEFTANKLE.write(90,100);         
   myservoRIGHTANKLE.write(90,100); 

   delay(300);

   myservoLEFTLEG.write(110,50);          
   myservoRIGHTLEG.write(70,50);

   delay(300);

   myservoLEFTLEG.write(90,50);          
   myservoRIGHTLEG.write(90,50);

   delay(300);

   myservoLEFTLEG.write(110,50);          
   myservoRIGHTLEG.write(70,50);

   delay(300);

   myservoLEFTLEG.write(90,50);          
   myservoRIGHTLEG.write(90,50);

   delay(300);

   myservoLEFTLEG.write(110,50);          
   myservoRIGHTLEG.write(70,50);

   delay(300);

   myservoLEFTLEG.write(90,50);          
   myservoRIGHTLEG.write(90,50);

   delay(300);
}  

void RobotDance2()
{
   myservoLEFTANKLE.attach(5);          //4 
   myservoRIGHTANKLE.attach(16);        //9 
   myservoLEFTLEG.attach(6);            //5  
   myservoRIGHTLEG.attach(15);          //8 
   myservoLEFTHIP.attach(7);            //6
   myservoRIGHTHIP.attach(14);          //7

   myservoLEFTANKLE.write(180);         
   myservoRIGHTANKLE.write(0); 

   delay(300);

   myservoLEFTANKLE.write(0);         
   myservoRIGHTANKLE.write(180); 

   delay(300);

   myservoLEFTANKLE.write(180);         
   myservoRIGHTANKLE.write(0); 

   delay(300);

   myservoLEFTANKLE.write(0);         
   myservoRIGHTANKLE.write(180); 

   delay(300);

   myservoLEFTANKLE.write(180);         
   myservoRIGHTANKLE.write(0); 

   delay(300);

   myservoLEFTANKLE.write(0);         
   myservoRIGHTANKLE.write(180); 

   delay(300);

   myservoLEFTANKLE.write(180);         
   myservoRIGHTANKLE.write(0); 

   delay(300);

   myservoLEFTANKLE.write(0);         
   myservoRIGHTANKLE.write(180); 

   delay(300);

   myservoLEFTANKLE.write(90);         
   myservoRIGHTANKLE.write(90); 

   delay(300);

   myservoRIGHTHIP.write(70);
   myservoLEFTHIP.write(110);

   delay(300);

   myservoRIGHTHIP.write(110);
   myservoLEFTHIP.write(70);

   delay(300);

   myservoRIGHTHIP.write(70);
   myservoLEFTHIP.write(110);

   delay(300);

   myservoRIGHTHIP.write(110);
   myservoLEFTHIP.write(70);

   delay(300);

   myservoRIGHTHIP.write(70);
   myservoLEFTHIP.write(110);

   delay(300);

   myservoRIGHTHIP.write(110);
   myservoLEFTHIP.write(70);

   delay(300);

   myservoRIGHTHIP.write(70);
   myservoLEFTHIP.write(110);

   delay(300);

   myservoRIGHTHIP.write(110);
   myservoLEFTHIP.write(70);

   delay(300);

   myservoRIGHTHIP.write(90);
   myservoLEFTHIP.write(90);

   delay(300);

   myservoLEFTLEG.write(120);          
   myservoRIGHTLEG.write(120);

   delay(300);

   myservoLEFTLEG.write(60);          
   myservoRIGHTLEG.write(60);

   delay(300);

   myservoLEFTLEG.write(120);          
   myservoRIGHTLEG.write(120);

   delay(300);

   myservoLEFTLEG.write(60);          
   myservoRIGHTLEG.write(60);

   delay(300);

   myservoLEFTLEG.write(120);          
   myservoRIGHTLEG.write(120);

   delay(300);

   myservoLEFTLEG.write(60);          
   myservoRIGHTLEG.write(60);

   delay(300);

   myservoLEFTLEG.write(120);          
   myservoRIGHTLEG.write(120);

   delay(300);

   myservoLEFTLEG.write(60);          
   myservoRIGHTLEG.write(60);

   delay(300);

   myservoLEFTLEG.write(90);          
   myservoRIGHTLEG.write(90);

   delay(300);

   

   

}        

void Smile()
{
   display.clearDisplay();
   display.drawBitmap(0, 0, SMILE, 128, 64, WHITE);
   display.display();  
}

void Frown()
{
  
   display.clearDisplay();
   display.drawBitmap(0, 0, FROWN, 128, 64, WHITE);
   display.display();
}

void HappySound()
{ 
   int HappySound = 100;
   for(HappySound = 100;HappySound < 4000;HappySound +=10) 
   {
        tone(BuzzerPin, HappySound, 0);
        delay(1);
   }    
   tone(BuzzerPin, 4000, 10);
   delay(20);  
}

void SadSound()
{
   int SadSound = 4000;
   for(SadSound = 4000;SadSound > 100;SadSound -=10) 
   {
        tone(BuzzerPin, SadSound, 0);
        delay(1);
   }    
   tone(BuzzerPin, 100, 10);
   delay(20);
}