C Example

Modified on Thu, 24 Sep 2020 at 01:44 PM

The following example is a simple implementation of TCP/IP socket communication with the Meca500 robot in TCP/IP on Windows system


socket_comm.h/.c contains all the socket/TCP-IP/Communication part of this example


socket_comm.h

#include <winsock2.h>
#include <windows.h>
#include <ws2tcpip.h>

// For now keep only socket inside, but could add more stuff
// when the program will require more functionnality.
typedef struct{
    SOCKET socket_;
}tcpip_connection;

/// Connect a TCP socket. Handles the windows init socket
/// Return 0 for success, -1 for failure.
int tcpip_connect(const char *address, const char *port, tcpip_connection * connection);

/// Disconnect a connection
void tcpip_disconnect(tcpip_connection *connection);

/// Send data to the buffer
/// Return 0 for success, -1 for failure, or the size that was actually sent
/// if the entire data did not fit in the TCP buffer. In that case, you should 
/// send the remaining. 
int tcpip_send(const char* buffer, int len, tcpip_connection *connection);

/// Try to recveive len byte of data.
/// Return 0 for CLOSED CONNECTION, -1 for failure, or the nb of data received. 
int tcpip_receive(char* buffer, int len, tcpip_connection *connection);


socket_comm.c

#include <stdio.h>
#include <stdlib.h>
#include "socket_comm.h"

#define DEBUG 0

// Keep count of the user to init/close the library.
static int socket_system_user_count = 0;
// the WSData required by windows sockets.
static WSADATA wsaData;

/// Utility to exit the function if the socket system is not initialized.
#define EXIT_ON_SYSTEM_NOT_INITED() if(socket_system_user_count == 0) {return -1;}

/// Initialise the socket library if needed.
int init_socket_system()
{
    if (socket_system_user_count == 0)
    {
        printf("Init WSA\r\n");
        int result = WSAStartup(MAKEWORD(2, 2), &wsaData);
        if (result != 0)
        {
            printf("WSAStartup failed with error: %d\r\n", result);
            return -1;
        }
    }
    socket_system_user_count++;
    return 0;
}

/// Close the socket library if no more user on it.
void cleanup_socket_system()
{
    socket_system_user_count--;
    if (socket_system_user_count == 0)
    {
        printf("Closing WSA\r\n");
        WSACleanup();
    }
}

//=============================================================================
//
int tcpip_connect(const char *address, const char *port, tcpip_connection *connection)
{
    int result = 0;
    result = init_socket_system();
    if (result != 0)
    {
        return -1;
    }

    struct addrinfo hints, *addr_result;
    ZeroMemory(&hints, sizeof(hints));
    hints.ai_family = AF_UNSPEC;
    hints.ai_socktype = SOCK_STREAM;
    hints.ai_protocol = IPPROTO_TCP;
    // Get the address information for the connection
    result = getaddrinfo(address, port, &hints, &addr_result);

    if (result != 0)
    {
        printf("getaddrinfo failed with error: %d\r\n", result);
        cleanup_socket_system();
        return -1;
    }

    // Create a SOCKET for connecting to server
    connection->socket_ = socket(addr_result->ai_family,
                                 addr_result->ai_socktype,
                                 addr_result->ai_protocol);
    if (connection->socket_ == INVALID_SOCKET)
    {
        printf("socket failed with error: %ld\r\n", WSAGetLastError());
        cleanup_socket_system();
        return -1;
    }

    // Connect to server.
    result = connect(connection->socket_,
                     addr_result->ai_addr,
                     (int)addr_result->ai_addrlen);
    if (result == SOCKET_ERROR)
    {
        closesocket(connection->socket_);
        cleanup_socket_system();
        connection->socket_ = INVALID_SOCKET;
        return -1;
    }

    // Done with socket creation.
    freeaddrinfo(addr_result);

    return 0;
}

//=============================================================================
//
void tcpip_disconnect(tcpip_connection *connection)
{
    if (socket_system_user_count == 0)
    {
        return;
    }
    closesocket(connection->socket_);
    cleanup_socket_system();
}

//=============================================================================
//
int tcpip_send(const char *buffer, int len, tcpip_connection *connection)
{
    EXIT_ON_SYSTEM_NOT_INITED();
// Send an initial buffer
#ifdef DEBUG
    printf("Sending: %s\r\n", buffer);
#endif

    int send_result = send(connection->socket_, buffer, len, 0);
    if (send_result == SOCKET_ERROR)
    {
        printf("send failed with error: %d\r\n", WSAGetLastError());
        closesocket(connection->socket_);
        cleanup_socket_system();
        return -1;
    }

    // Did not send everything, return the amount of data that was sent.
    if (send_result != len)
    {
        return send_result;
    }

    // everything was sent and
    return 0;
}

