robot

#include <Servo.h>
// Pins & Motors
const byte PIN_ANALOG_X = 0;
const byte PIN_ANALOG_Y = 1;
const byte PIN_BUTTON_LEFT = 2;
const byte PIN_BUTTON_DOWN = 3;
const byte PIN_BUTTON_RIGHT = 4;
const byte PIN_BUTTON_UP = 5;
Servo s1;
Servo s2;
#define echoPin 10
#define trigPin 9
// Mode switch
enum mode {autonomous, autonomousNoMap, manual} ;
mode currentMode;
// Speeds & Directional vars
int leftWheelSpeed = 84;// 80 ~ 90
int rightWheelSpeed = 96;// 100 ~ 90
int upperLimDif = 10;
bool forwards = false;
bool backwards = false;
bool waitForRightButtonUp = false;
bool waitForDownButtonUp = false;
bool waitForUpButtonUp = false;
bool waitForLeftButtonUp = false;
float turnIntervalTimer = 0;
bool stopped = false;
// Navigation and Mapping
int leftDistance;
int rightDistance;
int checkDelay = 300;
int turnDelay = 400;
int map2d[10][10] = {
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},

  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2},
  {2, 2, 2, 2, 2,  2, 2, 2, 2, 2}
};
// Ultrasonic
long duration;
int forwardsDistance;


void setup() {
  currentMode = autonomousNoMap;
  Serial.begin(9600);
  s1.attach(13);
  s2.attach(12);
  //pinMode(9, OUTPUT);
  //pinMode(10, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(PIN_BUTTON_RIGHT, INPUT);
  digitalWrite(PIN_BUTTON_RIGHT, HIGH);
  pinMode(PIN_BUTTON_LEFT, INPUT);
  digitalWrite(PIN_BUTTON_LEFT, HIGH);
  pinMode(PIN_BUTTON_UP, INPUT);
  digitalWrite(PIN_BUTTON_UP, HIGH);
  pinMode(PIN_BUTTON_DOWN, INPUT);
  digitalWrite(PIN_BUTTON_DOWN, HIGH);
  map2d[1][4] = 0;
  Serial.println(map2d[1][4]);
}

void loop() {
  switch(currentMode){
    case manual:
      ManualDrive();
      break;
    case autonomous:
      Auto();
      break;
    case autonomousNoMap:
      AutoNoMap();
      break;
  }
}
// Manual
void ManualDrive(){
  // Button press to INCREASE speed
  if (digitalRead(PIN_BUTTON_DOWN) == LOW && !waitForUpButtonUp && upperLimDif < 10){
    upperLimDif += 1;
    waitForUpButtonUp = false;
  }
  if (waitForUpButtonUp && digitalRead(PIN_BUTTON_UP) == HIGH) {
    waitForUpButtonUp = false;
  }

  // Button press to DECREASE speed
  if (digitalRead(PIN_BUTTON_DOWN) == LOW && !waitForDownButtonUp && upperLimDif > 0) {
    upperLimDif -= 1;
    waitForDownButtonUp = true;
  }
  if (waitForDownButtonUp && digitalRead(PIN_BUTTON_DOWN) == HIGH) {
    waitForDownButtonUp = false;
  }

  // Y-axis to directional change per second
  int Y = analogRead(PIN_ANALOG_Y);
  rightWheelSpeed = map(Y, 512, 1023, 90+upperLimDif, 90);
  leftWheelSpeed = map(Y, 512, 0, 90-upperLimDif, 90);

  // Forwards
  int X = analogRead(PIN_ANALOG_X);
  if (X > 510) {
    backwards = false;
    forwards = true;
  }
  // Backwards
  else if (X < 500) {
    forwards = false;
    backwards = true;
  }

  // Echo
  forwardsDistance = GetDistance();

  // Drive
  if (X > 500 && X < 510) {
    s1.write(90);
    s2.write(90);
  }
  else if (forwards && forwardsDistance > 10) {
    s1.write(rightWheelSpeed);
    s2.write(leftWheelSpeed);
  }
  else if (backwards) {
    s1.write(180 - rightWheelSpeed);
    s2.write(180 - leftWheelSpeed);
  }
}

void AutoNoMap(){
  forwardsDistance = GetDistance();
  if (forwardsDistance > 10 && !stopped) {
    s1.write(100);
    s2.write(80);
  }
  else if (!stopped){
    //CheckLeft();
    //CheckRight();
    TurnLeft(checkDelay);
    leftDistance = GetDistance();
    TurnRight(2*checkDelay);
    rightDistance = GetDistance();
    if (rightDistance > leftDistance && rightDistance > 10){
      TurnRight(turnDelay-checkDelay);
      Forwards(2000);
      TurnLeft(turnDelay);
      Forwards(0);
    }
    else if (leftDistance > 10){
      TurnLeft(checkDelay+turnDelay);
      Forwards(2000);
      TurnRight(turnDelay);
      Forwards(0);
    }
    else{
      stopped = true;
    }
  }
  else{
    s1.write(90);
    s2.write(90);
  }
  // Stop
  if (digitalRead(PIN_BUTTON_UP) == LOW && !waitForUpButtonUp) {
    stopped = !stopped;
    waitForUpButtonUp = true;
  }
  if (waitForUpButtonUp && digitalRead(PIN_BUTTON_UP == HIGH)) {
    waitForUpButtonUp = false;
  }
}

void Auto(){

}
// Movement related Functions
void Forwards(int _delay){
  Serial.println(";lkxfngsagoansrg");
  s1.write(100);
  s2.write(80);
  delay(_delay);

}
void TurnLeft(int _delay){
  s1.write(80);
  s2.write(80);
  delay(_delay);
}
void TurnRight(int _delay){
  s1.write(100);
  s2.write(100);
  delay(_delay);
}
void CheckLeft() {
  TurnLeft(checkDelay);
  leftDistance =  GetDistance();
  TurnRight(checkDelay);
}

void CheckRight() {
  TurnRight(checkDelay);
  leftDistance =  GetDistance();
  TurnLeft(checkDelay);
}

int GetDistance(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}