From 680cedc435a0f0c19207a712f3ed09a16990dee5 Mon Sep 17 00:00:00 2001 From: Gab_S Date: Sat, 25 Jan 2020 18:54:06 +0000 Subject: [PATCH] Upload files to 'SLAM_ROBOT/ESP' --- SLAM_ROBOT/ESP/esp.ino | 259 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 259 insertions(+) create mode 100644 SLAM_ROBOT/ESP/esp.ino diff --git a/SLAM_ROBOT/ESP/esp.ino b/SLAM_ROBOT/ESP/esp.ino new file mode 100644 index 0000000..396aece --- /dev/null +++ b/SLAM_ROBOT/ESP/esp.ino @@ -0,0 +1,259 @@ +#include // Used to speak with the VL53L1X through I2C +#include +#include +#include // 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