diff --git a/esp.ino b/esp.ino
new file mode 100644
index 0000000..57b5922
--- /dev/null
+++ b/esp.ino
@@ -0,0 +1,257 @@
+#include <Wire.h>
+#include <WiFi.h>
+#include <WiFiUdp.h>
+#include <SparkFun_VL53L1X.h>
+
+const char * networkName = "OTEfd57e0";
+const char * networkPswd = "akup8407";
+const char * udpAddress = "192.168.1.2";
+const int udpPort = 6000;
+SFEVL53L1X distanceSensor;
+WiFiUDP udp;
+boolean connected = false;
+
+#define       MOTOR_PHASE_A           26
+#define       MOTOR_PHASE_B           25
+#define       MOTOR_PHASE_C           33
+#define       MOTOR_PHASE_D           32
+#define       HOMING_PIN              35
+unsigned long starttime;
+unsigned long endtime;
+
+int count = 0;
+int flag = 0;
+
+int circle = 2048 * 3.25;
+
+char *buffer1;
+char *buffer2;
+float distance;
+float x,y;
+int angle;
+int max_angle,max_d,f_d;
+int flag_lastTurn[2];
+
+int step();
+
+void setup() {
+  flag_lastTurn[0]=0;
+  flag_lastTurn[1]=1;
+  Wire.begin();
+
+  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
+  {
+    Serial.println("Sensor online!");
+  }
+  //distanceSensor.setDistanceModeShort();//Sets it to max 1.3 works better with diffrent ambient lights
+  distanceSensor.setTimingBudgetInMs(120);
+  distanceSensor.setIntermeasurementPeriod(150);
+  connectToWiFi(networkName, networkPswd);
+   Serial2.begin(9600);
+  Serial.begin(9600);
+  Serial.println("Setup");
+  pinMode(MOTOR_PHASE_A, OUTPUT);
+  pinMode(MOTOR_PHASE_B, OUTPUT);
+  pinMode(MOTOR_PHASE_C, OUTPUT);
+  pinMode(MOTOR_PHASE_D, OUTPUT);
+  pinMode(HOMING_PIN, INPUT);
+  starttime = millis();
+  buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1);
+  buffer2 = (char*) malloc((sizeof(int)+(3*sizeof(float)) ) * sizeof(char) + 1);
+  while (digitalRead(HOMING_PIN) == HIGH)step();
+  while (digitalRead(HOMING_PIN) == LOW) {
+    step();
+  }
+  x=0;
+  y=0;
+}
+
+void loop() {
+   sprintf (buffer1, "%c%lu",'F',1000);
+   Serial2.print(buffer1);
+    max_angle=-1;
+  max_d=-1;
+  f_d=0;
+   //if(count%circle==0)delay(5000);
+   for(count=0;count<circle-1;){
+       count += step();
+
+   if (count % 18 == 0) {
+     delay(150);
+     distanceSensor.startRanging();
+     distanceSensor.stopRanging();
+     while(!distanceSensor.checkForDataReady()){};
+     distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
+     if(distanceSensor.getRangeStatus()==0){
+       if (connected) {
+         //Send a packet
+         udp.beginPacket(udpAddress, udpPort);
+         angle = ceil(count * 0.054086538);
+         sprintf(buffer2, "%d,%f,%f,%f",angle,distance/10,x*10,y*10);
+         udp.printf("%s",buffer2);
+         udp.endPacket();
+       }
+       if(distance>max_d){
+        max_d=distance;
+        max_angle=angle;
+        }
+     }
+     if(count==0){
+      if(distance>18||distanceSensor.getRangeStatus()==4){
+         f_d=1;
+        }
+      }
+     // Serial.print("ANGLE:");
+     // Serial.print(angle);
+     // Serial.print(" DISTANCE:");
+     // Serial.println(distance/10);
+     // Serial.println(buffer2);
+   }
+
+ }
+   count = count % circle-1;
+   if(f_d!=1){
+   		if(max_angle>180){
+   		  sprintf (buffer1, "%c%lu",'L',850);
+       	  Serial2.print(buffer1);
+   		}
+   		else
+   		{
+   		  sprintf (buffer1, "%c%lu",'R',850);
+       	  Serial2.print(buffer1);
+   		}
+   }
+   delay(1000);
+   /*
+   if(f_d!=1){
+    if(max_angle>180){
+       sprintf (buffer1, "%c%lu",'L',850);
+       Serial2.print(buffer1);
+       delay(500);
+       if(flag_lastTurn[0]==1){
+        switch(flag_lastTurn[1]){
+            case 1:
+                  // x-=12.5;
+                    break;
+            case 0:
+                  x+=12.5;
+                    break;
+         }
+        }
+       else
+       //y-=12.5;
+       sprintf (buffer1, "%c%lu",'F',1000);
+       Serial2.print(buffer1);
+      }
+     else{
+       sprintf (buffer1, "%c%lu",'R',850);
+       Serial2.print(buffer1);
+       delay(500);
+              if(flag_lastTurn[0]==1){
+        switch(flag_lastTurn[1]){
+            case 1:
+                   x+=12.5;
+                    break;
+            case 0:
+                 // x-=12.5;
+                    break;
+         }
+        }
+       else
+       y+=12.5;
+      }
+       sprintf (buffer1, "%c%lu",'F',1000);
+       Serial2.print(buffer1);
+   }
+   else{
+       sprintf (buffer1, "%c%lu",'F',1000);
+       Serial2.print(buffer1);
+       x+=12.5;
+       flag_lastTurn[0]=0;
+       flag_lastTurn[1]=0;
+   }*/
+
+}
+
+
+int step() {
+  static int count;
+
+  switch (count) {
+    case 0:
+      {
+        digitalWrite(MOTOR_PHASE_D, HIGH);
+        digitalWrite(MOTOR_PHASE_C, LOW);
+        digitalWrite(MOTOR_PHASE_B, LOW);
+        digitalWrite(MOTOR_PHASE_A, LOW);
+      }
+      break;
+    case 1:
+      {
+        digitalWrite(MOTOR_PHASE_D, LOW);
+        digitalWrite(MOTOR_PHASE_C, HIGH);
+        digitalWrite(MOTOR_PHASE_B, LOW);
+        digitalWrite(MOTOR_PHASE_A, LOW);
+      }
+      break;
+    case 2:
+      {
+        digitalWrite(MOTOR_PHASE_D, LOW);
+        digitalWrite(MOTOR_PHASE_C, LOW);
+        digitalWrite(MOTOR_PHASE_B, HIGH);
+        digitalWrite(MOTOR_PHASE_A, LOW);
+      }
+      break;
+    case 3:
+      {
+        digitalWrite(MOTOR_PHASE_D, LOW);
+        digitalWrite(MOTOR_PHASE_C, LOW);
+        digitalWrite(MOTOR_PHASE_B, LOW);
+        digitalWrite(MOTOR_PHASE_A, HIGH);
+      }
+      break;
+
+
+  }
+  delay(8);
+  count++;
+  count = count % 4;
+
+  return 1;
+}
+
+
+
+void connectToWiFi(const char * ssid, const char * pwd) {
+  Serial.println("Connecting to WiFi network: " + String(ssid));
+
+  // delete old config
+  WiFi.disconnect(true);
+  //register event handler
+  WiFi.onEvent(WiFiEvent);
+
+  //Initiate connection
+  WiFi.begin(ssid, pwd);
+
+  Serial.println("Waiting for WIFI connection...");
+}
+
+//wifi event handler
+void WiFiEvent(WiFiEvent_t event) {
+  switch (event) {
+    case SYSTEM_EVENT_STA_GOT_IP:
+      //When connected set
+      Serial.print("WiFi connected! IP address: ");
+      Serial.println(WiFi.localIP());
+      //initializes the UDP state
+      //This initializes the transfer buffer
+      udp.begin(WiFi.localIP(), udpPort);
+      connected = true;
+      break;
+    case SYSTEM_EVENT_STA_DISCONNECTED:
+      Serial.println("WiFi lost connection");
+      connected = false;
+      break;
+    default: break;
+  }
+}