Upload files to 'SLAM_ROBOT/ESP'

SLAM_ROBOT/ESP/esp.ino

@@ -0,0 +1,259 @@
+#include <Wire.h> // Used to speak with the VL53L1X through I2C
+#include <WiFi.h>
+#include <WiFiUdp.h>
+#include <SparkFun_VL53L1X.h> // Measuring senssor's lib
+
+const char * networkName = "YOUR_WIFI_SSID";
+const char * networkPswd = "WIFI_PASS";
+const char * udpAddress = "SERVER_IP";
+const int udpPort = 6000; //SERVER LISTENING PORT
+
+SFEVL53L1X distanceSensor; // creating object of sensor
+WiFiUDP udp;
+boolean connected = false;
+
+#define       MOTOR_PHASE_A           26  //SERVO MOTOR PIN1
+#define       MOTOR_PHASE_B           25  //SERVO MOTOR PIN2
+#define       MOTOR_PHASE_C           33  //SERVO MOTOR PIN3
+#define       MOTOR_PHASE_D           32  //SERVO MOTOR PIN4
+#define       HOMING_PIN              35  // HALL SENSOR INPUT PIN
+
+//counting how many steps the servo has made zeros after full 360 measuring
+//circle
+int count = 0;
+
+//it takes 2048 for the stepper motor to rotate once and 3.25 turns of
+//the stepper motor to rotate the top platform once,thus taking the 360 measurments
+int circle = 2048 * 3.25;
+
+char *buffer1;//Buffer used to talk with Arduino
+char *buffer2;//Buffer used to send data to udp
+float distance;// distance measured
+int angle;// angle of measured distance
+float x,y; // the coordinates of the robot
+ // a function created to implement FULL STEPPING in a stepper motor
+ // you can implement your own version based on the stepper you have
+ // maybe even try HALF STEPPING or get a ready made library
+int step();
+
+
+//BootStraping the system
+void setup() {
+  Wire.begin();//Begin I2C so we can talk with the measuring sensor
+
+  //Bringing the sensor online
+  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
+  {
+    Serial.println("Sensor online!");
+  }
+  //Sets it to max 1.3m works better with diffrent ambient lights
+  //if you comment this out the sensor will work on 4m but won't be as accurate
+  distanceSensor.setDistanceModeShort();
+  //Default is 100 ,the bigger the number you set in the budget the more accurate
+  //readings you'll get at the cost of time per measurment IntermeasurementPeriod
+  //must always be greater or equal than budget,accepted budget values are:
+  /*
+    15
+    20
+    33
+    50
+    100 (default)
+    200
+    500
+    15 ms only works with Short distance mode. 100 ms is the default value
+    Usefull websites:
+      https://www.st.com/content/ccc/resource/technical/document/user_manual/group1/63/e0/7d/f1/0e/52/4d/cd/DM00562924/files/DM00562924.pdf/jcr:content/translations/en.DM00562924.pdf
+      https://learn.sparkfun.com/tutorials/qwiic-distance-sensor-vl53l1x-hookup-guide/all
+ */
+  distanceSensor.setTimingBudgetInMs(100);
+  distanceSensor.setIntermeasurementPeriod(100);
+  //connecting to wifi
+  connectToWiFi(networkName, networkPswd);
+  //Serial2 is used for talkign with Arduino make sure to set the baud rate
+  //(9600) the same on both devices.
+  Serial2.begin(9600);
+  //Serial to print through USB for debugging purposes while you develop
+  Serial.begin(9600);
+  Serial.println("Setup");
+  //Setting output pins for stepper motor
+  pinMode(MOTOR_PHASE_A, OUTPUT);
+  pinMode(MOTOR_PHASE_B, OUTPUT);
+  pinMode(MOTOR_PHASE_C, OUTPUT);
+  pinMode(MOTOR_PHASE_D, OUTPUT);
+  //Setting input pin for HALL SENSOR
+  pinMode(HOMING_PIN, INPUT);
+  //Setting the size of buffer that will talk with the arduino
+  //Since the messages Sent to arduino are of the type :
+  //"L,100" - Rotates left for 100 ms
+  //"F,1000" - Moves Forward for 1 sec
+  // "R,500" -rotates right for 500 ms
+  //We need the size of the character L,R,F + size of variable for time
+  //multiplying by char (not neccesery) + 1 for terminating character
+  buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1);
+  //Same here for talking with the udp server but this time the messages are
+  //of the type:
+  //"1,14.5,0,5" meaning :
+  //Angle=1 degree
+  //distance measured=14.5 cm
+  //x coordinate=0; in cm
+  //y coordinate=5  in cm
+  buffer2 = (char*) malloc((sizeof(int)+(3*sizeof(float)) ) * sizeof(char) + 1);
+  //For the particular Hall Sensor and magnet setup this two while loops are
+  //used in order to bring the top platform in the 0 degrees position (homing
+  //procedure) from any position it is. You might find with your Hall senosor
+  // and magnet setup that you need to change from HIGH to LOW  etc. it is
+  //suggested that you experiment and use printfs to see how your sensor and
+  //magnet combo behaves so you can create your homing procedure
+  //the Sensor outputs HIGH when it meets the magnet at 0 degrees but
+  //i noticed that sometimes when the system truns on the sensor would be
+  //reading HIGH meaning it was latched on high so the first loop turns until
+  //the sensor unlatches and then till it latches again at 0 position this time
+  //meaing at worst case might take 2 turns for the homing procedure to end.
+  //The hall sensor used is : US1881
+  while (digitalRead(HOMING_PIN) == HIGH)step();
+  while (digitalRead(HOMING_PIN) == LOW) {
+    step();
+  }
+  //Since it's a Dead reckoning  system we start at 0,0
+  x=0;
+  y=0;
+}
+
+void loop() {
+   //Do one full circle of the top platform taking measurments.
+   for(count=0;count<circle-1;){
+
+  // every 18 steps is almost a degree (360/2048 * 3.25)*18=0.973557692 degrees
+   if (count % 18 == 0) {
+     delay(50); //precautionary delay so that IntermeasurementPeriod is met
+     distanceSensor.startRanging();
+     distanceSensor.stopRanging();
+     //while loop waiting until ranging data is ready
+     while(!distanceSensor.checkForDataReady()){};
+     //Get the result of the measurement from the sensor
+     distance = distanceSensor.getDistance();
+     //if the ranging result is good Status is 0 if it's out of range
+     //(above 1.3m for short or 4m for long ) it returns 4 check links above
+     //for more info or look for the st's manual guide to VL53L1X
+     if(distanceSensor.getRangeStatus()==0){
+       //check connection to wifi
+       if (connected) {
+        //Send a packet
+         udp.beginPacket(udpAddress, udpPort);
+        // every steps is (360/2048 * 3.25)=0.054086538 degrees
+         angle = ceil(count * 0.054086538);
+         //creating the message to send to server distance/10
+         //to go from millis to cm
+         sprintf(buffer2, "%d,%f,%f,%f",angle,distance/10,x,y);
+         udp.printf("%s",buffer2);
+         udp.endPacket();
+       }
+     }
+     // Serial.print("ANGLE:");
+     // Serial.print(angle);
+     // Serial.print(" DISTANCE:");
+     // Serial.println(distance/10);
+     // Serial.println(buffer2);
+   }
+   //the stepping function returns 1  so the counter is increased after stepping
+     count += step();
+ }
+   //Zero count since a full circle has been made
+   count = count % circle-1;
+
+   /* implement your Algorithm for mapping  here
+      In our case moving the motors forward for 1 sec tended to move the
+      robot for a distance of 12 cm  and for a 90 degree rotation it took
+      850 milliseconds.So for example sending these two messages at arduino
+      "L,850","F,1000" will move the robot left for 12 cm. so given the
+      commands above on how to move the arduino and a way to get distances of
+      each angle you can make an algorithm that makes the robot move in a way
+      you want base on furthest distance or what ever you see fit.
+
+
+   */
+
+}
+
+
+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;
+  }
+}