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;
}