MIngi arduino pask

#include <StackArray.h>

#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>

#include <MeAuriga.h>

//Encoder Motor
MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);

void isr_process_encoder1(void)
{
      if(digitalRead(Encoder_1.getPortB()) == 0){
            Encoder_1.pulsePosMinus();
      }else{
            Encoder_1.pulsePosPlus();
      }
}

void isr_process_encoder2(void)
{
      if(digitalRead(Encoder_2.getPortB()) == 0){
            Encoder_2.pulsePosMinus();
      }else{
            Encoder_2.pulsePosPlus();
      }
}

void move(int direction, int speed)
{
      int leftSpeed = 0;
      int rightSpeed = 0;
      if(direction == 1){
            leftSpeed = -speed;
            rightSpeed = speed;
      }else if(direction == 2){
            leftSpeed = speed;
            rightSpeed = -speed;
      }else if(direction == 3){
            leftSpeed = -speed;
            rightSpeed = -speed;
      }else if(direction == 4){
            leftSpeed = speed;
            rightSpeed = speed;
      }
      Encoder_1.setTarPWM(leftSpeed);
      Encoder_2.setTarPWM(rightSpeed);
}
void moveDegrees(int direction,long degrees, int speed_temp)
{
      speed_temp = abs(speed_temp);
      if(direction == 1)
      {
            Encoder_1.move(-degrees,(float)speed_temp);
            Encoder_2.move(degrees,(float)speed_temp);
      }
      else if(direction == 2)
      {
            Encoder_1.move(degrees,(float)speed_temp);
            Encoder_2.move(-degrees,(float)speed_temp);
      }
      else if(direction == 3)
      {
            Encoder_1.move(-degrees,(float)speed_temp);
            Encoder_2.move(-degrees,(float)speed_temp);
      }
      else if(direction == 4)
      {
            Encoder_1.move(degrees,(float)speed_temp);
            Encoder_2.move(degrees,(float)speed_temp);
      }
}

double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;
void turnRight();
double dir;
MeGyro gyro_0(0, 0x69);
void turnLeft();
void checkSIdes();
MeUltrasonicSensor ultrasonic_10(10);


void turnRight()
{
    if(((dir)==(0))){
        while(!(((gyro_0.getAngle(3)) < (95)) && ((85) < (gyro_0.getAngle(3)))))
        {
            _loop();
            move(4,125);
        }
        dir = 1;
    }else{
        if(((dir)==(1))){
            while(!(((gyro_0.getAngle(3)) < (-175)) || ((175) < (gyro_0.getAngle(3)))))
            {
                _loop();
                move(4,125);
            }
            dir = 2;
        }else{
            if(((dir)==(2))){
                while(!(((gyro_0.getAngle(3)) < (-85)) && ((-95) < (gyro_0.getAngle(3)))))
                {
                    _loop();
                    move(4,125);
                }
                dir = 3;
            }else{
                while(!(((gyro_0.getAngle(3)) < (5)) && ((-5) < (gyro_0.getAngle(3)))))
                {
                    _loop();
                    move(4,125);
                }
                dir = 0;
            }
        }
    }

}

void turnLeft()
{
    if(((dir)==(0))){
        while(!(((gyro_0.getAngle(3)) < (-65)) && ((-95) < (gyro_0.getAngle(3)))))
        {
            _loop();
            move(3,100);
        }
        dir = 3;
    }else{
        if(((dir)==(1))){
            while(!(((gyro_0.getAngle(3)) < (15)) && ((-5) < (gyro_0.getAngle(3)))))
            {
                _loop();
                move(3,100);
            }
            dir = 0;
        }else{
            if(((dir)==(3))){
                while(!(((gyro_0.getAngle(3)) < (-165)) || ((170) < (gyro_0.getAngle(3)))))
                {
                    _loop();
                    move(3,100);
                }
                dir = 2;
            }else{
                while(!(((gyro_0.getAngle(3)) < (105)) && ((80) < (gyro_0.getAngle(3)))))
                {
                    _loop();
                    move(3,100);
                }
                dir = 1;
            }
        }
    }

}

void checkSIdes()
{
    turnRight();

    if((ultrasonic_10.distanceCm()) < (15)){
        turnLeft();
        turnLeft();
    }

}


void setup(){
    gyro_0.begin();
    attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
    attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
    //Set Pwm 8KHz
    TCCR1A = _BV(WGM10);
    TCCR1B = _BV(CS11) | _BV(WGM12);
    TCCR2A = _BV(WGM21) | _BV(WGM20);
    TCCR2B = _BV(CS21);

     dir = 0;

}

void loop(){

    if((ultrasonic_10.distanceCm()) < (20)){
        checkSIdes();
    }else{
        move(1,100);
    }

    _loop();
}

void _delay(float seconds){
    long endTime = millis() + seconds * 1000;
    while(millis() < endTime)_loop();
}

void _loop(){
    gyro_0.update();

    Encoder_1.loop();
    Encoder_2.loop();


}