void uei_ethercat_cleanup(void)
{
ecrt_master_receive(master);
ecrt_domain_process(domain);
EC_WRITE_S16(rx_controller_state.current_val, 0);
EC_WRITE_U16(rx_controller_state.amp_state, ECAT_STATE_DISABLED);
// EC_WRITE_S16(pv_controller_state.current_val, 0);
// EC_WRITE_U16(pv_controller_state.amp_state, ECAT_STATE_DISABLED);
//
// EC_WRITE_S16(el_controller_state.current_val, 0);
// EC_WRITE_U16(el_controller_state.amp_state, ECAT_STATE_DISABLED);
ecrt_domain_queue(domain);
ecrt_master_send(master);
ecrt_release_master(master);
}
void lcec_el7342_write(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el7342_data_t *hal_data = (lcec_el7342_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i;
lcec_el7342_chan_t *chan;
double tmpval, tmpdc, raw_val;
// set outputs
for (i=0; i<LCEC_EL7342_CHANS; i++) {
chan = &hal_data->chans[i];
// validate duty cycle limits, both limits must be between
// 0.0 and 1.0 (inclusive) and max must be greater then min
if (*(chan->dcm_max_dc) > 1.0) {
*(chan->dcm_max_dc) = 1.0;
}
if (*(chan->dcm_min_dc) > *(chan->dcm_max_dc)) {
*(chan->dcm_min_dc) = *(chan->dcm_max_dc);
}
if (*(chan->dcm_min_dc) < -1.0) {
*(chan->dcm_min_dc) = -1.0;
}
if (*(chan->dcm_max_dc) < *(chan->dcm_min_dc)) {
*(chan->dcm_max_dc) = *(chan->dcm_min_dc);
}
// do scale calcs only when scale changes
if (*(chan->dcm_scale) != chan->dcm_old_scale) {
// validate the new scale value
if ((*(chan->dcm_scale) < 1e-20) && (*(chan->dcm_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->dcm_scale) = 1.0;
}
// get ready to detect future scale changes
chan->dcm_old_scale = *(chan->dcm_scale);
// we will need the reciprocal
chan->dcm_scale_recip = 1.0 / *(chan->dcm_scale);
}
// get command
tmpval = *(chan->dcm_value);
if (*(chan->dcm_absmode) && (tmpval < 0)) {
tmpval = -tmpval;
}
// convert value command to duty cycle
tmpdc = tmpval * chan->dcm_scale_recip + *(chan->dcm_offset);
if (tmpdc < *(chan->dcm_min_dc)) {
tmpdc = *(chan->dcm_min_dc);
}
if (tmpdc > *(chan->dcm_max_dc)) {
tmpdc = *(chan->dcm_max_dc);
}
// set output values
if (*(chan->dcm_enable) == 0) {
raw_val = 0;
*(chan->dcm_curr_dc) = 0;
} else {
raw_val = (double)0x7fff * tmpdc;
if (raw_val > (double)0x7fff) {
raw_val = (double)0x7fff;
}
if (raw_val < (double)-0x7fff) {
raw_val = (double)-0x7fff;
}
*(chan->dcm_curr_dc) = tmpdc;
}
// update value
*(chan->dcm_raw_val) = (int32_t)raw_val;
// set output data
EC_WRITE_BIT(&pd[chan->set_count_pdo_os], chan->set_count_pdo_bp, *(chan->set_raw_count));
EC_WRITE_BIT(&pd[chan->ena_latch_ext_pos_pdo_os], chan->ena_latch_ext_pos_pdo_bp, *(chan->ena_latch_ext_pos));
EC_WRITE_BIT(&pd[chan->ena_latch_ext_neg_pdo_os], chan->ena_latch_ext_neg_pdo_bp, *(chan->ena_latch_ext_neg));
EC_WRITE_S16(&pd[chan->set_count_val_pdo_os], *(chan->set_raw_count_val));
EC_WRITE_BIT(&pd[chan->dcm_ena_pdo_os], chan->dcm_ena_pdo_bp, *(chan->dcm_enable));
EC_WRITE_BIT(&pd[chan->dcm_reset_pdo_os], chan->dcm_reset_pdo_bp, *(chan->dcm_reset));
EC_WRITE_BIT(&pd[chan->dcm_reduce_torque_pdo_os], chan->dcm_reduce_torque_pdo_bp, *(chan->dcm_reduce_torque));
EC_WRITE_S16(&pd[chan->dcm_velo_pdo_os], (int16_t)raw_val);
}
}
void lcec_el7041_1000_write(struct lcec_slave *s, long period) {
lcec_master_t *m = s->master;
lcec_el7041_1000_data_t *hd = (lcec_el7041_1000_data_t *) s->hal_data;
uint8_t *pd = m->process_data;
double tmpval, tmpdc, raw_val;
int enable_on_edge;
// set outputs
// check for enable edge
enable_on_edge = *(hd->dcm_enable) && !hd->last_dcm_enable;
hd->last_dcm_enable = *(hd->dcm_enable);
// check for autoreset
if (enable_on_edge && hd->internal_fault) {
hd->fault_reset_retry = 1;
hd->fault_reset_state = 1;
hd->fault_reset_cycle = 0;
}
// validate duty cycle limits, both limits must be between
// 0.0 and 1.0 (inclusive) and max must be greater then min
if (*(hd->dcm_max_dc) > 1.0) {
*(hd->dcm_max_dc) = 1.0;
}
if (*(hd->dcm_min_dc) > *(hd->dcm_max_dc)) {
*(hd->dcm_min_dc) = *(hd->dcm_max_dc);
}
if (*(hd->dcm_min_dc) < -1.0) {
*(hd->dcm_min_dc) = -1.0;
}
if (*(hd->dcm_max_dc) < *(hd->dcm_min_dc)) {
*(hd->dcm_max_dc) = *(hd->dcm_min_dc);
}
// do scale calcs only when scale changes
if (*(hd->dcm_scale) != hd->dcm_old_scale) {
// validate the new scale value
if ((*(hd->dcm_scale) < 1e-20) && (*(hd->dcm_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(hd->dcm_scale) = 1.0;
}
// get ready to detect future scale changes
hd->dcm_old_scale = *(hd->dcm_scale);
// we will need the reciprocal
hd->dcm_scale_recip = 1.0 / *(hd->dcm_scale);
}
// get command
tmpval = *(hd->dcm_value);
if (*(hd->dcm_absmode) && (tmpval < 0)) {
tmpval = -tmpval;
}
// convert value command to duty cycle
tmpdc = tmpval * hd->dcm_scale_recip + *(hd->dcm_offset);
if (tmpdc < *(hd->dcm_min_dc)) {
tmpdc = *(hd->dcm_min_dc);
}
if (tmpdc > *(hd->dcm_max_dc)) {
tmpdc = *(hd->dcm_max_dc);
}
// set output values
if (*(hd->dcm_enable) == 0) {
raw_val = 0;
*(hd->dcm_curr_dc) = 0;
} else {
raw_val = (double)0x7fff * tmpdc;
if (raw_val > (double)0x7fff) {
raw_val = (double)0x7fff;
}
if (raw_val < (double)-0x7fff) {
raw_val = (double)-0x7fff;
}
*(hd->dcm_curr_dc) = tmpdc;
}
// update value
*(hd->dcm_raw_val) = (int32_t)raw_val;
// fault reset
if (*(hd->fault_reset)) {
*(hd->dcm_reset) = 1;
}
if (hd->fault_reset_retry > 0) {
if (hd->fault_reset_state) {
*(hd->dcm_reset) = 1;
}
} else {
if (*(hd->dcm_enable)) {
*(hd->dcm_reset) = 0;
}
*(hd->fault_reset) = 0;
}
// set output data
EC_WRITE_BIT(&pd[hd->set_count_pdo_os], hd->set_count_pdo_bp, *(hd->set_raw_count));
EC_WRITE_BIT(&pd[hd->ena_latch_c_pdo_os], hd->ena_latch_c_pdo_bp, *(hd->ena_latch_c));
EC_WRITE_BIT(&pd[hd->ena_latch_ext_pos_pdo_os], hd->ena_latch_ext_pos_pdo_bp, *(hd->ena_latch_ext_pos));
EC_WRITE_BIT(&pd[hd->ena_latch_ext_neg_pdo_os], hd->ena_latch_ext_neg_pdo_bp, *(hd->ena_latch_ext_neg));
EC_WRITE_S16(&pd[hd->set_count_val_pdo_os], *(hd->set_raw_count_val));
EC_WRITE_BIT(&pd[hd->dcm_ena_pdo_os], hd->dcm_ena_pdo_bp, *(hd->dcm_enable));
EC_WRITE_BIT(&pd[hd->dcm_reset_pdo_os], hd->dcm_reset_pdo_bp, *(hd->dcm_reset));
EC_WRITE_BIT(&pd[hd->dcm_reduce_torque_pdo_os], hd->dcm_reduce_torque_pdo_bp, *(hd->dcm_reduce_torque));
EC_WRITE_S16(&pd[hd->dcm_velo_pdo_os], (int16_t)raw_val);
}
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;
}
}
int uei_ethercat_initialize (void)
{
uint8_t *data;
ec_pdo_entry_reg_t rw_pdos[] = {
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off, NULL},
{0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t pv_pdos[] = {
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATUS, status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATE, state_off+1, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t el_pdos[] = {
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off+2, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
master = ecrt_request_master(0);
if (!master){
printf("Could not request master!\n");
return -1;
}
domain = ecrt_master_create_domain(master);
if (!domain) {
printf("Could not create domain!\n");
return -1;
}
printf("Created Domain\n");
if (!(rx_controller = ecrt_master_slave_config(master,RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
fprintf(stderr, "Failed to get slave configuration for Reaction Wheel controller!\n");
return -1;
}
// if (!(pv_controller = ecrt_master_slave_config(master,PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Pivot Motor controller!\n");
// return -1;
// }
// if (!(el_controller = ecrt_master_slave_config(master,EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Elevation Motor controller!\n");
// return -1;
// }
if (ecrt_slave_config_pdos(rx_controller, EC_END, copley_pdo_syncs)) {
perror("ecrt_slave_config_pdos() failed for RX controller.");
ecrt_release_master(master);
return 3;
}
// if (ecrt_slave_config_pdos(pv_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Pivot controller.");
// ecrt_release_master(master);
// return 3;
// }
// if (ecrt_slave_config_pdos(el_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Elevation controller.");
// ecrt_release_master(master);
// return 3;
// }
/// Register the PDO list and variable mappings
// if (ecrt_domain_reg_pdo_entry_list(domain, rw_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for reaction wheel!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, pv_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for pivot motor!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, el_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for Elevation motor!");
// ecrt_release_master(master);
// return -1;
// }
state_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_DRIVE_STATE, domain, NULL);
current_val_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_CURRENT_LOOP_VAL, domain, NULL);
// configure SYNC signals for this slave
ecrt_slave_config_dc(rx_controller, 0, 1000000000ll / 100, 4400000, 0, 0);
printf("Set Master/Slave Configuration\n");
if (ecrt_master_activate(master) < 0) {
printf("Could not activate master!\n");
return -1;
}
if (!(data = ecrt_domain_data(domain))) {
perror("ecrt_domain_data() failed!");
ecrt_release_master(master);
return -1;
}
ethercat_set_offsets(&rx_controller_state, data, 0);
// ethercat_set_offsets(&pv_controller_state, data, 1);
// ethercat_set_offsets(&el_controller_state, data, 2);
check_domain1_state();
check_master_state();
printf("Data: %p\t Current: %p\t State: %p\n", data, rx_controller_state.current_val, rx_controller_state.amp_state);
ecrt_master_receive(master);
ecrt_domain_process(domain);
EC_WRITE_S16(data + current_val_off[0], 0);
EC_WRITE_U16(data + state_off[0], ECAT_STATE_DISABLED);
ecrt_domain_queue(domain);
ecrt_master_send(master);
check_domain1_state();
check_master_state();
return 0;
}