#Once the bus is configured and the device is setup, the following 2 routines are called
# once each per program loop. Loop time is 1/1024 s.

# so essentially

#		while(1){
#		write_ecat();
#		read_ecat();
#		}

write_ecat(data_in)
int *data_in;
{
static short pval;
static int i,v,vs,k;

	if(!master)return(-1);

		CODE NOW DOES EITHER S300 or AKD DEPENDNG ON BUS CONFIGURATION

S300 CODE.............................

	vel[i] = ((data_in[i] * 10.0/32768) * 150 * 65536*32*16.0) / 
							(60.0 * 4000.0);
	vel[i] = vel[i] * velocity_factor[i];

	EC_WRITE_U32(domain_pd + 
		slaves[seat_config.axes[i].position].data_out[0],vel[i]);

	if(seat_engaged){
	sdo_dig1_sync[i]++;
	if(sdo_dig1_sync[i] > 50){
	if(read_sdo(sdo_dig1[i],8,&v)){
	sdo_dig1_value[i] = v;
	sdo_dig1_sync[i] = 0;
	}
	}

	sdo_dig2_sync[i]++;
	if(sdo_dig2_sync[i] > 50){
	if(read_sdo(sdo_dig2[i],8,&v)){
	sdo_dig2_value[i] = v;
	sdo_dig2_sync[i] = 0;
	}
	}
	}

	}

		END OF S300

AKD CODE.....................................

	vel[i] = (data_in[i] / 32768.0) * 8000.0;
	vel[i] = vel[i] * velocity_factor[i];


	EC_WRITE_U32(domain_pd + 
		slaves[seat_config.axes[i].position].data_out[0],vel[i]);

		END OF AKD

COMMAN CODE........................

	ecrt_domain_queue(domain);
	ecrt_master_send(master);

}

read_ecat(data_out)
INPUT_DATA *data_out;
{
static int v,ind = 0,j,i,k;
static short trq;
static float cal = 3276.8 / 1048576;
static ec_master_state_t ms;
static ec_domain_state_t dstate;
static ec_slave_config_state_t st;
static unsigned int status;

	if(!master)return(-1);

	COMMON CODE...........................

	ecrt_master_receive(master);
	ecrt_domain_process(domain);

        if(!(link_tmr++ % 128)){
        ecrt_master_state(master, &ms);

	ecrt_domain_state(domain,&dstate);

        if(ms.link_up != link_latch || dstate.wc_state == EC_WC_ZERO){

        printf("Link is %s.\n", 
		ms.link_up && dstate.wc_state != EC_WC_ZERO ? "up" : "down");

	link_latch = ms.link_up;
	started = 0;
	}

	if(!started){
	if(ms.slaves_responding < no_of_ecat_modules){
        printf("No of modules/ecat devices: %d %d\n",
				no_of_ecat_modules,ms.slaves_responding);
	started = 0;
	}

	}

      }

		END OF COMMON CODE

		CODE NOW DOES EITHER S300 or AKD DEPENDNG ON BUS CONFIGURATION

S300 CODE..................

	if(!engage_flag[i]){
	if(read_sdo(sdo_dstat[i],32,&v)){
	amp_homed[i] = (v & 0x00020000)?1:0;
	}
	}

	k = EC_READ_U32(domain_pd + 
		slaves[seat_config.axes[i].position].data_in[
					seat_config.axes[i].channel]);

	if(engage_flag[i] >= 2 || amp_homed[i]){

	j = k - posl[i];
	if(j > 500000){
	j -= 1048576;
	}
	else if(j < -500000){
	j += 1048576;
	}

	posl[i] = k;

	value[i] += j;

	if(value[i] < posm[i])posm[i] = value[i];
	if(value[i] > posx[i])posx[i] = value[i];

	data_out->input_data[i] = value[i] * cal;


/*
        if(!(heartBeatCount_Int3 % 512)){
	printf("%d %f %d\n",i,data_out->input_data[i],value[i]);
	}
*/

	trq = EC_READ_U16(domain_pd + 
			slaves[seat_config.axes[i].position].data_in[1]);
	data_out->trq_data[i] = trq;
	}
	else
	{
	posm[i] = k;
	posx[i] = k;
	posl[i] = posx[i];
	data_out->input_data[i] = 0;
	data_out->trq_data[i] = 0;
	value[i] = 0;
	}


	newstat = EC_READ_U16(domain_pd + 
			slaves[seat_config.axes[i].position].data_in[2]);
	status = newstat;
	data_out->status[i] = status;


AKD CODE.....................

	if(!engage_flag[i]){
	if(read_sdo(sdo_dstat[i],32,&v)){
	amp_homed[i] = (v & 0x00000002)?1:0;
	}
	}

	k = EC_READ_U32(domain_pd + 
		slaves[seat_config.axes[i].position].data_in[
					seat_config.axes[i].channel]);

	if(engage_flag[i] >= 2 || amp_homed[i]){


	j = k - posl[i];

	posl[i] = k;

	value[i] += j;

	if(value[i] < posm[i])posm[i] = value[i];
	if(value[i] > posx[i])posx[i] = value[i];

	data_out->input_data[i] = value[i] * cal;


/*
        if(!(heartBeatCount_Int3 % 512)){
	printf("%d %f %d\n",i,data_out->input_data[i],value[i]);
	}
*/


	trq = EC_READ_U16(domain_pd + 
			slaves[seat_config.axes[i].position].data_in[1]);
	data_out->trq_data[i] = trq;

	}
	else
	{
	posm[i] = k;
	posx[i] = k;
	posl[i] = posx[i];
	data_out->input_data[i] = 0;
	data_out->trq_data[i] = 0;
	value[i] = 0;
	}


	newstat = EC_READ_U16(domain_pd + 
			slaves[seat_config.axes[i].position].data_in[2]);
	status = newstat;
	data_out->status[i] = status;

/*
        if(!(heartBeatCount_Int3 % 512)){
printf("value = %d %d %d %x\n",value[i],k,heartBeatCount_Int3,newstat);
	}
*/

}



