arduino sensormaze

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

#include <Servo.h>

double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;
double sudut;
double servopin;
double sensor1_echopin;
double sensor1_triggerpin;
double sensor1;
Servo servo_servopin;
float getDistance(int trig,int echo){
    pinMode(trig,OUTPUT);
    digitalWrite(trig,LOW);
    delayMicroseconds(2);
    digitalWrite(trig,HIGH);
    delayMicroseconds(10);
    digitalWrite(trig,LOW);
    pinMode(echo, INPUT);
    return pulseIn(echo,HIGH,30000)/58.0;
}



void setup(){
    sudut = 0;
    servopin = A5;
    sensor1_echopin = A0;
    sensor1_triggerpin = A1;
    servo_servopin.attach(servopin); // init pin
    servo_servopin.write(sudut); // write to servo
    Serial.begin(9600);
}

void loop(){

    while(!(((sudut)==(180))))
    {
        _loop();
        servo_servopin.write(sudut); // write to servo
        sudut = (sudut) + (1);
        sensor1 = getDistance(sensor1_triggerpin,sensor1_echopin);
        Serial.write(sensor1);
    }
    while(!(((sudut)==(0))))
    {
        _loop();
        servo_servopin.write(sudut); // write to servo
        sudut = (sudut) - (1);
        sensor1 = getDistance(sensor1_triggerpin,sensor1_echopin);
        Serial.write(sensor1);
    }

    _loop();
}

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

void _loop(){

}