RumbaMcRumbaface 3.0

int trigPin1 = 11;    // Trigger
int echoPin1 = 12;    // Echo
int trigPin2 = 3;    // Trigger
int echoPin2 = 8;    // Echo
long duration, cm;
long beepTimer;
int shortBeepTime = 30;
int beepPause = 120;
bool beeperIsBeepin;
bool ArduinoInStandBy;
bool firstBoot = true;
int CurrentSpeed = 255;
int LeftRight;
int theDistance;
float changeDirectionDistance = 30;
bool isTurning = false;
float turningTime = 330;
long turningTimer;
float pauseBeforeStateChange = 130;
int forwardSpeed;



void setup() {
  //Serial Port begin
  Serial.begin (9600);
  //Inputs and outputs for Distance Meter
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  //Motor Pins

  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);

  //PWM
  //LEFT WHEEL
  pinMode(9, OUTPUT);
  //RIGHT WHEEL
  pinMode(13, OUTPUT);

  //beeper Pin
  pinMode(2, OUTPUT);

}

void setAllToLOW()  //Set all motor pins to LOW just in case
{
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, LOW);

  CurrentSpeed = 255;
}

void shortBeep()
{
  if (beeperIsBeepin == false)
    {
      beeperIsBeepin = true;
      beepTimer = millis();
      digitalWrite(2, HIGH);
    }

  if ((millis() >= beepTimer + shortBeepTime) && (beeperIsBeepin = true) )
    {
      digitalWrite(2, LOW);
    }
  if ((millis() >= beepTimer + shortBeepTime + beepPause) && (beeperIsBeepin = true))
    {
          beeperIsBeepin = false;
    }

}

float getDistance1() //right ultraconic distance sensor
{
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(13);
  digitalWrite(trigPin1, LOW);

  pinMode(echoPin1, INPUT);
  duration = pulseIn(echoPin1, HIGH, 13000);

  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343

  if (cm == 0)
    {
      return 666;
    }
  else
    {
      return cm;
    }
}

float getDistance2() //left ultraconic distance sensor
{
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(13);
  digitalWrite(trigPin2, LOW);

  pinMode(echoPin2, INPUT);
  duration = pulseIn(echoPin2, HIGH, 13000);

  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343


  if (cm == 0)
    {
      return 666;
    }
  else
    {
      return cm;
    }

}

float getDistance() // take the lowest mesured distance and run with it
{
  float Distance1;
  float Distance2;
  float Distance3;
  float Distance4;
  float firstWinner;
  float secondWinner;
  float finalWinner;
  float AllMeasurments[] = {Distance1, Distance2, Distance3, Distance4};

  Distance1 = getDistance1();
  Distance2 = getDistance2();
  Distance3 = getDistance1();
  Distance4 = getDistance2();

  AllMeasurments[0] = Distance1;
  AllMeasurments[1] = Distance2;
  AllMeasurments[2] = Distance3;
  AllMeasurments[3] = Distance4;

  firstWinner = min(Distance1, Distance2);
  secondWinner = min(Distance3, Distance4);
  finalWinner = min(firstWinner, secondWinner);


  //Serial.println(finalWinner);
  beepPause = map(finalWinner, 30, 300, 1, 2000); //the closer to obstacle the faster it beeps
  return finalWinner; //temp


}


void leftWhellBack()
{
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
}

void leftWhellFrw()
{
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
}

void rightWhellFrw()
{
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
}

void rightWhellBack()
{
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
}

void SoftStart(int desiredSpeed)
{
  int speed_substraction;

  speed_substraction = map(CurrentSpeed, 69, 254, 10, 34);



  if (CurrentSpeed == 255)
    {
      CurrentSpeed = 70;
    }
  if (CurrentSpeed < desiredSpeed)
    {
      ++CurrentSpeed;
    }
  else if (CurrentSpeed > desiredSpeed)
    {
      --CurrentSpeed;
    }

  analogWrite(9, CurrentSpeed-speed_substraction);
  analogWrite(13, CurrentSpeed);


}

void SoftTurnStart(int desiredSpeed)
{

  if (CurrentSpeed == 255)
    {
      CurrentSpeed = 90;
    }
  if (CurrentSpeed < desiredSpeed)
    {
      ++CurrentSpeed;
    }


  analogWrite(9, CurrentSpeed);
  analogWrite(13, CurrentSpeed);


}

void DriveForward()
{

  float scaledDistance = constrain(theDistance, changeDirectionDistance, 120);
  forwardSpeed = map(scaledDistance, changeDirectionDistance-2, 120, 120, 254); //the closer to obstacle the slower it goes
  SoftStart(forwardSpeed);
  rightWhellFrw();
  leftWhellFrw();
}

void DriveBack()
{
  SoftStart(150);
  rightWhellBack();
  leftWhellBack();
}

void TurnLeft()
{
  SoftTurnStart(165);
  rightWhellFrw();
  leftWhellBack();
}

void TurnRight()
{
  SoftTurnStart(165);
  rightWhellBack();
  leftWhellFrw();
}

void FirtBoot()
{
  if (firstBoot == true)
    {
      firstBoot = false;
      delay(50);
      digitalWrite(2, HIGH);
      delay(200);
      digitalWrite(2, LOW);
      delay(2000);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(2000);
      setAllToLOW();
    }

}


//Main_logic

void loop()
{

  FirtBoot();
  theDistance = getDistance();
  shortBeep();
  if ((theDistance <= changeDirectionDistance) && (isTurning == false))
    {
      setAllToLOW();
      LeftRight = random(1, 3);
      turningTimer = millis();
      isTurning = true;
    }

  if (isTurning == false)
    {
      DriveForward();
    }
  if (isTurning == true)
    {
      if (LeftRight == 1)
        {
          TurnLeft();
        }
       else
        {
          TurnRight();
        }
      if (millis() >= turningTimer + turningTime)
        {
          setAllToLOW();
          isTurning = false;
        }
    }

  Serial.println(forwardSpeed);

}