void disable_operation() { bool isOperationDisabled=false; // while(!isOperationDisabled) for(int i=0;i<1;++i) { EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0000);// p85, use state transition 9 switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: fprintf(stderr, "Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x03"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91 { isOperationDisabled = true; printf("motor state is in switch_on_disabled,exit safely \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { break; } else { printf("bad exit!\n"); } }//while }
int write_sdo(ec_sdo_request_t *req, unsigned data) { EC_WRITE_S32(ecrt_sdo_request_data(req), data&0xffffffff); switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_write(req); // trigger first read break; case EC_REQUEST_BUSY: //fprintf(stderr, "SDO write still busy...\n"); //logmsg(1, "SDO value written: \n",data ); pause(); break; case EC_REQUEST_SUCCESS: //logmsg(1, "SDO value written: 0x%X\n", data); pause(); ecrt_sdo_request_read(req); // trigger next read return 1; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to write SDO!\n"); ecrt_sdo_request_write(req); // retry writing return 0; break; } }
void read_sdo(void) { switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "SDO value: 0x%04X\n", EC_READ_U16(ecrt_sdo_request_data(sdo))); ecrt_sdo_request_read(sdo); // trigger next read break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(sdo); // retry reading break; } }
void read_sdo(ec_sdo_request_t *req) { switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(req); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "SDO still busy...\n"); break; case EC_REQUEST_SUCCESS: logmsg(1, "SDO value read: 0x%X\n", EC_READ_U32(ecrt_sdo_request_data(req))); ecrt_sdo_request_read(req); // trigger next read break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(req); // retry reading break; } }
int read_sdo(ec_sdo_request_t *req) { int sdo_read_value; switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(req); // trigger first read break; case EC_REQUEST_BUSY: //fprintf(stderr, "SDO still busy...\n"); break; case EC_REQUEST_SUCCESS: sdo_read_value = EC_READ_S32(ecrt_sdo_request_data(req)); //logmsg(1, "SDO value read: 0x%X\n", sdo_read_value); ecrt_sdo_request_write(req); // trigger next write break; case EC_REQUEST_ERROR: //fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(req); // retry reading break; } return sdo_read_value; }
int main(int argc, char **argv) { // ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) { return -1; } if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if SDO_ACCESS fprintf(stderr, "Creating operation mode read SDO requests...\n"); if (!(sdo_operation_mode_display = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION_DISPLAY, 0, 1))) // uint8 data size 1 { fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n"); return -1; } fprintf(stderr, "Creating operation mode write SDO requests...\n"); if (!(sdo_operation_mode_write = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION, 0, 1))) // uint8 data size 1 { fprintf(stderr, "Failed to create SDO MODES_OF_OPERATION request.\n"); return -1; } fprintf(stderr, "Creating controlword write SDO requests...\n"); if (!(sdo_controlword_write = ecrt_slave_config_create_sdo_request(sc_ana_in, CONTROLWORD, 0, 2))) // uint16 data size 2 { fprintf(stderr, "Failed to create SDO CONTROLWORD request.\n"); return -1; } fprintf(stderr, "Creating statusword read SDO requests...\n"); if (!(sdo_statusword_read = ecrt_slave_config_create_sdo_request(sc_ana_in, STATUSWORD, 0, 2))) // uint16 data size 2 { fprintf(stderr, "Failed to create SDO STATUSWORD request.\n"); return -1; } //EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); ecrt_sdo_request_timeout(sdo_operation_mode_display, 500); // ms ecrt_sdo_request_timeout(sdo_operation_mode_write, 500); // ms ecrt_sdo_request_timeout(sdo_controlword_write, 500); // ms ecrt_sdo_request_timeout(sdo_statusword_read, 500); // ms #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } // if (!(sc = ecrt_master_slave_config( // master, AnaOutSlavePos, Beckhoff_EL4102))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // if (!(sc = ecrt_master_slave_config( // master, DigOutSlavePos, Beckhoff_EL2032))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // // Create configuration for bus coupler // sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); // if (!sc) // return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } #endif printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; #if 1 if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #endif #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif printf("Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } // handle ctrl+c ,important , do not remove sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (signal(SIGINT, my_sig_handler) == SIG_ERR) { printf("\ncan't catch SIGUSR1\n"); return -1; } if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } // 1. check operation mode bool getModeOk=false; uint8_t mode_value; int i=0; while(1) { // printf("i=%d\n",i); i++; // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read_sdo(); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); break; case EC_REQUEST_BUSY: // printf( "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); getModeOk = true; mode_value = EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { printf("get mode value <%02x>\n",mode_value); break; } // ecrt_domain_queue(domain1); ecrt_master_send(master); // pause(); // sleep(1); // cyclic_task(); } printf("check mode done\n"); if(getModeOk == false) { exit(-1); } // exit(0); // 2. set operation mode to velocity mode printf("setting mode to velocity_mode...\n"); bool isWriteModeOk=false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_operation_mode_write), 0x03); ecrt_sdo_request_write(sdo_operation_mode_write); switch (ecrt_sdo_request_state(sdo_operation_mode_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_operation_mode_write write value 0x03 ok\n"); // ecrt_sdo_request_write(sdo_operation_mode_write); isWriteModeOk = true; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } if(isWriteModeOk) { break; } ecrt_master_send(master); } // 3. check operation mode, after write getModeOk=false; // for(int i=0;i<10;++i) i=0; while(1) { i++; ecrt_master_receive(master); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf("sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0) { printf("mode is in velocity_mode \n"); getModeOk = true; } else { printf("mode not in velocity_mode \n"); exit(-1); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } // if(i<10) { ecrt_master_send(master); } // sleep(1); } // 4. read target velocity while(1) { i++; ecrt_master_receive(master); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf("sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0) { printf("mode is in velocity_mode \n"); getModeOk = true; } else { printf("mode not in velocity_mode \n"); exit(-1); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } ecrt_master_send(master); } // 5. set target velocity to zero // 6. read statusword printf("6.read statusword...\n"); bool isOperationDisabled=false; // while(!isOperationDisabled) while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0080);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x0080\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91 { isOperationDisabled = true; printf("motor state is in switch_on_disabled \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good \n"); break; } ecrt_master_send(master); } // 7. set controlword to ready_to_switch_on printf("7. set controlword , ready_to_switch_on...\n"); isOperationDisabled = false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0006);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x0006\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F ^ 0x0021 == 0) // p91 { isOperationDisabled = true; printf("motor state is in ready_to_switch_on \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good2 \n"); break; } ecrt_master_send(master); } // 8. set controlword , enable operation printf("8. set controlword , enable operation...\n"); isOperationDisabled = false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x000F);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x000F\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); fprintf(stderr, "statusword value aa: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F); fprintf(stderr, "statusword value aaa: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F^ 0x0027); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F == 0x0027) // p91 { isOperationDisabled = true; printf("motor state is in operation_on \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good2 \n"); break; } ecrt_master_send(master); } // 8. set target velocity 100r/min // sa.sa_handler = signal_handler; // sigemptyset(&sa.sa_mask); // sa.sa_flags = 0; // if (sigaction(SIGALRM, &sa, 0)) { // fprintf(stderr, "Failed to install signal handler!\n"); // return -1; // } // printf("Started.\n"); // while (1) { // pause(); //#if 0 // struct timeval t; // gettimeofday(&t, NULL); // printf("%u.%06u\n", t.tv_sec, t.tv_usec); //#endif // while (sig_alarms != user_alarms) { // cyclic_task(); // user_alarms++; // } // } return 0; }
int lcec_el7342_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_el7342_data_t *hal_data; int i; lcec_el7342_chan_t *chan; uint8_t info1_select, info2_select; int err; // initialize callbacks slave->proc_read = lcec_el7342_read; slave->proc_write = lcec_el7342_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_el7342_data_t))) == NULL) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name); return -EIO; } memset(hal_data, 0, sizeof(lcec_el7342_data_t)); slave->hal_data = hal_data; // initialize sync info slave->sync_info = lcec_el7342_syncs; // initialize global data hal_data->last_operational = 0; // initialize pins for (i=0; i<LCEC_EL7342_CHANS; i++) { chan = &hal_data->chans[i]; // read sdos // Info1 selector if ((chan->sdo_info1_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x11, 1)) == NULL) { return -EIO; } info1_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info1_select)); // Info2 selector if ((chan->sdo_info2_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x19, 1)) == NULL) { return -EIO; } info2_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info2_select)); // initialize POD entries LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x02, &chan->latch_ext_valid_pdo_os, &chan->latch_ext_valid_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->set_count_done_pdo_os, &chan->set_count_done_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x04, &chan->count_overflow_pdo_os, &chan->count_overflow_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x05, &chan->count_underflow_pdo_os, &chan->count_underflow_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x08, &chan->expol_stall_pdo_os, &chan->expol_stall_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x09, &chan->ina_pdo_os, &chan->ina_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0a, &chan->inb_pdo_os, &chan->inb_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0d, &chan->inext_pdo_os, &chan->inext_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->sync_err_pdo_os, &chan->sync_err_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1800 + (i * 3), 0x09, &chan->tx_toggle_pdo_os, &chan->tx_toggle_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x12, &chan->latch_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x02, &chan->ena_latch_ext_pos_pdo_os, &chan->ena_latch_ext_pos_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x03, &chan->set_count_pdo_os, &chan->set_count_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x04, &chan->ena_latch_ext_neg_pdo_os, &chan->ena_latch_ext_neg_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x11, &chan->set_count_val_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x01, &chan->dcm_ready_to_enable_pdo_os, &chan->dcm_ready_to_enable_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x02, &chan->dcm_ready_pdo_os, &chan->dcm_ready_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x03, &chan->dcm_warning_pdo_os, &chan->dcm_warning_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x04, &chan->dcm_error_pdo_os, &chan->dcm_error_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x05, &chan->dcm_move_pos_pdo_os, &chan->dcm_move_pos_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x06, &chan->dcm_move_neg_pdo_os, &chan->dcm_move_neg_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x07, &chan->dcm_torque_reduced_pdo_os, &chan->dcm_torque_reduced_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0c, &chan->dcm_din1_pdo_os, &chan->dcm_din1_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0d, &chan->dcm_din2_pdo_os, &chan->dcm_din2_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->dcm_sync_err_pdo_os, &chan->dcm_sync_err_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1806 + (i << 1), 0x09, &chan->dcm_tx_toggle_pdo_os, &chan->dcm_tx_toggle_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x11, &chan->dcm_info1_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x12, &chan->dcm_info2_pdo_os, NULL); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x01, &chan->dcm_ena_pdo_os, &chan->dcm_ena_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x02, &chan->dcm_reset_pdo_os, &chan->dcm_reset_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x03, &chan->dcm_reduce_torque_pdo_os, &chan->dcm_reduce_torque_pdo_bp); LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x21, &chan->dcm_velo_pdo_os, NULL); // export encoder pins if ((err = hal_pin_bit_newf(HAL_IN, &(chan->reset), comp_id, "%s.%s.%s.enc-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->ina), comp_id, "%s.%s.%s.enc-%d-ina", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ina failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inb), comp_id, "%s.%s.%s.enc-%d-inb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inext), comp_id, "%s.%s.%s.enc-%d-inext", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inext failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->sync_err), comp_id, "%s.%s.%s.enc-%d-sync-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-sync-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->expol_stall), comp_id, "%s.%s.%s.enc-%d-expol-stall", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-expol-stall failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->tx_toggle), comp_id, "%s.%s.%s.enc-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->count_overflow), comp_id, "%s.%s.%s.enc-%d-count-overflow", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count-overflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->count_underflow), comp_id, "%s.%s.%s.enc-%d-count-underflow", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count-underflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->latch_ext_valid), comp_id, "%s.%s.%s.enc-%d-latch-ext-valid", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-latch-ext-valid failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->set_raw_count), comp_id, "%s.%s.%s.enc-%d-set-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->ena_latch_ext_pos), comp_id, "%s.%s.%s.enc-%d-index-ext-pos-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ndex-ext-pos-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(chan->ena_latch_ext_neg), comp_id, "%s.%s.%s.enc-%d-index-ext-neg-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ndex-ext-neg-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_IN, &(chan->set_raw_count_val), comp_id, "%s.%s.%s.enc-%d-set-raw-count-val", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count-val failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_count), comp_id, "%s.%s.%s.enc-%d-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->count), comp_id, "%s.%s.%s.enc-%d-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_latch), comp_id, "%s.%s.%s.enc-%d-raw-latch", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-latch failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->pos), comp_id, "%s.%s.%s.enc-%d-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->pos_scale), comp_id, "%s.%s.%s.enc-%d-pos-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } // export servo pins if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_scale), comp_id, "%s.%s.%s.srv-%d-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_offset), comp_id, "%s.%s.%s.srv-%d-offset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-offset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_min_dc), comp_id, "%s.%s.%s.srv-%d-min-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-min-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_max_dc), comp_id, "%s.%s.%s.srv-%d-max-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-max-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_curr_dc), comp_id, "%s.%s.%s.srv-%d-curr-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-curr-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_enable), comp_id, "%s.%s.%s.srv-%d-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_absmode), comp_id, "%s.%s.%s.srv-%d-absmode", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-absmode failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_float_newf(HAL_IN, &(chan->dcm_value), comp_id, "%s.%s.%s.srv-%d-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-value failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_val), comp_id, "%s.%s.%s.srv-%d-raw-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reset), comp_id, "%s.%s.%s.srv-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reduce_torque), comp_id, "%s.%s.%s.srv-%d-reduce-torque", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-reduce-torque failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready_to_enable), comp_id, "%s.%s.%s.srv-%d-ready-to-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready-to-enable%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready), comp_id, "%s.%s.%s.srv-%d-ready", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_warning), comp_id, "%s.%s.%s.srv-%d-warning", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-warning failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_error), comp_id, "%s.%s.%s.srv-%d-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_pos), comp_id, "%s.%s.%s.srv-%d-move-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_neg), comp_id, "%s.%s.%s.srv-%d-move-neg", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_torque_reduced), comp_id, "%s.%s.%s.srv-%d-torque-reduced", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-torque-reduced failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din1), comp_id, "%s.%s.%s.srv-%d-din1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din2), comp_id, "%s.%s.%s.srv-%d-din2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_sync_err), comp_id, "%s.%s.%s.srv-%d-sync-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sync-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_tx_toggle), comp_id, "%s.%s.%s.srv-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info1), comp_id, "%s.%s.%s.srv-%d-raw-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info2), comp_id, "%s.%s.%s.srv-%d-raw-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info1), comp_id, "%s.%s.%s.srv-%d-sel-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info2), comp_id, "%s.%s.%s.srv-%d-sel-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } if (info1_select == INFO_SEL_MOTOR_VELO || info2_select == INFO_SEL_MOTOR_VELO) { if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_velo_fb), comp_id, "%s.%s.%s.srv-%d-velo-fb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-velo-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } } if (info1_select == INFO_SEL_MOTOR_CURR || info2_select == INFO_SEL_MOTOR_CURR) { if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_current_fb), comp_id, "%s.%s.%s.srv-%d-current-fb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-current-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i); return err; } } // initialize pins *(chan->reset) = 0; *(chan->ina) = 0; *(chan->inb) = 0; *(chan->inext) = 0; *(chan->sync_err) = 0; *(chan->expol_stall) = 0; *(chan->tx_toggle) = 0; *(chan->count_overflow) = 0; *(chan->count_underflow) = 0; *(chan->latch_ext_valid) = 0; *(chan->ena_latch_ext_pos) = 0; *(chan->ena_latch_ext_neg) = 0; *(chan->set_raw_count) = 0; *(chan->set_raw_count_val) = 0; *(chan->raw_count) = 0; *(chan->raw_latch) = 0; *(chan->count) = 0; *(chan->pos) = 0; *(chan->pos_scale) = 1.0; *(chan->dcm_scale) = 1.0; *(chan->dcm_offset) = 0.0; *(chan->dcm_min_dc) = -1.0; *(chan->dcm_max_dc) = 1.0; *(chan->dcm_curr_dc) = 0.0; *(chan->dcm_enable) = 0; *(chan->dcm_absmode) = 0; *(chan->dcm_value) = 0.0; *(chan->dcm_raw_val) = 0; *(chan->dcm_reset) = 0; *(chan->dcm_reduce_torque) = 0; *(chan->dcm_ready_to_enable) = 0; *(chan->dcm_ready) = 0; *(chan->dcm_warning) = 0; *(chan->dcm_error) = 0; *(chan->dcm_move_pos) = 0; *(chan->dcm_move_neg) = 0; *(chan->dcm_torque_reduced) = 0; *(chan->dcm_din1) = 0; *(chan->dcm_din2) = 0; *(chan->dcm_sync_err) = 0; *(chan->dcm_tx_toggle) = 0; *(chan->dcm_raw_info1) = 0; *(chan->dcm_raw_info2) = 0; *(chan->dcm_sel_info1) = info1_select; *(chan->dcm_sel_info2) = info2_select; if (chan->dcm_velo_fb != NULL) { *(chan->dcm_velo_fb) = 0.0; } if (chan->dcm_current_fb != NULL) { *(chan->dcm_current_fb) = 0.0; } // initialize variables chan->enc_do_init = 1; chan->enc_last_count = 0; chan->enc_old_scale = *(chan->pos_scale) + 1.0; chan->enc_scale_recip = 1.0; chan->dcm_old_scale = *(chan->dcm_scale) + 1.0; chan->dcm_scale_recip = 1.0; } return 0; }
int lcec_stmds5k_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { lcec_master_t *master = slave->master; lcec_stmds5k_data_t *hal_data; int err; // initialize callbacks slave->proc_read = lcec_stmds5k_read; slave->proc_write = lcec_stmds5k_write; // alloc hal memory if ((hal_data = hal_malloc(sizeof(lcec_stmds5k_data_t))) == NULL) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name); return -EIO; } memset(hal_data, 0, sizeof(lcec_stmds5k_data_t)); slave->hal_data = hal_data; // read sdos // B18 : torque reference if ((hal_data->sdo_torque_reference = lcec_read_sdo(slave, 0x2212, 0x00, 4)) == NULL) { return -EIO; } hal_data->torque_reference = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_torque_reference)) * STMDS5K_TORQUE_REF_DIV; if (hal_data->torque_reference > 1e-20 || hal_data->torque_reference < -1e-20) { hal_data->torque_reference_rcpt = 1.0 / hal_data->torque_reference; } else { hal_data->torque_reference_rcpt = 0.0; } // C01 : max rpm if ((hal_data->sdo_speed_max_rpm = lcec_read_sdo(slave, 0x2401, 0x00, 4)) == NULL) { return -EIO; } hal_data->speed_max_rpm = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm)); // D02 : setpoint max rpm if ((hal_data->sdo_speed_max_rpm_sp = lcec_read_sdo(slave, 0x2602, 0x00, 2)) == NULL) { return -EIO; } hal_data->speed_max_rpm_sp = (double)EC_READ_S16(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm_sp)); if (hal_data->speed_max_rpm_sp > 1e-20 || hal_data->speed_max_rpm_sp < -1e-20) { hal_data->speed_max_rpm_sp_rcpt = 1.0 / hal_data->speed_max_rpm_sp; } else { hal_data->speed_max_rpm_sp_rcpt = 0.0; } // initialize POD entries // E200 : device state byte LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x28c8, 0x00, &hal_data->dev_state_pdo_os, NULL); // E100 : speed motor (x 0.1% relative to C01) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2864, 0x00, &hal_data->speed_mot_pdo_os, NULL); // E02 : torque motor filterd (x 0,1 Nm) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2802, 0x00, &hal_data->torque_mot_pdo_os, NULL); // D200 : speed state word LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26c8, 0x00, &hal_data->speed_state_pdo_os, NULL); // E09 : rotor position LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2809, 0x00, &hal_data->pos_mot_pdo_os, NULL); // A180 : Device Control Byte LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x20b4, 0x00, &hal_data->dev_ctrl_pdo_os, NULL); // D230 : speed setpoint (x 0.1 % relative to D02, -200.0% .. 200.0%) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26e6, 0x00, &hal_data->speed_sp_rel_pdo_os, NULL); // C230 : maximum torque (x 1%, 0% .. 200%) LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x24e6, 0x00, &hal_data->torque_max_pdo_os, NULL); // export pins if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->vel_cmd), comp_id, "%s.%s.%s.srv-vel-cmd", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb), comp_id, "%s.%s.%s.srv-vel-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb_rpm), comp_id, "%s.%s.%s.srv-vel-fb-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_rpm), comp_id, "%s.%s.%s.srv-vel-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_s32_newf(HAL_OUT, &(hal_data->enc_raw), comp_id, "%s.%s.%s.srv-enc-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enc-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_hi), comp_id, "%s.%s.%s.srv-pos-raw-hi", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-hi failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_lo), comp_id, "%s.%s.%s.srv-pos-raw-lo", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-lo failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->pos_fb), comp_id, "%s.%s.%s.srv-pos-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb), comp_id, "%s.%s.%s.srv-torque-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_abs), comp_id, "%s.%s.%s.srv-torque-fb-abs", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-abs failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_pct), comp_id, "%s.%s.%s.srv-torque-fb-pct", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-pct failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->torque_lim), comp_id, "%s.%s.%s.srv-torque-lim", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-lim failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->stopped), comp_id, "%s.%s.%s.srv-stopped", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-stopped failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->at_speed), comp_id, "%s.%s.%s.srv-at-speed", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-at-speed failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->overload), comp_id, "%s.%s.%s.srv-overload", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-overload failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->ready), comp_id, "%s.%s.%s.srv-ready", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->error), comp_id, "%s.%s.%s.srv-error", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-error failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->toggle), comp_id, "%s.%s.%s.srv-toggle", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->loc_ena), comp_id, "%s.%s.%s.srv-loc-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-loc-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->enable), comp_id, "%s.%s.%s.srv-enable", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->err_reset), comp_id, "%s.%s.%s.srv-err-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-err-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->fast_ramp), comp_id, "%s.%s.%s.srv-fast-ramp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-fast-ramp failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->brake), comp_id, "%s.%s.%s.srv-brake", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-brake failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IO, &(hal_data->index_ena), comp_id, "%s.%s.%s.srv-index-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-index-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->pos_reset), comp_id, "%s.%s.%s.srv-pos-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_neg), comp_id, "%s.%s.%s.srv-on-home-neg", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_pos), comp_id, "%s.%s.%s.srv-on-home-pos", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } // export parameters if ((err = hal_param_float_newf(HAL_RW, &(hal_data->pos_scale), comp_id, "%s.%s.%s.srv-pos-scale", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->torque_reference), comp_id, "%s.%s.%s.srv-torque-ref", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-ref failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm), comp_id, "%s.%s.%s.srv-max-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm_sp), comp_id, "%s.%s.%s.srv-max-rpm-sp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm-sp failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } if ((err = hal_param_s32_newf(HAL_RW, &(hal_data->home_raw), comp_id, "%s.%s.%s.srv-home-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) { rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-home-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name); return err; } // set default pin values *(hal_data->vel_cmd) = 0.0; *(hal_data->vel_fb) = 0.0; *(hal_data->vel_fb_rpm) = 1.0; *(hal_data->vel_rpm) = 0.0; *(hal_data->pos_raw_hi) = 0; *(hal_data->pos_raw_lo) = 0; *(hal_data->pos_fb) = 0.0; *(hal_data->torque_fb) = 0.0; *(hal_data->torque_fb_abs) = 0.0; *(hal_data->torque_fb_pct) = 0.0; *(hal_data->torque_lim) = 1.0; *(hal_data->stopped) = 0; *(hal_data->at_speed) = 0; *(hal_data->overload) = 0; *(hal_data->ready) = 0; *(hal_data->error) = 0; *(hal_data->toggle) = 0; *(hal_data->loc_ena) = 0; *(hal_data->enable) = 0; *(hal_data->err_reset) = 0; *(hal_data->fast_ramp) = 0; *(hal_data->brake) = 0; *(hal_data->index_ena) = 0; *(hal_data->pos_reset) = 0; *(hal_data->enc_raw) = 0; *(hal_data->on_home_neg) = 0; *(hal_data->on_home_pos) = 0; // initialize variables hal_data->pos_scale = 1.0; hal_data->do_init = 1; hal_data->pos_cnt = 0; hal_data->index_cnt = 0; hal_data->last_pos_cnt = 0; hal_data->pos_scale_old = hal_data->pos_scale + 1.0; hal_data->pos_scale_rcpt = 1.0; hal_data->pos_scale_cnt = 1.0; hal_data->last_index_ena = 0; hal_data->index_ref = 0; hal_data->home_raw = 0; return 0; }
int main(int argc, char **argv) { // ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) { return -1; } if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } // printf("sync mgr 0 config: %d \n",sc_ana_in->sync_configs[0].dir); // printf("sync mgr 1 config: %d \n",sc_ana_in->sync_configs[1].dir); #if SDO_ACCESS fprintf(stderr, "Creating SDO requests...\n"); if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x6061, 0, 1))) // data size 1 ? { fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n"); return -1; } //EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); ecrt_sdo_request_timeout(sdo, 500); // ms #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } // if (!(sc = ecrt_master_slave_config( // master, AnaOutSlavePos, Beckhoff_EL4102))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // if (!(sc = ecrt_master_slave_config( // master, DigOutSlavePos, Beckhoff_EL2032))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } #endif // // Create configuration for bus coupler // sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); // if (!sc) // return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } printf("Starting timer...\n"); // tv.it_interval.tv_sec = 0; // tv.it_interval.tv_usec = 1000000 / FREQUENCY; // tv.it_value.tv_sec = 0; // tv.it_value.tv_usec = 1000; // if (setitimer(ITIMER_REAL, &tv, NULL)) { // fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); // return 1; // } printf("Started.\n"); while (1) { // pause(); #if 1 // for(int i=0;i<10;++i) // { // my_cyclic_task(); // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read process data SDO switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); ecrt_sdo_request_read(sdo); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf( "SDO value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo))); ecrt_sdo_request_read(sdo); // trigger next read break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(sdo); // retry reading break; } // read_sdo(); // switch (ecrt_sdo_request_state(sdo)) { // case EC_REQUEST_UNUSED: // request was not used yet // printf("request was not used yet\n"); // ecrt_sdo_request_read(sdo); // trigger first read // break; // case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); // break; // case EC_REQUEST_SUCCESS: // fprintf(stderr, "sdo value: 0x%04X\n", // EC_READ_U8(ecrt_sdo_request_data(sdo))); //// getModeOk = true; // break; // case EC_REQUEST_ERROR: // fprintf(stderr, "Failed to read SDO!\n"); // break; // } // send process data // ecrt_domain_queue(domain1); // ecrt_sdo_request_read(sdo); // trigger next read ecrt_master_send(master); ecrt_master_receive(master); // sleep(1); // } #else // 1. check operation mode bool getModeOk=false; for(int i=0;i<10;++i) { printf("i=%d\n",i); // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read_sdo(); // ecrt_sdo_request_read(sdo); // trigger read switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); ecrt_sdo_request_read(sdo); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo))); getModeOk = true; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } // ecrt_domain_queue(domain1); ecrt_master_send(master); // sleep(1); // cyclic_task(); } if(getModeOk == false) { // exit(-1); } #endif } return 0; }
void el60xx_port_run(el60xx_port_t *port, u8 *pd) { u16 status = EC_READ_U16(pd + port->off_status); u8 *rx_data = pd + port->off_rx; uint8_t tx_accepted_toggle, rx_request_toggle; switch (port->state) { case SER_READY: /* Check, if hardware handshaking has to be configured. */ if (!port->config_error && port->requested_rtscts != port->current_rtscts) { EC_WRITE_U8(ecrt_sdo_request_data(port->rtscts_sdo), port->requested_rtscts); ecrt_sdo_request_write(port->rtscts_sdo); port->state = SER_SET_RTSCTS; break; } /* Check, if the baud rate has to be configured. */ if (!port->config_error && port->requested_baud_rate != port->current_baud_rate) { EC_WRITE_U8(ecrt_sdo_request_data(port->baud_sdo), port->requested_baud_rate); ecrt_sdo_request_write(port->baud_sdo); port->state = SER_SET_BAUD_RATE; break; } /* Check, if the data frame has to be configured. */ if (!port->config_error && port->requested_data_frame != port->current_data_frame) { EC_WRITE_U8(ecrt_sdo_request_data(port->frame_sdo), port->requested_data_frame); ecrt_sdo_request_write(port->frame_sdo); port->state = SER_SET_DATA_FRAME; break; } /* Send data */ tx_accepted_toggle = status & 0x0001; if (tx_accepted_toggle != port->tx_accepted_toggle) { // ready port->tx_data_size = ectty_tx_data(port->tty, port->tx_data, port->max_tx_data_size); if (port->tx_data_size) { #if DEBUG printk(KERN_INFO PFX "%s: Sending %u bytes.\n", port->name, port->tx_data_size); #endif port->tx_request_toggle = !port->tx_request_toggle; port->tx_accepted_toggle = tx_accepted_toggle; } } /* Receive data */ rx_request_toggle = status & 0x0002; if (rx_request_toggle != port->rx_request_toggle) { uint8_t rx_data_size = status >> 8; port->rx_request_toggle = rx_request_toggle; #if DEBUG printk(KERN_INFO PFX "%s: Received %u bytes.\n", port->name, rx_data_size); #endif ectty_rx_data(port->tty, rx_data, rx_data_size); port->rx_accepted_toggle = !port->rx_accepted_toggle; } port->control = port->tx_request_toggle | port->rx_accepted_toggle << 1 | port->tx_data_size << 8; break; case SER_REQUEST_INIT: if (status & (1 << 2)) { port->control = 0x0000; port->state = SER_WAIT_FOR_INIT_RESPONSE; } else { port->control = 1 << 2; // CW.2, request initialization } break; case SER_WAIT_FOR_INIT_RESPONSE: if (!(status & (1 << 2))) { printk(KERN_INFO PFX "%s: Init successful.\n", port->name); port->tx_accepted_toggle = 1; port->control = 0x0000; port->state = SER_READY; } break; case SER_SET_RTSCTS: switch (ecrt_sdo_request_state(port->rtscts_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted RTS/CTS.\n", port->name); port->current_rtscts = port->requested_rtscts; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set RTS/CTS on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; case SER_SET_BAUD_RATE: switch (ecrt_sdo_request_state(port->baud_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted baud rate.\n", port->name); port->current_baud_rate = port->requested_baud_rate; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set baud rate on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; case SER_SET_DATA_FRAME: switch (ecrt_sdo_request_state(port->frame_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted data frame.\n", port->name); port->current_data_frame = port->requested_data_frame; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set data frame on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; }