//=============================================================================
//
int tcpip_receive(char *buffer, int len, tcpip_connection *connection)
{
    EXIT_ON_SYSTEM_NOT_INITED();

    int recv_result = recv(connection->socket_, buffer, len, 0);
    if (recv_result == 0) // Closing connection case
    {
        printf("Connection closed\r\n");
        closesocket(connection->socket_);
        cleanup_socket_system();
        return 0;
    }
    else if (recv_result < 0) // Error case
    {
        printf("recv failed with error: %d\r\n", WSAGetLastError());
        return -1;
    }
#ifdef DEBUG
    printf("Receiving: %s\r\n", buffer);
#endif

    // contains the amout of data received.
    return recv_result;
}


robot_interface.h/.c contains the code that handles sending the robot the correct command

and parsing the return messages

robot_interface.h

#include "socket_comm.h"

// Contain the information about a robot's connection.
// Members should not be access individually
typedef struct 
{
    tcpip_connection robot_connection_;
    int connected_;
}Meca500;

/// Connect to a meca500 to the control port
int connect_robot(char* address, Meca500 *robot);

/// Disconnect a meca500
int disconnect_robot(Meca500 *robot);

/// Wait for a specified return code.
/// Return 0 if code was received.
/// Return 1 if an error code ( between 1000 and 2000) was received
/// Return -1 if there was a network error.
int wait_for_return_code(int code, Meca500 *robot);

/// Wait for End of Block flag.
/// Return 0 if code was received.
/// Return 1 if an error code ( between 1000 and 2000) was received
/// Return -1 if there was a network error.
int wait_for_EOB(Meca500 *robot);

/// Send an ActivateRobot command and wait for it to be completed.
/// return 0 for success
/// return -1 for failure
int activate_robot(Meca500 *robot);

/// Send an DeactivateRobot command and wait for it to be completed.
/// return 0 for success
/// return -1 for failure
int deactivate_robot(Meca500 *robot);

/// Send an activatesim command and wait for it to be completed.
/// return 0 for success
/// return -1 for failure
int activate_sim(Meca500 *robot);

/// Send an deactivatesim command and wait for it to be completed.
/// return 0 for success
/// return -1 for failure
int deactivate_sim(Meca500 *robot);

/// Send an home command and wait for it to be completed.
/// return 0 for success
/// return -1 for failure
int home(Meca500 *robot);

/// Sends a MoveJoint command. Do not wait for completion.
/// return 0 for success
/// return -1 for failure
int movejoints(Meca500 *robot, float joints[6]);

/// Sends a MovePose command. Do not wait for completion.
/// return 0 for success
/// return -1 for failure
int movepose(Meca500 *robot, float euler[6]);

/// Sends a MoveJoint command to go to zero. Do not wait for completion.
/// return 0 for success
/// return -1 for failure
int movetozero(Meca500 *robot);

/// Sends a MoveJoint command for the shipping position. Do not wait for completion.
/// return 0 for success
/// return -1 for failure
int movetoshipping(Meca500 *robot);


robot_interface.c

#include "robot_interface.h"

#include "stdio.h"
#include "stdlib.h"
#include "string.h"

/// The control port for the meca500
static const char *CONTROL_PORT = "10000";

/// Utility to exit the function if the robot is not connected
#define EXIT_ON_ROBOT_NOT_CONNECTED(connected_flag) \
    if (connected_flag == 0)                        \
    {                                               \
        printf("Robot not connected.\r\n");         \
        return -1;                                  \
    }

//=============================================================================
//
/// Extract the code ("[3000][..." -> 3000) from a message and return it as int.
/// if not found, return 0;
int extract_code(const char *message)
{
    int code = 0;
    char *first_bracket = NULL;
    char *second_bracket = NULL;
    do
    {
        first_bracket = strchr(message, '[');
        second_bracket = strchr(message, ']');
        if ((int)(second_bracket - first_bracket) == 5)
        {
            code = 1000 * (first_bracket[1]-'0') + 
            100 * (first_bracket[2]-'0') + 
            10 * (first_bracket[3]-'0') + 
            1 * (first_bracket[4]-'0');
        }
    } while (code == 0 || first_bracket == NULL || second_bracket == NULL);
    return code;
}

//=============================================================================
//
int wait_for_return_code(int code, Meca500 *robot)
{
    int found_error_code = 0;
    int found_requested_code = 0;
    char recv_buffer[512] = {0};
    int recv_result = 0;
    int code_received = 0;
    do
    {
        recv_result = tcpip_receive(recv_buffer, 512, &robot->robot_connection_);
        if (recv_result < 0)
        {
            return -1;
        }
        code_received = extract_code(recv_buffer);
        found_requested_code = (code_received == code);
        found_error_code = code_received >= 1000 && code_received < 2000;
    } while (!found_requested_code && !found_error_code);

    if (found_error_code)
    {
        return 1;
    }
    if (found_requested_code)
    {
        return 0;
    }

    // Should not come here...
    return -1;
}

//=============================================================================
//
int wait_for_EOB(Meca500 *robot)
{
    return wait_for_return_code(3012, robot);
}

