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 lcec_el5152_write(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el5152_data_t *hal_data = (lcec_el5152_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i;
lcec_el5152_chan_t *chan;
// set outputs
for (i=0; i<LCEC_EL5152_CHANS; i++) {
chan = &hal_data->chans[i];
// set output data
EC_WRITE_BIT(&pd[chan->set_count_pdo_os], chan->set_count_pdo_bp, *(chan->set_raw_count));
EC_WRITE_S32(&pd[chan->set_count_val_pdo_os], *(chan->set_raw_count_val));
}
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain1_pd + status_word);
inpu[1]=EC_READ_U32(domain1_pd + pos_act);
// if(servooff==1){//servo off
// if(stop==1){
// if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){
// EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
// EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 );
// }
// else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){
// printk(KERN_INFO PFX "want to PREOP");
// deactive++;
// }
// }
if( (inpu[0]&0x004f) == 0x0040 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x006f) == 0x0023){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+tar_pos, 0);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
}
else if( (inpu[0]&0x006f) == 0x0027){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
EC_WRITE_S32(domain1_pd+tar_pos , value ); //for mode 8 no sin
if(value==180000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
value=value-1;
}
else if(speeddown==1 && value!=0){
value=value-1;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && value==0){
speedup=0;
speeddown=0;
// stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
value=value+1;
//printk(KERN_INFO PFX "fast up ");
}
// change++;
// }
// else
// change = 0;
}
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void cyclic_task()
{
struct timespec wakeupTime, time;
// get current time
clock_gettime(CLOCK_TO_USE, &wakeupTime);
while(1)
{
if(deactive==1)
{
break;
}
wakeupTime = timespec_add(wakeupTime, cycletime);
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
// writter_receive(master);
ecrt_master_receive(master);
ecrt_domain_process(domain_r);
ecrt_domain_process(domain_w);
temp[0]=EC_READ_U16(domain_w_pd + status_word);
temp[1]=EC_READ_S32(domain_w_pd + mode_display);
if (counter)
{
counter--;
}
else
{ // do this at 1 Hz
counter = FREQUENCY;
check_master_state();
blink = !blink;
}
// write process data
if(servo_flag==1)
{
//servo off
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x004f) == 0x0040 )
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x006f) == 0x0021)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0007 );
}
else if( (temp[0]&0x006f) == 0x0023)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f );
EC_WRITE_S32(domain_r_pd+tar_pos,0);
EC_WRITE_S32(domain_r_pd+tar_vel, 0xffff);
EC_WRITE_S32(domain_r_pd+max_torq, 0xf00);
}
//operation enabled
else if( (temp[0]&0x006f) == 0x0027)
{
EC_WRITE_S32(domain_r_pd+tar_pos, (move_value+=2000) );
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f);
}
clock_gettime(CLOCK_TO_USE, &time);
ecrt_master_application_time(master, TIMESPEC2NS(time));
if (sync_ref_counter)
{
sync_ref_counter--;
}
else
{
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domain_r);
ecrt_domain_queue(domain_w);
ecrt_master_send(master);
}
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
/*
if ( (inpu[0]==0x0650) &&(stop==1) )
break;
*/
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
ecrt_domain_process(domain2);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain2_pd + status_word);
inpu[1]=EC_READ_U32(domain2_pd + actual_pos);
/*
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
*/
/*
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
*/
// write process data
/*
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
*/
// if(servooff==1){//servo off
if(stop==1){
if ( inpu[0] == 0x1637 ) {
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( inpu[0] == 0x0650 ){
//++deactive ;
//EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
deactive++;
}
/*
else{
EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
break;
}
*/
}
else if( (inpu[0]&0x0040) == 0x0040){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x027f) == 0x0233){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+interpolateddata, 0);
//EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
EC_WRITE_S32(domain1_pd+modeofoper, 8);
}
else if( (inpu[0]&0x027f) == 0x0237){
//if(change >= 0 && change<2 ){
if( change<1 ){
//start=1;
//if(i==0){
//EC_WRITE_S32(domain1_pd+interpolateddata , 0 );
//EC_WRITE_S32(domain1_pd+interpolateddata2 , 0 );
//EC_WRITE_S32(domain1_pd+target_pos , 0 );
//EC_WRITE_S32(domain1_pd+target_pos2 , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo2 , 0 );
//}
//else {
//EC_WRITE_S32(domain1_pd+interpolateddata , (sin(i)*180000) ); //for mode 7
//EC_WRITE_S32(domain1_pd+interpolateddata2 , (sin(i)*180000) );
//EC_WRITE_S32(domain1_pd+target_pos , (sin(i)*180000) ); //for mode 8 with sin
//EC_WRITE_S32(domain1_pd+target_pos2 , (sin(i)*180000) );
EC_WRITE_S32(domain1_pd+target_pos , inpu[7] ); //for mode 8 no sin
//EC_WRITE_S32(domain1_pd+tar_velo , 500000 ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , 500000 );
//if(1){
if(inpu[7]==1800000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
inpu[7]=inpu[7]-200;
}
else if(speeddown==1 && inpu[7]!=0){
inpu[7]=inpu[7]-200;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && inpu[7]==0){
speedup=0;
speeddown=0;
stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
inpu[7]=inpu[7]+2000;
//printk(KERN_INFO PFX "fast up ");
}
/*
if(speedup==1)
inpu[7]+=500;
else if(speeddown==1)
inpu[7]-=1000;
else{
inpu[7]=0;
servooff=1;
}
*/
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//else{
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//}
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
change++;
/*
if(datacount<10001){
data[datacount][0]=(sin(i)*360000);
data[datacount][1]=inpu[1];
data[datacount][2]=inpu[3];
data[datacount][3]=(inpu[1] - inpu[3]);
datacount++;
}
*/
}
else
change = 0;
}
//printk(KERN_INFO PFX "pos1=%d pos2=%d inpu7=%d\n",inpu[1],inpu[3],inpu[7]);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_domain_queue(domain2);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
int main(int argc, char **argv)
{
// Создаем мастер-объект
gkMaster = ecrt_request_master(0);
if (gkMaster) {
fprintf(stdout, "1. Master created.\n");
} else {
fprintf(stderr, "Unable to get requested master.\n");
return -1;
}
// Создаем объект для обмена PDO в циклическом режиме.
gkDomain1 = ecrt_master_create_domain(gkMaster);
if (gkDomain1) {
fprintf(stdout, "2. Process data domain created.\n");
} else {
fprintf(stderr, "Unable to create process data domain.\n");
return -1;
}
// Создаем объект конфигурации подчиненного.
ec_slave_config_t* sc = ecrt_master_slave_config(gkMaster, 0, gkDriveNum, 0x00007595, 0x00000000);
if (sc) {
fprintf(stdout, "3. Slave configuration object created.\n");
} else {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
// Конфигурируем PDO подчиненного
// TxPDO
ec_pdo_entry_info_t l7na_tx_channel1[] = {
{0x6041, 0, 16}, // Statusword
{0x6061, 0, 8}, // The Modes of Operation Display
{0x6062, 0, 32}, // The Position Demand Value
{0x6064, 0, 32}, // The Position Actual Value
{0x606B, 0, 32}, // The Velocity Demand Value
{0x6081, 0, 32}, // The Profile Velocity
{0x606C, 0, 32}, // The Actual Velocity Value
{0x607A, 0, 32}, // The Target Position
{0x6077, 0, 16}, // Actual torque value
// {0x200F, 0, 16}, // Position Scale Denominator
};
ec_pdo_info_t l7na_tx_pdos[] = {
{0x1A00, 9, l7na_tx_channel1}
};
// RxPDO
ec_pdo_entry_info_t l7na_rx_channel1[] = {
{0x6040, 0, 16}, // Controlword
{0x6060, 0, 8}, // Modes of Operation
{0x607A, 0, 32}, // The Target Position
{0x606C, 0, 32}, // The Velocity Demand value
{0x6081, 0, 32}, // The Profile Velocity
{0x60FF, 0, 32}, // The Target Velocity (in Profile Velocity (Pv) mode and Cyclic Synchronous Velocity (Csv) modes)
{0x6071, 0, 16}, // The Target Torque
};
ec_pdo_info_t l7na_rx_pdos[] = {
{0x1600, 7, l7na_rx_channel1}
};
// Конфигурация SyncManagers 2 (FMMU0) и 3 (FMMU1)
// { sync_mgr_idx, sync_mgr_direction, pdo_num, pdo_ptr, watch_dog_mode }
// { 0xFF - end marker}
ec_sync_info_t l7na_syncs[] = {
{2, EC_DIR_OUTPUT, 1, l7na_rx_pdos, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 1, l7na_tx_pdos, EC_WD_DISABLE},
{0xFF}
};
if (ecrt_slave_config_pdos(sc, EC_END, l7na_syncs)) {
fprintf(stderr, "Failed to configure slave pdo.\n");
return -1;
}
fprintf(stdout, "4. Configuring slave PDOs and sync managers done.\n");
// Регистируем PDO в домене
if (ecrt_domain_reg_pdo_entry_list(gkDomain1, gkDomain1Regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
fprintf(stdout, "5. PDO entries registered in domain.\n");
if (ecrt_master_activate(gkMaster)) {
fprintf(stderr,"Master activation failed.\n");
return -1;
}
fprintf(stdout, "6. Master activated.\n");
if (!(gkDomain1PD = ecrt_domain_data(gkDomain1))) {
fprintf(stderr,"Domain data initialization failed.\n");
return -1;
}
fprintf(stdout, "7. Domain data registered.\n");
//goto end;
check_master_state();
check_domain1_state();
int32_t op_flag = 0, ipos = 0;
uint16_t istatus = 0;
//ждать режим OP
for(uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster); //RECEIVE A FRAME
ecrt_domain_process(gkDomain1); //DETERMINE THE DATAGRAM STATES
// check_slave_config_states();
if (! op_flag) {
check_domain1_state();
}
if (gkDomain1State.wc_state == EC_WC_COMPLETE && !op_flag) {
printf("Domain is up at %d cycles.\n", j);
op_flag = 1;
}
ipos = EC_READ_U32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
istatus = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
// send process data
ecrt_domain_queue(gkDomain1); //MARK THE DOMAIN DATA AS READY FOR EXCHANGE
ecrt_master_send(gkMaster); //SEND ALL QUEUED DATAGRAMS
usleep(1000); //WAIT 1mS
if (op_flag) {
printf("1-Position: %d Status: 0x%x\n", ipos, istatus);
break;
}
}
fprintf(stdout, "8. Got OP state.\n");
if(argc > 1) {
//перейти в позицию
const int cmdpos = atoi(argv[1]);
printf("cmd pos: %d\n", cmdpos);
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0xF); //0x6040 ControlWord
EC_WRITE_U8(gkDomain1PD + gkOffOMode, 1); // 0x6060 Profile position mode // 3 - for velocity mode, 1- for position mode
EC_WRITE_S32(gkDomain1PD + gkOffPVel, 1000000); // 0x60ff profile velocity // gkOffTVel - for velocity mode
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
/* comment 2 lines for velocity mode */
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x11F);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
/* ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);*/
//wait
/* for (uint32_t i = 0; i < 1000; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
*/
timespec tbegin, tend;
::clock_gettime(CLOCK_MONOTONIC, &tbegin);
printf("Time begin: %lds/%ldns\n", tbegin.tv_sec, tbegin.tv_nsec);
const uint32_t kIterationMax = 500000;
uint32_t change_count = 0;
bool target_reached = false;
for (uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
int32_t ipos_new = EC_READ_S32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
uint16_t istatus_new = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
int32_t imode = EC_READ_S8(gkDomain1PD + gkOffIMode);
int32_t ipvel = EC_READ_S32(gkDomain1PD + gkOffPVel);
int32_t idvel = EC_READ_S32(gkDomain1PD + gkOffDVel);
int32_t iavel = EC_READ_S32(gkDomain1PD + gkOffIVel);
int32_t idpos = EC_READ_S32(gkDomain1PD + gkOffDPos);
int32_t itpos = EC_READ_S32(gkDomain1PD + gkOffOPos);
int32_t icontrol = EC_READ_U16(gkDomain1PD + gkOffOControl);
int16_t iatorq = EC_READ_S16(gkDomain1PD + gkOffITorq);
// int32_t ipdenom = EC_READ_S16(gkDomain1PD + gkOffPDenom);
if (ipos_new != ipos) {
ipos = ipos_new;
change_count++;
printf("Position: %d Status: 0x%x Mode: %d ATorq: %d PVel: %d DVel: %d AVel: %d DPos: %d TPos: %d OControl: 0x%x\n", ipos, istatus, imode, iatorq, ipvel, idvel, iavel, idpos, itpos, icontrol);
}
// position mode
if(! target_reached && ((istatus_new >> 10) & 0x1)) {
::clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Target reached. Pos: %d Status: 0x%x TEnd=%lds/%ldns\n", ipos, istatus, tend.tv_sec, tend.tv_nsec);
target_reached = true;
//break;
}
/* Velocity mode */
if (j == kIterationMax) {
/* clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Iterations=%d, change_count=%d. time_end=%lds/%ldns Stopping...\n", j, change_count, tend.tv_sec, tend.tv_nsec);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x6);
break;
*/
}
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(100); //WAIT 1mS
}
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
printf("...Done. Releasing the master!\n");
// Освобождаем мастер-объект
ecrt_release_master(gkMaster);
return 0;
}
