void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); 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(); 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); 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 = 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 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 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(); } }