/** \file
* \brief Example code for Simple Open EtherCAT master
*
* Usage : simple_test [ifname1]
* ifname is NIC interface, f.e. eth0
*
* This is a minimal test.
*
* (c)Arthur Ketels 2010 - 2011
*/
#include <grpc/support/log.h>
#include <grpcpp/channel.h>
#include <grpcpp/client_context.h>
#include <grpcpp/create_channel.h>
#include <grpcpp/grpcpp.h>
#include <grpcpp/server.h>
#include <grpcpp/server_builder.h>
#include <grpcpp/server_context.h>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include "absl/memory/memory.h"
#include "robot.grpc.pb.h"
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerAsyncWriter;
using grpc::ServerBuilder;
using grpc::ServerCompletionQueue;
using grpc::ServerContext;
using grpc::Status;
using RobotExample::Robot;
using RobotExample::RobotStatus;
using RobotExample::RobotStatusRequest;
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include "etherCatConfig.h"
#include "ethercat.h"
#define EC_TIMEOUTMON 500
char IOmap[4096];
OSAL_THREAD_HANDLE thread1;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
volatile int64_t current_time = 0;
int64_t g_ControlWord = 0;
int64_t g_ModesOfOperation = 0;
int64_t g_ProfileAcceleration = 0;
int64_t g_ProfileDeceleration = 0;
int64_t g_ProfileVelocity = 0;
int64_t g_TargetVelocity = 0;
int64_t g_TargetPosition = 0;
int64_t g_MaxProfileVelocity = 0;
int64_t g_positive_torquee_limit = 0;
int64_t g_negative_torquee_limit = 0;
int64_t g_ControlWord_2 = 0;
int64_t g_ModesOfOperation_2 = 0;
int64_t g_ProfileAcceleration_2 = 0;
int64_t g_ProfileDeceleration_2 = 0;
int64_t g_ProfileVelocity_2 = 0;
int64_t g_TargetVelocity_2 = 0;
int64_t g_TargetPosition_2 = 0;
int64_t g_MaxProfileVelocity_2 = 0;
int64_t g_positive_torquee_limit_2 = 0;
int64_t g_negative_torquee_limit_2 = 0;
int64_t g_ControlWord_3 = 0;
int64_t g_ModesOfOperation_3 = 0;
int64_t g_ProfileAcceleration_3 = 0;
int64_t g_ProfileDeceleration_3 = 0;
int64_t g_ProfileVelocity_3 = 0;
int64_t g_TargetVelocity_3 = 0;
int64_t g_TargetPosition_3 = 0;
int64_t g_MaxProfileVelocity_3 = 0;
int64_t g_positive_torquee_limit_3 = 0;
int64_t g_negative_torquee_limit_3 = 0;
int64_t g_ControlWord_4 = 0;
int64_t g_ModesOfOperation_4 = 0;
int64_t g_ProfileAcceleration_4 = 0;
int64_t g_ProfileDeceleration_4 = 0;
int64_t g_ProfileVelocity_4 = 0;
int64_t g_TargetVelocity_4 = 0;
int64_t g_TargetPosition_4 = 0;
int64_t g_MaxProfileVelocity_4 = 0;
int64_t g_positive_torquee_limit_4 = 0;
int64_t g_negative_torquee_limit_4 = 0;
int64_t g_IOSensors_Channel_9_Output = 0;
int64_t g_IOSensors_Channel_10_Output = 0;
int64_t g_IOSensors_Channel_11_Output = 0;
int64_t g_IOSensors_Channel_12_Output = 0;
class RequestHandlerBase {
public:
virtual void Proceed(bool succes) = 0;
};
class GetStatusHandler : public RequestHandlerBase {
public:
// Take in the "service" instance (in this case representing an asynchronous
// server) and the completion queue "cq" used for asynchronous communication
// with the gRPC runtime.
GetStatusHandler(Robot::AsyncService* service, ServerCompletionQueue* cq)
: service_(service),
cq_(cq),
responder_(&ctx_),
status_(CREATE),
number_(0) {
// Invoke the serving logic right away.
Proceed();
}
void Proceed(bool succes = true) override {
if (status_ == CREATE) {
// Make this instance progress to the PROCESS state.
status_ = PROCESS;
service_->RequestgetStatus(&ctx_, &request_, &responder_, cq_, cq_, this);
} else if (status_ == PROCESS) {
// each CallData object should create only one new CallData
if (number_ == 0) {
std::cout << new GetStatusHandler(service_, cq_) << std::endl;
number_++;
}
/*if (number_++ >= 10000) // we want to send the response 3 times (for
whatever reason)
{
status_ = FINISH;
responder_.Finish(Status::OK, this);
std::cout << "finished\n";
}
else*/
{
if (succes) {
InputsProcessImage * input = &g_inputsProcessImage; //reinterpret_cast<InputsProcessImage*>(&g_outputsProcessImage);
status_reply_.set_current_position(input->Joint1_1st_transmit_PDO_Mapping_Position_actual_value);
status_reply_.set_status(input->Joint1_1st_transmit_PDO_Mapping_Statusword);
status_reply_.set_modes_of_operation(input->Joint1_1st_transmit_PDO_Mapping_Modes_of_operation_display);
status_reply_.set_modes_of_operation_2(input->Joint2_1st_transmit_PDO_Mapping_Modes_of_operation_display);
status_reply_.set_modes_of_operation_3(input->Joint3_1st_transmit_PDO_Mapping_Modes_of_operation_display);
status_reply_.set_modes_of_operation_4(input->Joint4_1st_transmit_PDO_Mapping_Modes_of_operation_display);
status_reply_.set_error_code_1(input->Joint1_1st_transmit_PDO_Mapping_Error_code);
//status_reply_.set_current_speed(input->joint1_1st_Transmit_PDO_mapping_Velocity_Actual_Value);
status_reply_.set_current_position_2(input->Joint2_1st_transmit_PDO_Mapping_Position_actual_value);
status_reply_.set_status_2(input->Joint2_1st_transmit_PDO_Mapping_Statusword);
//status_reply_.set_modes_of_operation_2(input->joint2_1st_Transmit_PDO_mapping_Mode_of_Operation_Display);
status_reply_.set_error_code_2(input->Joint2_1st_transmit_PDO_Mapping_Error_code);
//status_reply_.set_current_speed_2(input->joint2_1st_Transmit_PDO_mapping_Velocity_Actual_Value);
status_reply_.set_current_position_3(input->Joint3_1st_transmit_PDO_Mapping_Position_actual_value);
status_reply_.set_status_3(input->Joint3_1st_transmit_PDO_Mapping_Statusword);
//status_reply_.set_modes_of_operation_2(input->joint2_1st_Transmit_PDO_mapping_Mode_of_Operation_Display);
status_reply_.set_error_code_3(input->Joint3_1st_transmit_PDO_Mapping_Error_code);
//status_reply_.set_current_speed_2(input->joint2_1st_Transmit_PDO_mapping_Velocity_Actual_Value);
status_reply_.set_current_position_4(input->Joint4_1st_transmit_PDO_Mapping_Position_actual_value);
status_reply_.set_status_4(input->Joint4_1st_transmit_PDO_Mapping_Statusword);
//status_reply_.set_modes_of_operation_2(input->joint2_1st_Transmit_PDO_mapping_Mode_of_Operation_Display);
status_reply_.set_error_code_4(input->Joint4_1st_transmit_PDO_Mapping_Error_code);
//status_reply_.set_current_speed_2(input->joint2_1st_Transmit_PDO_mapping_Velocity_Actual_Value);
status_reply_.set_current_time(current_time);
responder_.Write(status_reply_, this);
} else {
status_ = FINISH;
}
}
}
else {
std::cout << "bye\n";
delete this;
}
}
private:
// The means of communication with the gRPC runtime for an asynchronous
// server.
Robot::AsyncService* service_;
// The producer-consumer queue where for asynchronous server notifications.
ServerCompletionQueue* cq_;
// Context for the rpc, allowing to tweak aspects of it such as the use
// of compression, authentication, as well as to send metadata back to the
// client.
ServerContext ctx_;
// What we get from the client.
RobotStatusRequest request_;
// What we send back to the client.
RobotStatus status_reply_;
// The means to get back to the client.
ServerAsyncWriter<RobotStatus> responder_;
// Let's implement a tiny state machine with the following states.
enum CallStatus { CREATE, PROCESS, FINISH, WRITE };
CallStatus status_; // The current serving state.
int number_;
};
class ChangeVariableHandler : public RequestHandlerBase {
public:
ChangeVariableHandler(Robot::AsyncService* service, ServerCompletionQueue* cq)
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE) {
Proceed();
}
void Proceed(bool ok = true) override {
if (status_ == CREATE) {
status_ = PROCESS;
service_->RequestChangeVariable(&ctx_, &request_, &responder_, cq_, cq_,
this);
} else if ((status_ == PROCESS) && ok) {
new ChangeVariableHandler(service_, cq_);
bool operation_ok = true;
std::cout << "received request " << request_.name() << " value " << request_.i64_value() << std::endl ;
switch (request_.name()) {
case 0:
g_ControlWord = request_.i64_value();
break;
case 1:
g_ModesOfOperation = request_.i64_value();
break;
case 2:
g_ProfileAcceleration = request_.i64_value();
break;
case 3:
g_ProfileDeceleration = request_.i64_value();
break;
case 4:
g_ProfileVelocity = request_.i64_value();
break;
case 5:
g_TargetVelocity = request_.i64_value();
break;
case 6:
g_TargetPosition = request_.i64_value();
break;
case 7:
g_MaxProfileVelocity = request_.i64_value();
break;
case 8:
g_positive_torquee_limit = request_.i64_value();
break;
case 9:
g_negative_torquee_limit = request_.i64_value();
break;
case 10:
g_ControlWord_2 = request_.i64_value();
break;
case 11:
g_ModesOfOperation_2 = request_.i64_value();
break;
case 12:
g_ProfileAcceleration_2 = request_.i64_value();
break;
case 13:
g_ProfileDeceleration_2 = request_.i64_value();
break;
case 14:
g_ProfileVelocity_2 = request_.i64_value();
break;
case 15:
g_TargetVelocity_2 = request_.i64_value();
break;
case 16:
g_TargetPosition_2 = request_.i64_value();
break;
case 17:
g_MaxProfileVelocity_2 = request_.i64_value();
break;
case 18:
g_positive_torquee_limit_2 = request_.i64_value();
break;
case 19:
g_negative_torquee_limit_2 = request_.i64_value();
break;
case 20:
g_ControlWord_3 = request_.i64_value();
break;
case 21:
g_ModesOfOperation_3 = request_.i64_value();
break;
case 22:
g_ProfileAcceleration_3 = request_.i64_value();
break;
case 23:
g_ProfileDeceleration_3 = request_.i64_value();
break;
case 24:
g_ProfileVelocity_3 = request_.i64_value();
break;
case 25:
g_TargetVelocity_3 = request_.i64_value();
break;
case 26:
g_TargetPosition_3 = request_.i64_value();
break;
case 27:
g_MaxProfileVelocity_3 = request_.i64_value();
break;
case 28:
g_positive_torquee_limit_3 = request_.i64_value();
break;
case 29:
g_negative_torquee_limit_3 = request_.i64_value();
break;
case 44:
g_ControlWord_4 = request_.i64_value();
break;
case 45:
g_ModesOfOperation_4 = request_.i64_value();
break;
case 46:
g_ProfileAcceleration_4 = request_.i64_value();
break;
case 47:
g_ProfileDeceleration_4 = request_.i64_value();
break;
case 48:
g_ProfileVelocity_4 = request_.i64_value();
break;
case 49:
g_TargetVelocity_4 = request_.i64_value();
break;
case 50:
g_TargetPosition_4 = request_.i64_value();
break;
case 51:
g_MaxProfileVelocity_4 = request_.i64_value();
break;
case 52:
g_positive_torquee_limit_4 = request_.i64_value();
break;
case 53:
g_negative_torquee_limit_4 = request_.i64_value();
break;
case 30:
g_IOSensors_Channel_9_Output = request_.i64_value();
case 31:
g_IOSensors_Channel_10_Output = request_.i64_value();
case 32:
g_IOSensors_Channel_11_Output = request_.i64_value();
case 33:
g_IOSensors_Channel_12_Output = request_.i64_value();
default:
operation_ok = false;
break;
}
reply_.set_ok(operation_ok);
status_ = FINISH;
responder_.Finish(reply_, Status::OK, this);
} else {
GPR_ASSERT(status_ == FINISH);
delete this;
}
}
private:
Robot::AsyncService* service_;
ServerCompletionQueue* cq_;
ServerContext ctx_;
RobotExample::ChangeVariableRequest request_;
RobotExample::ChangeVariableResponse reply_;
ServerAsyncResponseWriter<RobotExample::ChangeVariableResponse> responder_;
enum CallStatus { CREATE, PROCESS, FINISH };
CallStatus status_; // The current serving state.
};
class ServerImpl final {
public:
~ServerImpl() {
server_->Shutdown();
// Always shutdown the completion queue after the server.
cq_->Shutdown();
}
// There is no shutdown handling in this code.
void Run() {
ServerBuilder builder;
// Listen on the given address without any authentication mechanism.
builder.AddListeningPort(server_address, grpc::InsecureServerCredentials());
// Register "service_" as the instance through which we'll communicate with
// clients. In this case it corresponds to an *asynchronous* service.
builder.RegisterService(&service_);
// Get hold of the completion queue used for the asynchronous communication
// with the gRPC runtime.
cq_ = builder.AddCompletionQueue();
// Finally assemble the server.
server_ = builder.BuildAndStart();
std::cout << "Server listening on " << server_address << std::endl;
// Proceed to the server's main loop.
new GetStatusHandler(&service_, cq_.get());
new ChangeVariableHandler(&service_, cq_.get());
}
void communication_cyle() {
void* tag = 0; // uniquely identifies a request.
bool ok;
int got_event;
auto now = std::chrono::system_clock::now();
std::unordered_map<void*, bool> map;
do {
got_event = cq_->AsyncNext(&tag, &ok, now);
if (got_event == ServerCompletionQueue::GOT_EVENT) map[tag] = ok;
} while (got_event == ServerCompletionQueue::GOT_EVENT);
for (const auto& elt : map) {
static_cast<RequestHandlerBase*>(elt.first)->Proceed(elt.second);
}
}
private:
std::unique_ptr<ServerCompletionQueue> cq_;
Robot::AsyncService service_;
std::unique_ptr<Server> server_;
};
int read_physical_addresses(int slave_count)
{
for(int i = 1 ; i <= slave_count ; i++)
{
int16_t data;
wkc = ecx_APRD(ecx_context.port, -i+1, 16, sizeof(data), &data,EC_TIMEOUTRET3);
if ( wkc <= 0 )
{
printf("error reading slave %d physicaladdress", i);
return -1;
}
ecx_context.slavelist[i].configadr = data ;
print_data(&data,2,"reading adress");
}
return 0;
}
int main(int argc, char* argv[])
{
printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
if (argc < 2)
{
printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
return -1;
}
const char * ifname = argv[1];
ServerImpl grpc_server;
grpc_server.Run();
/* initialise SOEM, bind socket to ifname */
if (!ec_init(ifname))
{
printf("No socket connection on %s\nExecute as root\n", ifname);
return -1;
}
if (ec_config_init(FALSE) <= 0)
{
printf("error ec_config_init\n");
return -1;
}
printf("%d slaves found and configured.\n", ec_slavecount);
memset(&g_inputsProcessImage, 0, sizeof(InputsProcessImage) );
memset(&g_outputsProcessImage, 0, sizeof(OutputsProcessImage) );
if ( init_ethercat_master(IP_TRANSITION) < 0 )
return-1;
if ( init_ethercat_slaves(IP_TRANSITION) < 0 )
return -1;
if ( read_physical_addresses(ec_slavecount) < 0 )
return-1;
if ( init_coe(PS_TRANSITION) < 0 )
return -1;
ec_configdc();
// ec_dcsync0(1,true,1000000,0);
// ec_dcsync0(2,true,1000000,0);
osal_usleep(100 * 1000);
if ( init_ethercat_slaves(PS_TRANSITION) < 0 )
return -1;
osal_usleep(100 * 1000);
if ( init_ethercat_slaves(SO_TRANSITION) < 0 )
return -1;
if ( init_coe(SO_TRANSITION) < 0 )
return -1;
while (1)
{
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Controlword = g_ControlWord;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Modes_of_operation = g_ModesOfOperation;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Target_velocity = g_TargetVelocity;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Profile_velocity = g_ProfileVelocity;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Target_position = g_TargetPosition;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Max_profile_velocity = g_MaxProfileVelocity;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Profile_acceleration = g_ProfileAcceleration;
g_outputsProcessImage.Joint1_1st_receive_PDO_Mapping_Profile_deceleration = g_ProfileDeceleration;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Controlword = g_ControlWord_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Modes_of_operation = g_ModesOfOperation_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Target_velocity = g_TargetVelocity_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Profile_velocity = g_ProfileVelocity_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Target_position = g_TargetPosition_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Max_profile_velocity = g_MaxProfileVelocity_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Profile_acceleration = g_ProfileAcceleration_2;
g_outputsProcessImage.Joint2_1st_receive_PDO_Mapping_Profile_deceleration = g_ProfileDeceleration_2;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Controlword = g_ControlWord_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Modes_of_operation = g_ModesOfOperation_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Target_velocity = g_TargetVelocity_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Profile_velocity = g_ProfileVelocity_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Target_position = g_TargetPosition_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Max_profile_velocity = g_MaxProfileVelocity_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Profile_acceleration = g_ProfileAcceleration_3;
g_outputsProcessImage.Joint3_1st_receive_PDO_Mapping_Profile_deceleration = g_ProfileDeceleration_3;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Controlword = g_ControlWord_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Modes_of_operation = g_ModesOfOperation_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Target_velocity = g_TargetVelocity_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Profile_velocity = g_ProfileVelocity_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Target_position = g_TargetPosition_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Max_profile_velocity = g_MaxProfileVelocity_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Profile_acceleration = g_ProfileAcceleration_4;
g_outputsProcessImage.Joint4_1st_receive_PDO_Mapping_Profile_deceleration = g_ProfileDeceleration_4;
gen_send_cyclic_cmd(OP_STATE);
//check for slave states
grpc_server.communication_cyle();
current_time++;
//print_data(&g_inputsProcessImage, sizeof(InputsProcessImage) , "Inputs:" );
osal_usleep(1000);
}
printf("End program\n");
return 0;
}
CLIENT CODE:
//client configurations
stub_ = Robot::NewStub( grpc::CreateChannel("localhost:50051", grpc::InsecureChannelCredentials()) );
timer_ = new QTimer(this);
context_ = new ClientContext();
RobotStatusRequest request;
stream_rpc_ = stub_->AsyncgetStatus(context_, request, &cq_, (void*)10);
stream_rpc_->Finish(&status_, (void*)2);
//subscriber button click event code finishd
timer_->start(1);
//to pass the data to grpc server
auto * req2 = new SetVariableRequestHandler(RobotExample::ChangeVariableRequest_varName_ControlWord,
6,
&cq_,
stub_.get() );
class SetVariableRequestHandler
{
public:
SetVariableRequestHandler(RobotExample::ChangeVariableRequest_varName name,
int64_t value ,
grpc::CompletionQueue * cq,
RobotExample::Robot::Stub * stub)
{
RobotExample::ChangeVariableRequest request;
request.set_i64_value( value );
request.set_name(name);
rpc_ = stub->AsyncChangeVariable(new grpc::ClientContext() , request, cq );
rpc_->Finish(&changeVariableResponse_, &status_, (void*)this);
}
void proceed(bool ok)
{
}
private:
grpc::Status status_;
std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::RobotExample::ChangeVariableResponse>> rpc_;
RobotExample::ChangeVariableResponse changeVariableResponse_;
};