Upload files to 'SLAM_ROBOT/Processing'

SLAM_ROBOT/Processing/Server.pde

@@ -0,0 +1,107 @@
+
+// import UDP library
+import hypermedia.net.*;
+import java.util.*;
+
+// Queue that holds the messages received from Esp32
+Deque<String> d = new ArrayDeque<String>();
+
+
+float x,y,angle;
+float num; //angle send by esp32
+float num2;//distance send by esp32
+int counter; //index for json array
+JSONArray dataArray; // json array holding all data received
+JSONObject measurements; // json object a pair of measurments & coordinates
+JSONObject sensor_data;
+JSONObject coordinates;
+float x_1,y_1; // x,y received from robot
+UDP udp;  // define the UDP object
+
+
+void setup() {
+  size(820, 820); //size of graph
+  noSmooth();
+  background(0);
+  translate(410, 410); // move start draw point to center
+  stroke(255);
+  strokeWeight(3);
+  dataArray=new JSONArray();
+  counter=0;
+  // create a new datagram connection on port 6000
+  // and wait for incomming message
+  udp = new UDP( this, 6000 );
+  //udp.log( true );     // <-- printout the connection activity
+  udp.listen( true );
+}
+
+void savedata(){
+   saveJSONArray(dataArray,"new.json"); //save JSONArray to file
+}
+//process events
+void draw() {
+//check if queue is empty
+if (!d.isEmpty()) {
+//split into substrings using , as delimiter
+ String[] q = splitTokens(d.pop(), ",");
+           measurements = new JSONObject();
+           sensor_data = new JSONObject();
+           coordinates = new JSONObject();
+           if(q[0].equals("end")){
+              //"end" message means no more measurements coming so we save the array
+              println("yoo "+dataArray.size());
+              savedata();
+             return;
+           }
+
+           num=float(q[0]);  // Angle read from Esp32
+           num2 = float(q[1]);  // Distance read from Esp32
+           x_1=int(q[2]); // X coordinate from Esp32
+           y_1=int(q[3]); // Y coordinate from esp32
+
+           sensor_data.setFloat("Angle",num);
+           sensor_data.setFloat("Distance",num2);
+           coordinates.setFloat("X",x_1);
+           coordinates.setFloat("Y",y_1);
+           measurements.setJSONObject("Sensor",sensor_data);
+           measurements.setJSONObject("Location",coordinates);
+           dataArray.setJSONObject(counter++,measurements);
+
+    //Pass from polars to cartesians adna add 410 to be in the middle of the 820 by 820 screen.
+    angle = num * 0.0174533;
+    x = (sin(angle)*num2 + 410);
+    y = (cos(angle)*num2 + 410);
+    //something like the commands below should be used to move the
+    // drawing point int the graph based on receuved coordinates
+    //x = x_1 +( cos( angle ) * num2+130);
+    //y = y_1 +(sin( angle ) * num2+130);
+
+
+    }
+    //this clears the graph and re-centers it
+    //comment this out if you're going to have multiple position measurments
+    //in your graph
+    if(num == 0 )
+    {
+      background(0);
+      translate(410, 410);
+    }
+    point(x, y);
+
+}
+
+
+/**
+ * To perform any action on datagram reception, you need to implement this
+ * handler in your code. This method will be automatically called by the UDP
+ * object each time he receive a nonnull message.
+ * By default, this method have just one argument (the received message as
+ * byte[] array), but in addition, two arguments (representing in order the
+ * sender IP address and his port) can be set like below.
+ */
+// void receive( byte[] data ) {       // <-- default handler
+void receive( byte[] data, String ip, int port ) {  // <-- extended handler
+  data = subset(data, 0, data.length);
+  String message = new String( data );
+  d.add(message);
+}

SLAM_ROBOT/Processing/udpclient.c

@@ -0,0 +1,84 @@
+// Client side implementation of UDP client-server model
+//this is a dummy test server prentending to be the esp32 and sending
+//data on two diffrent postions  (it's supposed to make processign draw 2
+//circles on diffrent positions) .
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <time.h>
+#define PORT	 6000
+#define MAXLINE 1024
+void delay(int number_of_seconds) ;
+// Driver code
+int main() {
+	int sockfd,i;
+	char buffer[MAXLINE];
+	char *hello = "Hello from client";
+	struct sockaddr_in	 servaddr;
+
+	// Creating socket file descriptor
+	if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
+		perror("socket creation failed");
+		exit(EXIT_FAILURE);
+	}
+
+	memset(&servaddr, 0, sizeof(servaddr));
+
+	// Filling server information
+	servaddr.sin_family = AF_INET;
+	servaddr.sin_port = htons(PORT);
+	servaddr.sin_addr.s_addr =inet_addr("127.0.0.1");
+
+	int n, len;
+	int a,b;
+	char buffer2[(sizeof(int)+3*sizeof(float))+1];
+	for(i=1;i<=360;i++){
+		a=i;
+		b=i;
+		sprintf(buffer2, "%d,%d,%d,%d", i,30,5,5);
+		sendto(sockfd,buffer2, strlen(buffer2),
+			MSG_CONFIRM, (const struct sockaddr *) &servaddr,
+				sizeof(servaddr));
+				delay(100);
+
+	}
+
+	for(i=1;i<=360;i++){
+		a=i;
+		b=i;
+		sprintf(buffer2, "%d,%d,%d,%d", i,130,100,250);
+		sendto(sockfd,buffer2, strlen(buffer2),
+			MSG_CONFIRM, (const struct sockaddr *) &servaddr,
+				sizeof(servaddr));
+				delay(200);
+
+	}
+	sprintf(buffer2, "end");
+	sendto(sockfd,buffer2, strlen(buffer2),
+		MSG_CONFIRM, (const struct sockaddr *) &servaddr,
+			sizeof(servaddr));
+
+
+
+	close(sockfd);
+	return 0;
+}
+
+void delay(int number_of_seconds)
+{
+    // Converting time into milli_seconds
+    int milli_seconds = number_of_seconds;
+
+    // Storing start time
+    clock_t start_time = clock();
+
+    // looping till required time is not achieved
+    while (clock() < start_time + milli_seconds)
+        ;
+}