component k1MbServer "ModBus Server to comunicate with modbus master tcp device trought holding_reg"; pin in float jaaddress "default port connection if not declared"; pin in float ja30ip "first part of default address" ; pin in float ja31ip "second part of default address" ; pin in float ja32ip "third part of default address" ; pin in float ja33ip "fouth part of default address" ; pin out bit shutdown "1 connect to shutdown linuxcnc command"; pin out bit on "2 connect to pin halui.machine.on"; pin out bit off "3 connect to pin halui.machine.off"; pin out bit pause "4 connect to pin halui.program.pause"; pin out bit resume "5 connect to pin halui.program.resume"; pin out bit stop "6 connect to pin halui.program.stop"; pin out bit run "7 connect to pin halui.program.run"; pin out bit manual "8 connect to pin halui.mode.manual"; pin out bit automode "9 connect to pin halui.mode.auto"; pin out bit mdi "10 connect to pin halui.mode.mdi"; pin out bit teleop "11 connect to pin halui.mode.teleop"; pin out bit aborte "12 connect to pin halui.abort"; pin out bit step "13 connect to pin halui.program.step"; pin out bit home0 "14 connect to pin halui.joint.0.home"; pin out bit home1 "15 connect to pin halui.joint.1.home"; pin out bit home2 "16 connect to pin halui.joint.2.home"; pin out bit home3 "17 connect to pin halui.joint.3.home"; pin out bit home4 "18 connect to pin halui.joint.4.home"; pin out bit homeall "19 connect to pin halui.joint.home.all"; pin out bit m00 "20 connect to pin halui.mdi_command_00"; pin out bit m01 "21 connect to pin halui.mdi_command_01"; pin out bit m02 "22 connect to pin halui.mdi_command_02"; pin out bit m03 "23 connect to pin halui.mdi_command_03"; pin out bit m04 "24 connect to pin halui.mdi_command_04"; pin out bit miston "25 connect to pin halui.mist.on"; pin out bit mistoff "26 connect to pin halui.mist.off"; pin out bit lubeon "27 connect to pin halui.lube.on"; pin out bit lubeoff "28 connect to pin halui.lube.off"; pin out bit floodon "29 connect to pin halui.flood.on"; pin out bit floodoff "30 connect to pin halui.flood.off"; pin out bit estopon "31 connect to pin halui.estop.activated"; pin out bit estopoff "31 connect to pin halui.estop.reset"; pin out float cmdj0 "32 connect to pin halui.axis.0.pos_commanded"; pin out float cmdj1 "33 connect to pin halui.axis.1.pos_commanded"; pin out float cmdj2 "34 connect to pin halui.axis.2.pos_commanded"; pin out float cmdj3 "35 connect to pin halui.axis.3.pos_commanded"; pin out float cmdj4 "36 connect to pin halui.axis.4.pos_commanded"; pin out float jogspeed "37 connect to pin halui.feed_override.direct_value"; pin out float maxvel "38 connect to pin halui.max_velocity.direct_value"; pin out bit connection "no connection to register only check if connection exist"; pin out bit mberror "ModBus error"; pin out bit testo "ModBus error testo"; pin out bit tst1 "ModBus error test1"; pin out bit tst2 "ModBus error test2"; pin out bit tst3 "ModBus error test3"; pin in bit resetconnection "reset modbus connection after disconnect"; pin in bit ison "1 connect to pin halui.machine.ison"; pin in bit idle "2 connect to pin halui.program.isidle"; pin in bit pgrrunning "3 connect to pin halui.program.isrunning"; pin in bit paused "4 connect to pin halui.program.ispaused"; pin in bit isautomode "5 connect to pin halui.mode.isauto"; pin in bit ismanual "6 connect to pin halui.mode.ismanual"; pin in bit ismdi "7 connect to pin halui.mode.ismdi"; pin in bit isteleop "8 connect to pin halui.mode.isteleop"; pin in bit mistison "9 connect to pin halui.mist.ison"; pin in bit lubeison "10 connect to pin halui.lube.ison"; pin in bit floodison "11 connect to pin halui.flood.ison"; pin in bit faultj0 "12 connect to pin halui.joint.0.hasfault"; pin in bit faultj1 "13 connect to pin halui.joint.1.hasfault"; pin in bit faultj2 "14 connect to pin halui.joint.2.hasfault"; pin in bit faultj3 "15 connect to pin halui.joint.3.hasfault"; pin in bit faultj4 "16 connect to pin halui.joint.4.hasfault"; pin in bit hardlimitj0 "17 connect to pin halui.joint.0.onhardmax&minlimit"; pin in bit hardlimitj0a "17a connect to pin halui.joint.0.onhardmax&minlimit"; pin in bit hardlimitj1 "18 connect to pin halui.joint.1.onhardmax&minlimit"; pin in bit hardlimitj1a "18a connect to pin halui.joint.1.onhardmax&minlimit"; pin in bit hardlimitj2 "19 connect to pin halui.joint.2.onhardmax&minlimit"; pin in bit hardlimitj2a "19a connect to pin halui.joint.2.onhardmax&minlimit"; pin in bit hardlimitj3 "20 connect to pin halui.joint.3.onhardmax&minlimit"; pin in bit hardlimitj3a "20a connect to pin halui.joint.3.onhardmax&minlimit"; pin in bit hardlimitj4 "21 connect to pin halui.joint.4.onhardmax&minlimit"; pin in bit hardlimitj4a "21a connect to pin halui.joint.4.onhardmax&minlimit"; pin in bit softlimitj0 "22 connect to pin halui.joint.0.onsoftmax&minlimit"; pin in bit softlimitj0a "22a connect to pin halui.joint.0.onsoftmax&minlimit"; pin in bit softlimitj1 "23 connect to pin halui.joint.1.onsoftmax&minlimit"; pin in bit softlimitj1a "23a connect to pin halui.joint.1.onsoftmax&minlimit"; pin in bit softlimitj2 "24 connect to pin halui.joint.2.onsoftmax&minlimit"; pin in bit softlimitj2a "24a connect to pin halui.joint.2.onsoftmax&minlimit"; pin in bit softlimitj3 "25 connect to pin halui.joint.3.onsoftmax&minlimit"; pin in bit softlimitj3a "25a connect to pin halui.joint.3.onsoftmax&minlimit"; pin in bit softlimitj4 "26 connect to pin halui.joint.4.onsoftmax&minlimit"; pin in bit softlimitj4a "26a connect to pin halui.joint.4.onsoftmax&minlimit"; pin in bit estopactive "27 connect to pin halui.estop.isactivated"; pin in float posj0 "28 connect to pin halui.axis.0.posfeedback"; pin in float posj1 "29 connect to pin halui.axis.1.posfeedback"; pin in float posj2 "30 connect to pin halui.axis.2.posfeedback"; pin in float posj3 "31 connect to pin halui.axis.3.posfeedback"; pin in float posj4 "32 connect to pin halui.axis.4.posfeedback"; pin in float ismaxvel "33 connect to pin halui.maxvelocity.value"; pin in float isjogspeed "34 connect to pin halui.feedoverride.value"; option singleton no; option userspace yes; //option userinit yes; option extra_compile_args "-lm -ldl -L/usr/local/include/modbus -lmodbus"; author "giorgiofoga <-> k-1"; license "GPL"; ;; #include /* Standard input/output definitions */ #include #include /* Standard types */ #include /* String function definitions */ //#include /* UNIX standard function definitions */ #include /* File control definitions */ #include /* Error number definitions */ #include #include #include #include #define MAP(n) mb_mapping->tab_registers[n] #define INP(g) mb_mapping->tab_input_registers[g] #define WRI(nr , val) ret=modbus_write_register(ctx, nr, val) char *address; char ad0[4]; char ad1[4]; char ad2[4]; char ad3[4]; char point[2] = "."; char str[16]; float test = 1502.0; float test1 = 169.0; float test2 = 254.0; float test3 = 000.0; float test4 = 103.0; int tt1 = 0; int tt2 = 0; int tt3 = 0; int tt4 = 0; int hole; int done = 0; int firstimeconnect = 0; int ret = -1; int CorrectOnOfSign =0; float kft; uint16_t FCpy[2]; uint16_t RfCpy[2]; int s = -1; modbus_t *ctx; modbus_mapping_t *mb_mapping; int rc; int i; int test_of = 0; uint8_t *query; uint8_t In_tab[100]; uint16_t Reg_tab[100]; int header_length; //void adios(int sig){done = 1;} float Rcf(uint16_t a, uint16_t b) { uint16_t a1[2]; float r1; a1[0] = a; a1[1] = b; r1 = modbus_get_float(a1); kft = r1; return kft; } uint16_t SdFloat(float a) { modbus_set_float(a, FCpy); return *FCpy; } void user_mainloop(void) /* start "main" */ { /* signal(SIGINT, adios); signal(SIGTERM, adios);*/ while(!done) /*"start main loop " */ { usleep(20000); FOR_ALL_INSTS() { testo =1; if (firstimeconnect == 0){ hole = floor(jaaddress); //jaaddress; if (hole != 0) { tt1 = floor(ja30ip); tt2 = floor(ja31ip); //test2); tt3 = floor(ja32ip); tt4 = floor(ja33ip); sprintf(ad0, "%hu", tt1); //ja30ip); sprintf(ad1, "%hu", tt2); //ja31ip); sprintf(ad2, "%hu", tt3); //ja32ip); sprintf(ad3, "%hu", tt4); //ja33ip); strcpy(str, ad0); strcat(str, point); strcat(str, ad1); strcat(str, point); strcat(str, ad2); strcat(str, point); strcat(str, ad3); address = str; } else { address = "0";} ctx = modbus_new_tcp(address, hole); query = malloc(MODBUS_TCP_MAX_ADU_LENGTH); header_length = modbus_get_header_length(ctx); modbus_set_debug(ctx, false); mb_mapping = modbus_mapping_new(0,0,200,0); if (mb_mapping == NULL) { //fprintf(stderr, "Failed to allocate the mapping: %s\n", //modbus_strerror(errno)); modbus_free(ctx); //return -1; } /* mb_mapping->tab_registers[1] = 1; mb_mapping->tab_registers[2] = 1; mb_mapping->tab_registers[3] = 1; mb_mapping->tab_registers[10] = 1; mb_mapping->tab_registers[18] = 1; mb_mapping->tab_registers[19] = 1;*/ /* mb_mapping->tab_registers[45] = 1; mb_mapping->tab_registers[46] = 1; mb_mapping->tab_registers[47] = 1;*/ s = modbus_tcp_listen(ctx, 1); modbus_tcp_accept(ctx, &s); firstimeconnect = 1; connection = 1; mberror = 0; } /*---end firstimeconnect-----*/ /*firstimeconnect = 1; connection = 1;*/ if (mb_mapping->tab_registers[0] == 1){off = 0; usleep(500); on = 1; } else { on = 0; usleep(500); off = 1;} //if (mb_mapping->tab_registers[1] == 1){off = 1;} else { off = 0;} if (mb_mapping->tab_registers[2] == 1){pause = 1;} else { pause = 0;} if (mb_mapping->tab_registers[3] == 1){resume = 1;} else { resume = 0;} if (mb_mapping->tab_registers[4] == 1){stop = 1;} else { stop = 0;} if (mb_mapping->tab_registers[5] == 1){run = 1;} else { run = 0;} if (mb_mapping->tab_registers[6] == 1){manual = 1;} else { manual = 0;} if (mb_mapping->tab_registers[7] == 1){automode = 1;} else { automode = 0;} if (mb_mapping->tab_registers[8] == 1){mdi = 1;} else { mdi = 0;} if (mb_mapping->tab_registers[9] == 1){teleop = 1;} else { teleop = 0;} if (mb_mapping->tab_registers[10] == 1){aborte = 1;} else { aborte = 0;} if (mb_mapping->tab_registers[11] == 1){step = 1;} else { step = 0;} if (mb_mapping->tab_registers[12] == 1){home0 = 1;} else { home0 = 0;} if (mb_mapping->tab_registers[13] == 1){home1 = 1;} else { home1 = 0;} if (mb_mapping->tab_registers[14] == 1){home2 = 1;} else { home2 = 0;} if (mb_mapping->tab_registers[15] == 1){home3 = 1;} else { home3 = 0;} if (mb_mapping->tab_registers[16] == 1){home4 = 1;} else { home4 = 0;} if (mb_mapping->tab_registers[17] == 1){homeall = 1;} else { homeall = 0;} if (mb_mapping->tab_registers[18] == 1){m00 = 1;} else { m00 = 0;} if (mb_mapping->tab_registers[19] == 1){m01 = 1;} else { m01 = 0;} if (mb_mapping->tab_registers[20] == 1){m02 = 1;} else { m02 = 0;} if (mb_mapping->tab_registers[21] == 1){m03 = 1;} else { m03 = 0;} if (mb_mapping->tab_registers[22] == 1){m04 = 1;} else { m04 = 0;} if (mb_mapping->tab_registers[23] == 1){miston = 1; mistoff = 0 ;} else { miston = 0; mistoff = 1;} if (mb_mapping->tab_registers[24] == 1){/*mistoff = 0; */} else { /*mistoff = 1;*/} if (mb_mapping->tab_registers[25] == 1){lubeon = 1; lubeoff = 0; } else { lubeon = 0; lubeoff = 1;} if (mb_mapping->tab_registers[26] == 1){/*lubeoff = 1;*/} else { /*lubeoff = 0;*/} if (mb_mapping->tab_registers[27] == 1){floodon = 1;floodoff = 0;} else { floodon = 0;floodoff = 1;} if (mb_mapping->tab_registers[28] == 1){/*floodoff = 1;*/} else { /*floodoff = 0;*/} if (mb_mapping->tab_registers[29] == 1){ estopoff = 0; estopon = 1; mb_mapping->tab_registers[0] = 0; on = 0; usleep(500); off = 1; } else { estopon = 0; estopoff = 1;} Rcf(mb_mapping->tab_registers[30], mb_mapping->tab_registers[31]); { cmdj0 = kft;} Rcf(mb_mapping->tab_registers[32], mb_mapping->tab_registers[33]); { cmdj1 = kft;} Rcf(mb_mapping->tab_registers[34], mb_mapping->tab_registers[35]); { cmdj2 = kft;} Rcf(mb_mapping->tab_registers[36], mb_mapping->tab_registers[37]); { cmdj3 = kft;} Rcf(mb_mapping->tab_registers[38], mb_mapping->tab_registers[39]); { cmdj4 = kft;} Rcf(mb_mapping->tab_registers[40], mb_mapping->tab_registers[41]); { jogspeed = kft;} Rcf(mb_mapping->tab_registers[42], mb_mapping->tab_registers[43]); { maxvel = kft;} //if (mb_mapping->tab_registers[44] == 1){estopoff = 0;} else { estopoff = 1;} if (ison) { mb_mapping->tab_registers[45] = 0; mb_mapping->tab_registers[72] = 0; }else{ mb_mapping->tab_registers[45] = 5;} if (idle) { mb_mapping->tab_registers[46] = 1; } if (!idle) { mb_mapping->tab_registers[46] = 0; } if (pgrrunning) { mb_mapping->tab_registers[47] = 1; } if (!pgrrunning) { mb_mapping->tab_registers[47] = 0; } if (paused) { mb_mapping->tab_registers[48] = 1; } if (!paused) { mb_mapping->tab_registers[48] = 0; } if (isautomode) { mb_mapping->tab_registers[49] = 1; } if (!isautomode) { mb_mapping->tab_registers[49] = 0; } if (ismanual) { mb_mapping->tab_registers[50] = 1; } if (!ismanual) { mb_mapping->tab_registers[50] = 0; } if (ismdi) { mb_mapping->tab_registers[51] = 1; } if (!ismdi) { mb_mapping->tab_registers[51] = 0; } if (isteleop) { mb_mapping->tab_registers[52] = 1; } if (!isteleop) { mb_mapping->tab_registers[52] = 0; } if (mistison) { mb_mapping->tab_registers[53] = 1; } if (!mistison) { mb_mapping->tab_registers[53] = 0; } if (lubeison) { mb_mapping->tab_registers[54] = 1; } if (!lubeison) { mb_mapping->tab_registers[54] = 0; } if (floodison) { mb_mapping->tab_registers[55] = 1; } if (!floodison) { mb_mapping->tab_registers[55] = 0; } if (faultj0) { mb_mapping->tab_registers[56] = 1; } if (faultj1) { mb_mapping->tab_registers[57] = 1; } if (faultj2) { mb_mapping->tab_registers[58] = 1; } if (faultj3) { mb_mapping->tab_registers[59] = 1; } if (faultj4) { mb_mapping->tab_registers[60] = 1; } if (hardlimitj0 || hardlimitj0a ) { mb_mapping->tab_registers[61] = 1;} if (hardlimitj1 || hardlimitj1a ) { mb_mapping->tab_registers[62] = 1;} if (hardlimitj2 || hardlimitj2a ) { mb_mapping->tab_registers[63] = 1;} if (hardlimitj3 || hardlimitj3a ) { mb_mapping->tab_registers[64] = 1;} if (hardlimitj4 || hardlimitj4a ) { mb_mapping->tab_registers[65] = 1;} if (softlimitj0 || softlimitj0a ) { mb_mapping->tab_registers[66] = 1; } if (softlimitj1 || softlimitj1a ) { mb_mapping->tab_registers[67] = 1; } if (softlimitj2 || softlimitj2a ) { mb_mapping->tab_registers[68] = 1; } if (softlimitj3 || softlimitj3a ) { mb_mapping->tab_registers[69] = 1; } if (softlimitj4 || softlimitj4a ) { mb_mapping->tab_registers[70] = 1; } if (estopactive) { mb_mapping->tab_registers[71] = 1; } if (!ison) { mb_mapping->tab_registers[72] = 1; } SdFloat(posj0); { mb_mapping->tab_registers[75] = FCpy[0]; mb_mapping->tab_registers[76] = FCpy[1];} SdFloat(posj1); { mb_mapping->tab_registers[77] = FCpy[0]; mb_mapping->tab_registers[78] = FCpy[1];} SdFloat(posj2); { mb_mapping->tab_registers[79] = FCpy[0]; mb_mapping->tab_registers[80] = FCpy[1];} SdFloat(posj3); { mb_mapping->tab_registers[81] = FCpy[0]; mb_mapping->tab_registers[82] = FCpy[1];} SdFloat(posj4); { mb_mapping->tab_registers[83] = FCpy[0]; mb_mapping->tab_registers[84] = FCpy[1];} SdFloat(ismaxvel); { mb_mapping->tab_registers[85] = FCpy[0]; mb_mapping->tab_registers[86] = FCpy[1];} SdFloat(isjogspeed); { mb_mapping->tab_registers[87] = FCpy[0]; mb_mapping->tab_registers[87] = FCpy[1];} if (mb_mapping->tab_registers[1] == 1){ mb_mapping->tab_registers[56] = 0; mb_mapping->tab_registers[57] = 0; mb_mapping->tab_registers[58] = 0; mb_mapping->tab_registers[59] = 0; mb_mapping->tab_registers[60] = 0; mb_mapping->tab_registers[61] = 0; mb_mapping->tab_registers[62] = 0; mb_mapping->tab_registers[63] = 0; mb_mapping->tab_registers[64] = 0; mb_mapping->tab_registers[65] = 0; mb_mapping->tab_registers[66] = 0; mb_mapping->tab_registers[67] = 0; mb_mapping->tab_registers[68] = 0; mb_mapping->tab_registers[69] = 0; mb_mapping->tab_registers[70] = 0; mb_mapping->tab_registers[71] = 0; tst1 = 0; tst2 = 0; tst3 = 0; } do { rc = modbus_receive(ctx, query); /* Filtered queries return 0 */ } while (rc == 0); /* The connection is not closed on errors which require on reply such as bad CRC in RTU. */ // if (rc == -1 && errno != EMBBADCRC) {break;} rc = modbus_reply(ctx, query, rc, mb_mapping); //if (rc == -1) {connection = 0; break;} //if (resetconnection == 1) //((rc == -1 && errno != EMBBADCRC) || (resetconnection == 1))// || ///(errno==EBADF || errno==ECONNRESET || errno==EPIPE)) testo =0; } /* end FOR_ALL_INSTS() */ } /* stop main loop " */ /* close connection + errno */ //printf("Quit the loop: %s\n", modbus_strerror(errno)); if (s != -1) { close(s);} modbus_mapping_free(mb_mapping); free(query); modbus_close(ctx); modbus_free(ctx); firstimeconnect = 0; // } /* stop main loop " */ }/* end "main" */