read_sdo(sdo,sz,val)
ec_sdo_request_t *sdo;
int sz,*val;
{
int stat = 0;

	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:
//        printf("Still busy...\n");
        break;

case EC_REQUEST_SUCCESS:
	stat = 1;

	switch(sz){
case 8:
	*val = (int)EC_READ_U8(ecrt_sdo_request_data(sdo));
	break;

case 16:
	*val = (int)EC_READ_U16(ecrt_sdo_request_data(sdo));
	break;

case 32:
	*val = (int)EC_READ_U32(ecrt_sdo_request_data(sdo));
	break;

	}

        ecrt_sdo_request_read(sdo); // trigger next read
        break;

case EC_REQUEST_ERROR:
//        printf("Failed to read SDO %d!\n",sdo);
        ecrt_sdo_request_read(sdo); // retry reading
        break;
	}

	return(stat);
}

write_sdo(sdo,sz,val)
ec_sdo_request_t *sdo;
int sz,val;
{
int stat = 0;
unsigned char vc;
unsigned short vs;

	switch (ecrt_sdo_request_state(sdo)){

case EC_REQUEST_BUSY:
        break;

case EC_REQUEST_UNUSED: // request was not used yet
case EC_REQUEST_SUCCESS:
	stat = 1;

	switch(sz){
case 8:
	vc = (unsigned char)val;
	EC_WRITE_U8(ecrt_sdo_request_data(sdo),vc);
	break;

case 16:
	vs = (unsigned short)val;
	EC_WRITE_U16(ecrt_sdo_request_data(sdo),vs);
	break;

case 32:
	EC_WRITE_U32(ecrt_sdo_request_data(sdo),val);
	break;

	}
        ecrt_sdo_request_write(sdo); // trigger next write
        break;

case EC_REQUEST_ERROR:
        ecrt_sdo_request_write(sdo); // retry writing
//        printf("Failed to write SDO %d!\n",sdo);
        break;
	}

	return(stat);
}