diff --git a/Mapping/Esp32/esp.ino b/Mapping/Esp32/esp.ino
new file mode 100644
index 0000000..f27d8ff
--- /dev/null
+++ b/Mapping/Esp32/esp.ino
@@ -0,0 +1,208 @@
+#include <Wire.h>
+#include <WiFi.h>
+#include <WiFiUdp.h>
+#include <SparkFun_VL53L1X.h>
+
+const char * networkName = "SSID";
+const char * networkPswd = "PASS";
+const char * udpAddress = "192.168.1.76";
+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;
+float  rotationtime;
+int count = 0;
+int flag = 0;
+
+int circle = 2048 * 3.25;
+
+char *buffer1;
+char buffer2[20];
+int distance;
+int angle;
+
+int step();
+
+void setup() {
+  Wire.begin();
+  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
+  {
+    Serial.println("Sensor online!");
+  }
+
+  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);
+  while (digitalRead(HOMING_PIN) == HIGH)step();
+  while (digitalRead(HOMING_PIN) == LOW) {
+    step();
+  }
+  delay(2000);
+}
+
+void loop() {
+  //if(count%circle==0)delay(5000);
+  for(count=0;count<circle-1;){
+      count += step();
+
+  if (count % 18 == 0) {
+    distanceSensor.startRanging();
+    distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
+    distanceSensor.stopRanging();
+    if (connected) {
+      //Send a packet
+      udp.beginPacket(udpAddress, udpPort);
+      angle = ceil(count * 0.054086538);
+      sprintf(buffer2, "%d,%f",angle,(float)distance);
+      udp.printf("%s",buffer2);
+      udp.endPacket();
+    }
+    Serial.print("ANGLE:");
+    Serial.print(angle);
+    Serial.print(" DISTANCE:");
+    Serial.println(distance/10);
+    Serial.println(buffer2);
+  }
+
+}
+
+  //  if(digitalRead(HOMING_PIN)==LOW)
+  //        Serial.println("LOW");
+  //  else
+  //  Serial.println("HIGH");
+  count = count % circle-1;
+
+
+     sprintf (buffer1, "%c%lu",'F',2500);
+     Serial2.print(buffer1);
+     delay(5000);
+
+  /*
+      sprintf (buffer1, "%c%lu",'L',200);
+    Serial2.print(buffer1);
+    delay(2000);
+
+      sprintf (buffer1, "%c%lu",'F',200);
+    Serial2.print(buffer1);
+      delay(2000);
+      sprintf (buffer1, "%c%lu",'R',200);
+    Serial2.print(buffer1);
+       delay(2000);*/
+
+
+  /*
+
+
+      if(count==2048*3.25){
+        endtime=millis();
+        rotationtime=(endtime-starttime)/1000;
+        if(flag==0){
+              Serial.print("It took:");
+        Serial.print(rotationtime);
+        Serial.println("seconds");
+        flag++;
+        }
+      }
+      count=count%2048*3.25; */
+
+
+}
+
+
+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;
+  }
+}