diff --git a/SLAM_ROBOT/Processing/Server.pde b/SLAM_ROBOT/Processing/Server.pde new file mode 100644 index 0000000..514675b --- /dev/null +++ b/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 d = new ArrayDeque(); + + +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); +} diff --git a/SLAM_ROBOT/Processing/udpclient.c b/SLAM_ROBOT/Processing/udpclient.c new file mode 100644 index 0000000..382d679 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#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) + ; +}