示例#1
0
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);
}
示例#2
0
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);
}
示例#4
0
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;
    }

}
示例#5
0
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;
}