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 cyclic_task()
{
/* sync the dc clock of the slaves */
ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
blink = !blink;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
#if SDO_ACCESS
// read process data SDO
read_sdo(sdo);
read_sdo(request[0]);
read_sdo(request[1]);
read_sdo(request[2]);
write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */
#endif
}
/* Read process data */
unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in);
unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in);
unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in);
unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in);
unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in);
logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n",
sn_status, sn_modes,
sn_position, sn_velocity, sn_torque);
#if 0
// read process data
printf("AnaIn: state %u value %u\n",
EC_READ_U8(domain1_pd + off_ana_in_status),
EC_READ_U16(domain1_pd + off_ana_in_value));
#endif
// write process data
//EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#ifdef CIA402
#define STATUSW1 0x88AA
#define STATUSW2 0xAA88
#define OPMODES1 0xf1
#define OPMODES2 0x1f
#define TORVAL1 0xabab
#define TORVAL2 0xbaba
#define VELVAL1 0x2d2d4d4d
#define VELVAL2 0xd4d4d2d2
#define POSVAL1 0xe4e4e2e2
#define POSVAL2 0x2e2e4e4e
EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff);
EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff);
EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff);
EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2);
EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2);
#else
#define TESTWORD1 0xdead
#define TESTWORD2 0xbeef
#define TESTWORD3 0xfefe
#define TESTWORD4 0xa5a5
EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2);
EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
//printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2);
}
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();
/*
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();
}
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
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);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}