//=============================================================================
//
int connect_robot(char *address, Meca500 *robot)
{
    int result = 0;
    // Connect with the good port
    result = tcpip_connect(address, CONTROL_PORT, &robot->robot_connection_);

    if (result == -1)
    {
        printf("Robot connection failed on network error.\r\n");
        return -1;
    }

    // Wait for the robot to return data.

    char recv_buffer[512] = {0};

    result = tcpip_receive(recv_buffer, 512, &robot->robot_connection_);
    if (result == -1)
    {
        printf("Robot connection failed on network error (recv).\r\n");
        return -1;
    }
    else if (result == 0)
    {
        printf("Robot connection failed because robot closed the connection.\r\n");
        return -1;
    }

    // Check for the connection acknowledged from the robot.
    char *search_result = strstr(recv_buffer, "[3000]");
    if (search_result == NULL)
    {
        printf("Robot connection failed because robot did not accept the control.\r\n");
        printf("%s\r\n", recv_buffer);
        return -1;
    }
    robot->connected_ = 1;
    printf("Connected to the Meca500 at addresss %s\r\n", address);
    return 0;
}

//=============================================================================
//
int disconnect_robot(Meca500 *robot)
{
    tcpip_disconnect(&robot->robot_connection_);
    robot->connected_ = 0;
}

//=============================================================================
//
int activate_robot(Meca500 *robot)
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char *msg = "ActivateRobot\0";
    tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    int result = wait_for_return_code(2000, robot);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int deactivate_robot(Meca500 *robot)
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char *msg = "DeactivateRobot\0";
    tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    int result = wait_for_return_code(2004, robot);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int activate_sim(Meca500 *robot)
{
    //EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char *msg = "ActivateSim\0";
    int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    if (result != 0)
    {
        return -1;
    }
    result = wait_for_return_code(2045, robot);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int deactivate_sim(Meca500 *robot)
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char *msg = "DeactivateSim\0";
    int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    if (result != 0)
    {
        return -1;
    }
    result = wait_for_return_code(2046, robot);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int home(Meca500 *robot)
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char *msg = "Home\0";
    int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    if (result != 0)
    {
        return -1;
    }
    result = wait_for_return_code(2002, robot);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int movejoints(Meca500 *robot, float joints[6])
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char msg[100] = {0};
    sprintf(msg, "MoveJoints(%10.4f, %10.4f, %10.4f, %10.4f, %10.4f, %10.4f)\0", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]);
    int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int movepose(Meca500 *robot, float euler[6])
{
    EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_);
    char msg[100] = {0};
    sprintf(msg, "MovePose(%10.4f, %10.4f, %10.4f, %10.4f, %10.4f, %10.4f)\0", euler[0], euler[1], euler[2], euler[3], euler[4], euler[5]);
    int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_);
    if (result != 0)
    {
        return -1;
    }
    return 0;
}

//=============================================================================
//
int movetozero(Meca500 *robot)
{
    float joints[6] = {0};
    return movejoints(robot, joints);
}

//=============================================================================
//
int movetoshipping(Meca500 *robot)
{
    float joints[6] = {0, -60, 60, 0, 0, 0};
    return movejoints(robot, joints);
}


main.c is the main program that calls the function from the robot's interface.


main.c

#define WIN32_LEAN_AND_MEAN

#include <stdlib.h>
#include <stdio.h>
#include "robot_interface.h"

// Need to link with Ws2_32.lib, Mswsock.lib, and Advapi32.lib
#pragma comment (lib, "Ws2_32.lib")
#pragma comment (lib, "Mswsock.lib")
#pragma comment (lib, "AdvApi32.lib")


/// Little example to show usage of the Meca500 in C
/// This example Connect to the robot, activate the simulation,
/// Activate/home the robot and do some movement.
/// Then, it deactivate the robot and the simulation and disconnect.
int main(int argc, char **argv) 
{
    Meca500 robot;
    // Connect
    int result = connect_robot("192.168.0.100", &robot);
    if( result != 0)
    {
        return -1;
    }
    /// For visual clarity, will no check for the return value of the function.
    activate_sim(&robot);
    activate_robot(&robot);
    home(&robot);

    float pose_1 [6] = {250,0,250,0,90,0};
    float pose_2 [6] = {150,0,150,180,00,180};
    movepose(&robot, pose_1);
    movepose(&robot, pose_2);
    movetozero(&robot);
    movetoshipping(&robot);
    /// Movement function to not wait for completion before returning from their call
    /// So we wait eob here.
    wait_for_EOB(&robot);
    
    deactivate_robot(&robot);
    deactivate_sim(&robot);
    disconnect_robot(&robot);

    return 0;
}



Was this article helpful?

That’s Great!

Thank you for your feedback

Sorry! We couldn't be helpful

Thank you for your feedback

Let us know how can we improve this article!

Select atleast one of the reasons

Feedback sent

We appreciate your effort and will try to fix the article