From 5234c7031b863eb27a0ed23ac5a401d678da6941 Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Sun, 8 Aug 2021 14:03:37 +0200 Subject: [PATCH] added current reading --- .../Motor_Master/EtherCAT_SlaveDef.h | 27 +++++------ .../Motor_Master/Motor_Master/bldc.c | 29 ------------ .../Motor_Master/Motor_Master/bldc.h | 6 --- .../Motor_Master/Motor_Master/configuration.h | 5 ++ .../Motor_Master/Motor_Master/interrupts.h | 44 +---------------- .../Motor_Master/Motor_Master/main.c | 47 +++++++++++++++++-- 6 files changed, 61 insertions(+), 97 deletions(-) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h index 41d46db..8cb4e8e 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -129,10 +129,10 @@ static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1); void update_telemetry(void) { - //inline int16_t convert_to_mA(volatile float32_t current_PU) - //{ - //return (int16_t)(current_PU*1000.0f); - //} + inline int16_t convert_to_mA(volatile float32_t current_PU) + { + return (int16_t)(current_PU*1000.0f); + } // //*M1_Status = 0; //*M1_Mode = 0; @@ -141,11 +141,10 @@ void update_telemetry(void) *M1_Joint_rel_position = Motor1.motor_status.Num_Steps; //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); - *M1_Motor_current_bus = COMMUTATION_PATTERN[Motor1.motor_status.currentHallPattern + - Motor1.motor_setpoints.directionOffset]; - //*M1_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[2])+1); - //*M1_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[3]); - //*M1_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[3])+1); + *M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus); + *M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A); + *M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B); + *M1_Motor_currentPhC = convert_to_mA(Motor1.Iphase_pu.C); *M1_Motor__hallState = Motor1.motor_status.currentHallPattern; *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle; // @@ -153,12 +152,10 @@ void update_telemetry(void) *M2_Joint_rel_position = Motor2.motor_status.Num_Steps; //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); - *M2_Motor_current_bus = COMMUTATION_PATTERN[Motor2.motor_status.currentHallPattern + - Motor2.motor_setpoints.directionOffset]; - //*M1_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[2]); - //*M1_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[2])+1); - //*M1_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[3]); - //*M1_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[3])+1); + *M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus); + *M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A); + *M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B); + *M2_Motor_currentPhC = convert_to_mA(Motor2.Iphase_pu.C); *M2_Motor__hallState = Motor2.motor_status.currentHallPattern; *M2_Motor_dutyCycle = Motor2.motor_status.duty_cycle; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index b029f46..f606c6c 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -158,35 +158,6 @@ void exec_commutation(BLDCMotor_t *motor) //toc_port(DEBUG_2_PORT); } -void process_currents(BLDCMotor_t *motor1, BLDCMotor_t *motor2) -{ - Motor1.timerflags.adc_readings_ready_tic = false; - Motor2.timerflags.adc_readings_ready_tic = false; - ////tic_port(DEBUG_2_PORT); - //volatile int16_t phase_A_current_raw, phase_B_current_raw; - // - ///* Motor 1 */ - //phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A); - //phase_B_current_raw = (adc_res[1] - Motor1.Voffset_lsb.B)*-1; - //// Covert from LSB to PU (A) and filter out small readings - //Motor1.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; - //Motor1.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; - //// i_c = -i_a - i_b because i_a + i_b + i_c = 0 - //Motor1.Iphase_pu.C = -Motor1.Iphase_pu.A - Motor1.Iphase_pu.B; - // - ///* Motor 2 */ - //phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A); - //phase_B_current_raw = (adc_res[3] - Motor2.Voffset_lsb.B)*-1; - //// Covert from LSB to PU (A) and filter out small readings - //Motor2.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; - //Motor2.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; - //// i_c = -i_a - i_b because i_a + i_b + i_c = 0 - //Motor2.Iphase_pu.C = -Motor2.Iphase_pu.A - Motor2.Iphase_pu.B; - - // Set Current Loop Flag - Motor1.timerflags.current_loop_tic = true; - Motor2.timerflags.current_loop_tic = true; -} void calculate_motor_speed(BLDCMotor_t *motor) { diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index 56a3656..285d3a0 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h @@ -69,19 +69,13 @@ static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 5, 6, 4, 9}; volatile BLDCMotor_t Motor1; volatile BLDCMotor_t Motor2; -static uint32_t adc_seq_regs[4] = {0x1806, 0x1807, 0x1808, 0x1809}; -static volatile uint16_t adc_res[4] = {0}; -static volatile bool adc_dma_done = 0; -struct _dma_resource *adc_sram_dma_resource; -struct _dma_resource *adc_dmac_sequence_resource; // ---------------------------------------------------------------------- // functions // ---------------------------------------------------------------------- void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param); void exec_commutation(BLDCMotor_t *motor); -void process_currents(BLDCMotor_t *motor1, BLDCMotor_t *motor2); static void select_active_phase(BLDCMotor_t *Motor, const uint8_t hall_state); static void calculate_motor_speed(BLDCMotor_t *motor); static void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float32_t speedRef); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/configuration.h b/2_Motor_Master/Motor_Master/Motor_Master/configuration.h index 1818dce..8a1858a 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/configuration.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/configuration.h @@ -18,6 +18,11 @@ #define DMAC_CHANNEL_ADC_SEQ 2U #define DMAC_CHANNEL_ADC_SRAM 3U +const uint32_t adc_seq_regs[4] = {0x1806, 0x1807, 0x1808, 0x1809}; +volatile uint16_t adc_res[4] = {0}; +struct _dma_resource *adc_sram_dma_resource; +struct _dma_resource *adc_dmac_sequence_resource; + inline static void configure_tcc_pwm(void) { /* TCC0 */ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index a9f39cc..c650168 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -15,53 +15,13 @@ // ---------------------------------------------------------------------- static void pwm_cb(const struct pwm_descriptor *const descr) { - //tic_port(DEBUG_2_PORT); - volatile uint8_t x = 1; + } void adc_sram_dma_callback(struct _dma_resource *adc_dma_res) { - volatile uint8_t x = 1; Motor1.timerflags.adc_readings_ready_tic = true; - //tic_port(DEBUG_2_PORT); - //Motor1.timerflags.adc_readings_ready_tic = true; - //tic_port(DEBUG_2_PORT); - - //toc_port(DEBUG_2_PORT); - //tic_port(DEBUG_3_PORT); - //tic_port(DEBUG_3_PORT); - - //volatile int16_t phase_A_current_raw; - //volatile int16_t phase_B_current_raw; - // - ///* Motor 1 */ - //phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A); - //phase_B_current_raw = (adc_res[1] - Motor1.Voffset_lsb.B)*-1; - //// Covert from LSB to PU (A) and filter out small readings - //Motor1.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; - //Motor1.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; - //// i_c = -i_a - i_b because i_a + i_b + i_c = 0 - //Motor1.Iphase_pu.C = -Motor1.Iphase_pu.A - Motor1.Iphase_pu.B; - - /* Motor 2 */ - //phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A); - //phase_B_current_raw = (adc_res[3] - Motor2.Voffset_lsb.B)*-1; - //Motor2.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; - //Motor2.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; - //// i_c = -i_a - i_b because i_a + i_b + i_c = 0 - //Motor2.Iphase_pu.C = -Motor2.Iphase_pu.A - Motor2.Iphase_pu.B; - // - ///* Motor 3 */ - //phase_A_current_raw = (adc_res[4] - Motor3.Voffset_lsb.A); - //phase_B_current_raw = (adc_res[5] - Motor3.Voffset_lsb.B)*-1; - //Motor3.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; - //Motor3.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; - //// i_c = -i_a - i_b because i_a + i_b + i_c = 0 - //Motor3.Iphase_pu.C = -Motor3.Iphase_pu.A - Motor3.Iphase_pu.B; - - // Set Current Loop Flag - //Motor1.timerflags.current_loop_tic = true; - //toc_port(DEBUG_3_PORT); + Motor2.timerflags.adc_readings_ready_tic = true; } diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 29884d2..06504e7 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -11,13 +11,49 @@ #include "interrupts.h" #include "statemachine.h" + +void process_currents() +{ + Motor1.timerflags.adc_readings_ready_tic = false; + Motor2.timerflags.adc_readings_ready_tic = false; + + //////tic_port(DEBUG_2_PORT); + volatile int16_t phase_A_current_raw, phase_B_current_raw; + + /* Motor 1 */ + phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A); + phase_B_current_raw = (adc_res[1] - Motor1.Voffset_lsb.B)*-1; + // Covert from LSB to PU (A) and filter out small readings + Motor1.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; + Motor1.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; + // i_c = -i_a - i_b because i_a + i_b + i_c = 0 + Motor1.Iphase_pu.C = -Motor1.Iphase_pu.A - Motor1.Iphase_pu.B; + + /* Motor 2 */ + phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A); + phase_B_current_raw = (adc_res[3] - Motor2.Voffset_lsb.B)*-1; + // Covert from LSB to PU (A) and filter out small readings + Motor2.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU; + Motor2.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU; + // i_c = -i_a - i_b because i_a + i_b + i_c = 0 + Motor2.Iphase_pu.C = -Motor2.Iphase_pu.A - Motor2.Iphase_pu.B; + + // Set Current Loop Flag + Motor1.timerflags.current_loop_tic = true; + Motor2.timerflags.current_loop_tic = true; +} + /** * Example of using TIMER_0. */ static void One_ms_cycle_callback(const struct timer_task *const timer_task) { - if ((ecat_state == wait2)|(ecat_state == wait)) ecat_state= write_fifo; - run_ECAT =true; + if ((ecat_state == wait2)|(ecat_state == wait)) + { + ecat_state= write_fifo; + run_ECAT =true; + } + update_telemetry(); //run_ECAT = true; } @@ -137,9 +173,10 @@ int main(void) //x += 1; //comms_check(); //update_setpoints(); - if (run_ECAT) ECAT_STATE_MACHINE(); - if (Motor1.timerflags.adc_readings_ready_tic) process_currents(&Motor1, &Motor2); - if (Motor1.timerflags.current_loop_tic) CONTROLLER_StateMachine(); + if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} + if (Motor1.timerflags.current_loop_tic) {CONTROLLER_StateMachine();} + if (run_ECAT) {ECAT_STATE_MACHINE();} + } }