diff --git a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart index 999de40..94e76c3 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart +++ b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart @@ -2207,7 +2207,7 @@ drivers: tcc_arch_dbgrun: false tcc_arch_evact0: Event action disabled tcc_arch_evact1: Event action disabled - tcc_arch_lupd: true + tcc_arch_lupd: false tcc_arch_mcei0: false tcc_arch_mcei1: false tcc_arch_mcei2: false @@ -2227,7 +2227,7 @@ drivers: tcc_arch_trgeo: false tcc_arch_wave_duty_val: 500 tcc_arch_wave_per_val: 48 - tcc_arch_wavegen: Single-slope PWM + tcc_arch_wavegen: Dual-slope, interrupt/event at ZERO (DSBOTTOM) tcc_per: 10000 tcc_prescaler: Divide by 2 timer_event_control: true 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 1448fd4..41d46db 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -127,41 +127,54 @@ static volatile int16_t *M4_Max_velocity = ((int16_t *)&QSPI_rx_buffer[14]+1) static volatile int16_t *M4_Max_current = ((int16_t *)&QSPI_rx_buffer[15]); static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1); -//void update_telemetry(void) -//{ +void update_telemetry(void) +{ //inline int16_t convert_to_mA(volatile float32_t current_PU) //{ //return (int16_t)(current_PU*1000.0f); //} // - //*state = applicationStatus.currentstate; - //*status = applicationStatus.fault; - //*motor_rel_position = Motor1.motor_status.Num_Steps; - //*motor_abs_position = 0; - //*motor_dutyCycle = Motor1.motor_status.duty_cycle; - //*motor_speed = (int16_t)Motor1.motor_status.calc_rpm; - //*motor_torque = 0; - //*motor_currentPHA = convert_to_mA(Motor1.Iphase_pu.A); - //*motor_currentPHB = convert_to_mA(Motor1.Iphase_pu.B); - //*motor_currentPHC = convert_to_mA(Motor1.Iphase_pu.C); - //*motor_currentBUS = convert_to_mA(Motor1.Iphase_pu.Bus); - //*hall_state = Motor1.motor_status.currentHallPattern; - //*Spare_byte1 = Motor1.motor_setpoints.directionOffset; - //*Spare_1 = Motor1.motor_status.actualDirection; - ////*Spare_2 = 0; -//} -// + //*M1_Status = 0; + //*M1_Mode = 0; + + /* Motor 1 */ + *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__hallState = Motor1.motor_status.currentHallPattern; + *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle; + // + /* Motor 2 */ + *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__hallState = Motor2.motor_status.currentHallPattern; + *M2_Motor_dutyCycle = Motor2.motor_status.duty_cycle; + +} + void update_setpoints(void) { - volatile uint8_t a = *M1_Control_mode; - volatile uint8_t b = *M1_Control_set; - volatile int16_t c = *M1_Desired_pos; - volatile int16_t d = *M1_Desired_speed; - volatile int16_t e = *M1_Desired_current; - volatile int16_t f = *M1_Max_pos; - volatile int16_t g = *M1_Max_velocity; - volatile int16_t h = *M1_Max_current; - volatile int16_t i = *M1_Spare; + //volatile uint8_t a = *M1_Control_mode; + //volatile uint8_t b = *M1_Control_set; + //volatile int16_t c = *M1_Desired_pos; + //volatile int16_t d = *M1_Desired_speed; + //volatile int16_t e = *M1_Desired_current; + //volatile int16_t f = *M1_Max_pos; + //volatile int16_t g = *M1_Max_velocity; + //volatile int16_t h = *M1_Max_current; + //volatile int16_t i = *M1_Spare; //inline float32_t convert_int_to_PU(volatile int16_t input) //{ //return ((float32_t)(input/1000.0f)); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj index f3cccbf..fa8614f 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -29,14 +29,14 @@ 0 - + - + @@ -301,6 +301,7 @@ C:\Users\Nick-XMG\Documents\github\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis + C:\Users\ge37vez\Documents\Git Repos\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis %24(ProjectDir)\Device_Startup @@ -413,11 +414,11 @@ ../hri - Optimize (-O1) + Optimize more (-O2) True Maximum (-g3) True - -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16 + -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16 True @@ -427,8 +428,9 @@ - %24(ProjectDir)\Device_Startup C:\Users\Nick-XMG\Documents\github\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis + C:\Users\ge37vez\Documents\Git Repos\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis + %24(ProjectDir)\Device_Startup True @@ -518,6 +520,9 @@ compile + + compile + compile @@ -572,6 +577,9 @@ compile + + compile + compile @@ -1052,6 +1060,12 @@ compile + + compile + + + compile + diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index be2c5a1..78ce2af 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -5,3 +5,341 @@ * Author: Nick-XMG */ +#include "bldc.h" + +#include "statemachine.h" +#include "utilities.h" + + +void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param) +{ + // ---------------------------------------------------------------------- + // Initialize all voltage and current objects, variables and helpers: + motor->motor_param = motor_param; + motor->motor_status.actualDirection = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_status.duty_cycle = 150; + motor->motor_status.calc_rpm = 0; + motor->motor_status.Num_Steps = 0; + motor->motor_status.cur_comm_step = 0; + motor->motor_status.currentHallPattern = 1; + motor->motor_status.nextHallPattern = 3; + motor->motor_setpoints.directionOffset = 0; + + motor->Iphase_pu.A = 0; + motor->Iphase_pu.B = 0; + motor->Iphase_pu.C = 0; + motor->Iphase_pu.Bus = 0; + motor->Voffset_lsb.A = 0; + motor->Voffset_lsb.B = 0; + motor->timerflags.pwm_cycle_tic = false; + motor->timerflags.control_loop_tic = false; + motor->timerflags.motor_telemetry_flag = false; + motor->timerflags.control_loop_tic = false; + motor->timerflags.adc_readings_ready_tic = false; + + motor->regulation_loop_count = 0; + + + motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V; + motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V; + + motor->motor_setpoints.desired_torque = 0.0; + motor->motor_setpoints.desired_speed = 0; + motor->motor_setpoints.desired_position = 0; + motor->motor_setpoints.max_current = 0.0; + motor->motor_setpoints.max_torque = 0.0; + motor->motor_setpoints.max_velocity = 0; + + //// ------------------------------------------------------------------------------ + //// pi current control init + //// ------------------------------------------------------------------------------ + + //// ------------------------------------------------------------------------------ + //// pi current control init + //// ------------------------------------------------------------------------------ + PI_objectInit(&motor->controllers.Pi_Idc); + float32_t motorLs_H = motor_param->motor_LQ_H * 2.0f; + float32_t motorRs_OHM = motor_param->motor_RS_Ohm * 2.0f; + motor->controllers.Pi_Idc.Kp = PI_calcKp(motorLs_H, DEVICE_SHUNT_CURRENT_A, DEVICE_DC_VOLTAGE_V, DEVICE_ISR_PERIOD_Sec); + motor->controllers.Pi_Idc.Ki = PI_calcKi(motorRs_OHM, motorLs_H, DEVICE_ISR_PERIOD_Sec); + + //// ------------------------------------------------------------------------------ + //// pi velocity control init + //// ------------------------------------------------------------------------------ + PID_objectInit(&motor->controllers.Pid_Speed); + + /* V only Control Gains */ + //motor->controllers.Pid_Speed.Kp = 0.005f; + //motor->controllers.Pid_Speed.Ki = 0.01f; + + /* VI Control Gains */ + //motor->controllers.Pid_Speed.Kp = 0.0003f; + //motor->controllers.Pid_Speed.Ki = 0.001f; + motor->controllers.Pid_Speed.Kp = 0.0005f; + motor->controllers.Pid_Speed.Ki = 0.0f; + motor->controllers.Pid_Speed.Kd = 0.0001f; + + //motor->controllers.Pid_Speed.Ki = 0.0001f; + motor->controllers.Pid_Speed.OutMax_pu = (motor_param->motor_Max_Current_IDC_A); + motor->controllers.Pid_Speed.OutMin_pu = -(motor_param->motor_Max_Current_IDC_A); + + //// ------------------------------------------------------------------------------ + //// pi position control init + //// ------------------------------------------------------------------------------ + PI_objectInit(&motor->controllers.Pi_Pos); + motor->controllers.Pi_Pos.Kp = 40.0f; + motor->controllers.Pi_Pos.Ki = 0.0f; + motor->controllers.Pi_Pos.OutMax_pu = (motor_param->motor_Max_Spd_RPM); + motor->controllers.Pi_Pos.OutMin_pu = -(motor_param->motor_Max_Spd_RPM); + +} + +void exec_commutation(BLDCMotor_t *motor) +{ + // ---------------------------------------------------------------------- + // Read Motor Hall Sensors + // ---------------------------------------------------------------------- + //tic_port(DEBUG_2_PORT); + //motor->motor_status.currentHallPattern = readM1Hall(); + motor->motor_status.currentHallPattern = motor->readHall(); + + // ---------------------------------------------------------------------- + // Multi Motor Register Masking + // ---------------------------------------------------------------------- + //tic_port(DEBUG_2_PORT); + //volatile uint16_t temp_M1 = COMMUTATION_PATTERN_M1[Motor1.motor_status.currentHallPattern + Motor1.motor_setpoints.directionOffset]; + //volatile uint16_t temp_M2 = COMMUTATION_PATTERN_M2[Motor2.motor_status.currentHallPattern + oh ]; + volatile uint16_t temp_M1 = COMMUTATION_PATTERN[motor->motor_status.currentHallPattern + + motor->motor_setpoints.directionOffset]; + // ---------------------------------------------------------------------- + // Set Pattern Buffers + // ---------------------------------------------------------------------- + hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, (hri_tcc_pattbuf_reg_t)temp_M1); + //(Tcc *)(motor->motor_param.pwm_desc->device.hw)->PATTBUF.reg = temp_M1; + //TCC0->PATTBUF.reg = (uint16_t)temp_M2; + //TCC1->PATTBUF.reg = (uint16_t)temp_M1; + + // ---------------------------------------------------------------------- + // Set Calculated Duty Cycles + // ---------------------------------------------------------------------- + hri_tcc_write_CCBUF_CCBUF_bf(motor->motor_param->pwm_desc->device.hw, 0, motor->motor_status.duty_cycle); + //SetM1DutyCycle(Motor1.motor_status.duty_cycle); + //SetM2DutyCycle(Motor1.motor_status.duty_cycle); + + motor->motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[motor->motor_status.currentHallPattern]; + volatile int8_t step_change1 = motor->motor_status.cur_comm_step - motor->motor_status.prev_comm_step; + + switch(step_change1) + { + case 1: case -5: + motor->motor_status.Num_Steps = motor->motor_status.Num_Steps+1; + motor->motor_status.actualDirection = CW; + //Motor1.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET; + break; + case -1: case 5: + motor->motor_status.Num_Steps = motor->motor_status.Num_Steps-1; + motor->motor_status.actualDirection = CCW; + //Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET; + break; + default: + // do nothing + break; + } + motor->motor_status.prev_comm_step = motor->motor_status.cur_comm_step; + //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) +{ + //tic_port(DEBUG_2_PORT); + volatile uint32_t temp_rpm = 0; + hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/ + volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1); + if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) { + Motor1.motor_status.calc_rpm = 0; + } else { + //uint32_t test = (SPEEDFACTOR, period_after_capture); + //temp_rpm = SPEEDFACTOR / period_after_capture; + //tic_port(DEBUG_3_PORT); + temp_rpm = HZ_TO_RPM / (period_after_capture*motor->motor_param->motor_commutationStates); + //temp_rpm = HZ_TO_RPM * period_after_capture; + //toc_port(DEBUG_3_PORT); + } + + if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ + { + //*motor_speed = -1* Motor1.calc_rpm; + temp_rpm = -1 * temp_rpm; + } else { + //*motor_speed = Motor1.calc_rpm; + temp_rpm = temp_rpm; + } + + Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; + //toc_port(DEBUG_2_PORT); + + //#ifdef AVERAGE_SPEED_MEASURE + //// To avoid noise an average is realized on 8 samples + //speed_average += temp_rpm; + //if(count >= n_SAMPLE) + //{ + //count = 1; + //Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32 + ////Motor1.motor_status.calc_rpm = (speed_average); // divide by 32 + //speed_average = 0; + ////*Spare_byte1 = motorState.actualDirection; + //if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ + //{ + ////*motor_speed = -1* Motor1.calc_rpm; + //Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm; + //} else { + ////*motor_speed = Motor1.calc_rpm; + //Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm; + //} + //return; + //} + //else count++; + //#else + //Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; + //#endif + //toc_port(DEBUG_2_PORT); +} + + +//------------------------------------------------------------------------------ +// pi current control +//------------------------------------------------------------------------------ +void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t curRef) +{ + motor->controllers.Pi_Idc.Fbk_pu = f_clamp(curfbk, -DEVICE_SHUNT_CURRENT_A, DEVICE_SHUNT_CURRENT_A); // Clamped to max current sensor readingspeedfbk; + motor->controllers.Pi_Idc.Ref_pu = f_clamp(curRef, -motor->motor_param->motor_Max_Current_IDC_A, + motor->motor_param->motor_Max_Current_IDC_A); // Clamp desired to Motor Max Current i_ref_clamped; + + //*Spare_1 = (int16_t)(motor->controllers.Pi_Idc.Ref_pu * 1000); // Send Req Current to TC + + motor->controllers.Pi_Idc.OutMax_pu = motor->VdcBus_pu; + motor->controllers.Pi_Idc.OutMin_pu = -motor->VdcBus_pu; + PI_run_series(&motor->controllers.Pi_Idc); + + if(motor->controllers.Pi_Idc.Out_pu > 0) { + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; + } else { + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; + } + + volatile float32_t duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu)); + motor->motor_status.duty_cycle = (uint16_t)f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); + // Remove Low duty cycle values + //if(duty_cycle < 80.0) motor->duty_cycle = (uint16_t)0; + //else motor->duty_cycle = (uint16_t)duty_cycle; + +} + +//// ------------------------------------------------------------------------------ +// Speed Control: Input in RPM units (int16_t) +//------------------------------------------------------------------------------ +void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float32_t speedRef) +{ + //tic_port(DEBUG_3_PORT); + motor->controllers.Pid_Speed.Fbk_pu = speedfbk; + motor->controllers.Pid_Speed.Ref_pu = f_clamp(speedRef, -motor->motor_param->motor_Max_Spd_RPM, + motor->motor_param->motor_Max_Spd_RPM); + + if (applicationStatus.currentstate == MOTOR_V_CTRL_STATE) + { + /* Output Pu in Volts (PWM %) */ + motor->controllers.Pid_Speed.OutMax_pu = motor->VdcBus_pu; + motor->controllers.Pid_Speed.OutMin_pu = -motor->VdcBus_pu; + + PID_run_parallel(&motor->controllers.Pid_Speed); + + if(motor->controllers.Pid_Speed.Out_pu > 0) { + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; + } else { + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; + } + /* Process unit is Volts */ + volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu)); + volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); + motor->motor_status.duty_cycle = (uint16_t)duty_cycle; + } else { + /* Pu in Current (Amps) */ + motor->controllers.Pid_Speed.OutMax_pu = (motor->motor_param->motor_Max_Current_IDC_A); + motor->controllers.Pid_Speed.OutMin_pu = -(motor->motor_param->motor_Max_Current_IDC_A); + PID_run_parallel(&motor->controllers.Pid_Speed); + motor->controllers.Pi_Idc.Ref_pu = motor->controllers.Pid_Speed.Out_pu; + } + //toc_port(DEBUG_3_PORT); +} + +//// ------------------------------------------------------------------------------ +// Position Control:Input in Hall step units (int16_t) +//------------------------------------------------------------------------------ +void BLDC_runPosCntl(BLDCMotor_t *motor, const int16_t posfbk, const int16_t posRef) +{ + /* Output Pu in RPM */ + motor->controllers.Pi_Pos.OutMax_pu = motor->motor_param->motor_Max_Spd_RPM; + motor->controllers.Pi_Pos.OutMin_pu = -motor->motor_param->motor_Max_Spd_RPM; + motor->controllers.Pi_Pos.Fbk_pu = posfbk; + motor->controllers.Pi_Pos.Ref_pu = posRef; + PI_run_series(&motor->controllers.Pi_Pos); + motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu; +} + +uint8_t readHallSensorM1(void) +{ + volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(M1_HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(M1_HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); + +} + +uint8_t readHallSensorM2(void) +{ + volatile uint8_t a = gpio_get_pin_level(M2_HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(M2_HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(M2_HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); +} \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index d86572f..9ffba42 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h @@ -5,7 +5,6 @@ * Author: Nick-XMG */ - #ifndef BLDC_H_ #define BLDC_H_ @@ -13,7 +12,7 @@ // header files // ---------------------------------------------------------------------- #include "atmel_start.h" -#include "arm_math.h" +#include "bldc_types.h" #include "motorparameters.h" #define PWM_TOP (1000) @@ -25,68 +24,78 @@ #define CCW (1) //ABC #define DIRECTION_CCW_OFFSET (8) //CBA +/* if the Hall sensor reads 0x0 or 0x7 that means either one of the hall sensor is damaged or disconnected*/ +#define INVALID_HALL_0 (0U) +#define INVALID_HALL_7 (7U) -static uint32_t adc_seq_regs[5] = {0x1800, 0x1806, 0x1807, 0x1808, 0x1809}; -static volatile uint16_t adc_res[5] = {0}; +// ---------------------------------------------------------------------- +// ADC Parameters +// ---------------------------------------------------------------------- +#define ADC_VOLTAGE_REFERENCE (3.3f) +#define ADC_RESOLUTION (12) +#define ADC_MAX_COUNTS (1<Group[M2_HALL_A_GROUP].IN.reg & M2_HALL_A_PORT)>>(M2_HALL_A_LSR)); + ////motor_read = (motor_read & M2_HALL_B_MASK) | (uint8_t)((PORT->Group[M2_HALL_B_GROUP].IN.reg & M2_HALL_B_PORT)>>(M2_HALL_B_LSR)); + ////motor_read = (motor_read & M2_HALL_C_MASK) | (uint8_t)((PORT->Group[M2_HALL_C_GROUP].IN.reg & M2_HALL_C_PORT)>>(M2_HALL_C_LSR)); + //// + ////return motor_read; + // + //Motor1. + // + //volatile uint8_t a = gpio_get_pin_level(motor->motor_param->hallPins.hallA); + ////volatile uint8_t b = gpio_get_pin_level(motor->motor_param->hallsensors[1]); + ////volatile uint8_t c = gpio_get_pin_level(motor->motor_param->hallsensors[2]); + //// + ////return ((a << 2) | + ////(b << 1) | + ////(c << 0)); +//} #endif /* BLDC_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h new file mode 100644 index 0000000..3085cd6 --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h @@ -0,0 +1,109 @@ +/* + * bldc_types.h + * + * Created: 8/5/2021 9:40:45 AM + * Author: ge37vez + */ + + +#ifndef BLDC_TYPES_H_ +#define BLDC_TYPES_H_ + +#include "atmel_start.h" +#include "control.h" +#include "motorparameters.h" + +// ---------------------------------------------------------------------- +// function Pointer +// ---------------------------------------------------------------------- +typedef uint8_t (*readHallSensor)(void); + +// ---------------------------------------------------------------------- +// Type Definitions +// ---------------------------------------------------------------------- +volatile typedef struct +{ + volatile float32_t A; // Phase A + volatile float32_t B; // Phase B + volatile float32_t C; // Phase C + volatile float32_t Bus; // Currently Active Phase Current +} MOTOR_3PHASES_t; + +volatile typedef struct +{ + volatile int16_t A; // Phase A measured offset + volatile int16_t B; // Phase B measured offset +} MOTOR_phase_offset_t; + +volatile typedef struct timerflags +{ + volatile bool pwm_cycle_tic; + volatile bool current_loop_tic; + volatile bool control_loop_tic; + volatile bool adc_readings_ready_tic; + volatile bool motor_telemetry_flag; +} TIMERflags_t; + +volatile typedef struct +{ + volatile PI_t Pi_Idc; + volatile PID_t Pid_Speed; + volatile PI_t Pi_Pos; +} MOTOR_Control_Structs; + +volatile typedef struct +{ + volatile uint8_t desiredDirection; //! The desired direction of rotation. + volatile uint8_t directionOffset; + volatile float32_t desired_torque; + volatile int16_t desired_speed; + volatile int16_t desired_position; + volatile float32_t max_torque; + volatile float32_t max_current; + volatile int16_t max_velocity; +} MOTOR_Setpoints; + +volatile typedef struct +{ + volatile uint8_t actualDirection; //! The actual direction of rotation. + volatile uint16_t duty_cycle; + volatile float32_t calc_rpm; + volatile int32_t* Num_Steps; + /* Hall States */ + volatile uint8_t prevHallPattern; + volatile uint8_t currentHallPattern; + volatile uint8_t nextHallPattern; + /* Commutation State */ + volatile uint8_t cur_comm_step; + volatile uint8_t prev_comm_step; +} MOTOR_Status; + +// ---------------------------------------------------------------------- +// Main BLDC Struct +// ---------------------------------------------------------------------- +volatile typedef struct BLDCmotor +{ + /* Hardware */ + const uint32_t current_sensor_channels[2]; + BLDCMotor_param_t *motor_param; + /* Status */ + MOTOR_Status motor_status; + /* Measured Values */ + volatile MOTOR_3PHASES_t Iphase_pu; + volatile MOTOR_phase_offset_t Voffset_lsb; + volatile float32_t VdcBus_pu; + volatile float32_t VoneByDcBus_pu; + /* Motor Flags */ + volatile TIMERflags_t timerflags; + volatile uint8_t regulation_loop_count; + /* Controllers */ + volatile MOTOR_Control_Structs controllers; + /* Setpoints */ + volatile MOTOR_Setpoints motor_setpoints; + /* Function Pointers*/ + readHallSensor readHall; +} BLDCMotor_t; + + + +#endif /* BLDC_TYPES_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/configuration.h b/2_Motor_Master/Motor_Master/Motor_Master/configuration.h index 7e1061d..1818dce 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/configuration.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/configuration.h @@ -21,40 +21,43 @@ inline static void configure_tcc_pwm(void) { /* TCC0 */ - //hri_tcc_set_WEXCTRL_OTMX_bf(TCC0, 0); + hri_tcc_set_WEXCTRL_OTMX_bf(TCC0, 0x02); - //hri_tcc_set_WAVE_POL0_bit(TCC0); - //hri_tcc_set_WAVE_POL1_bit(TCC0); - //hri_tcc_set_WAVE_POL2_bit(TCC0); - //hri_tcc_set_WAVE_POL3_bit(TCC0); - //hri_tcc_set_WAVE_POL4_bit(TCC0); - //hri_tcc_set_WAVE_POL5_bit(TCC0); - //hri_tcc_write_CC_CC_bf(TCC0, 0, 0); - //hri_tcc_write_CC_CC_bf(TCC0, 1, 0); - //hri_tcc_write_CC_CC_bf(TCC0, 2, 0); - //hri_tcc_write_CC_CC_bf(TCC0, 3, 0); - //hri_tcc_write_CC_CC_bf(TCC0, 4, 0); - //hri_tcc_write_CC_CC_bf(TCC0, 5, 0); + hri_tcc_set_WAVE_POL0_bit(TCC0); + hri_tcc_set_WAVE_POL1_bit(TCC0); + hri_tcc_set_WAVE_POL2_bit(TCC0); + hri_tcc_set_WAVE_POL3_bit(TCC0); + hri_tcc_set_WAVE_POL4_bit(TCC0); + hri_tcc_set_WAVE_POL5_bit(TCC0); + hri_tcc_write_CC_CC_bf(TCC0, 0, 150); + hri_tcc_write_CC_CC_bf(TCC0, 1, 150); + hri_tcc_write_CC_CC_bf(TCC0, 2, 150); + hri_tcc_write_CC_CC_bf(TCC0, 3, 150); + hri_tcc_write_CC_CC_bf(TCC0, 4, 150); + hri_tcc_write_CC_CC_bf(TCC0, 5, 150); hri_tcc_write_PER_reg(TCC0,1200); + //hri_tcc_write_CCBUF_CCBUF_bf(TCC0, 0,250); hri_tcc_write_CTRLA_ENABLE_bit(TCC0, 1 << TCC_CTRLA_ENABLE_Pos); /* TCC1 */ - //hri_tcc_set_WEXCTRL_OTMX_bf(TCC1, 3); - //hri_tcc_write_CC_CC_bf(TCC1, 0, 0); - //hri_tcc_write_CC_CC_bf(TCC1, 1, 0); - //hri_tcc_write_CC_CC_bf(TCC1, 2, 0); - //hri_tcc_write_CC_CC_bf(TCC1, 3, 0); - ////hri_tcc_write_CC_CC_bf(TCC1, 0, 0); + hri_tcc_set_WEXCTRL_OTMX_bf(TCC1, 0x02); + hri_tcc_write_CC_CC_bf(TCC1, 0, 150); + hri_tcc_write_CC_CC_bf(TCC1, 1, 150); + hri_tcc_write_CC_CC_bf(TCC1, 2, 150); + hri_tcc_write_CC_CC_bf(TCC1, 3, 150); + hri_tcc_write_CC_CC_bf(TCC0, 4, 150); + hri_tcc_write_CC_CC_bf(TCC0, 5, 150); // ////pwm_set_parameters(&TCC_PWM, 1000, 250); - //hri_tcc_set_WAVE_POL0_bit(TCC1); - //hri_tcc_set_WAVE_POL1_bit(TCC1); - //hri_tcc_set_WAVE_POL2_bit(TCC1); - //hri_tcc_set_WAVE_POL3_bit(TCC1); - //hri_tcc_set_WAVE_POL4_bit(TCC1); - //hri_tcc_set_WAVE_POL5_bit(TCC1); + hri_tcc_set_WAVE_POL0_bit(TCC1); + hri_tcc_set_WAVE_POL1_bit(TCC1); + hri_tcc_set_WAVE_POL2_bit(TCC1); + hri_tcc_set_WAVE_POL3_bit(TCC1); + hri_tcc_set_WAVE_POL4_bit(TCC1); + hri_tcc_set_WAVE_POL5_bit(TCC1); hri_tcc_write_PER_reg(TCC1,1200); + //hri_tcc_write_CCBUF_CCBUF_bf(TCC1, 0, 250); hri_tcc_clear_CTRLA_ENABLE_bit(TCC1); hri_tcc_write_CTRLA_MSYNC_bit(TCC1, true); //hri_tcc_write_CTRLA_ENABLE_bit(TCC1, 1 << TCC_CTRLA_ENABLE_Pos); /* Enable: enabled */ @@ -90,7 +93,7 @@ inline void adc_dmac_sequence_init() */ _dma_set_source_address(DMAC_CHANNEL_ADC_SEQ, (const void *)adc_seq_regs); _dma_set_destination_address(DMAC_CHANNEL_ADC_SEQ, (const void *)&ADC1->DSEQDATA.reg); - _dma_set_data_amount(DMAC_CHANNEL_ADC_SEQ, 5); + _dma_set_data_amount(DMAC_CHANNEL_ADC_SEQ, 4); _dma_set_next_descriptor(DMAC_CHANNEL_ADC_SEQ, DMAC_CHANNEL_ADC_SEQ); _dma_enable_transaction(DMAC_CHANNEL_ADC_SEQ, false); //_dma_get_channel_resource(&adc_dmac_sequence_resource, DMAC_CHANNEL_ADC_SEQ); @@ -106,7 +109,7 @@ inline void adc_sram_dmac_init() * next descriptor address, data count and Enable the DMAC Channel */ _dma_set_source_address(DMAC_CHANNEL_ADC_SRAM, (const void *)&ADC1->RESULT.reg); _dma_set_destination_address(DMAC_CHANNEL_ADC_SRAM, (const void *)adc_res); - _dma_set_data_amount(DMAC_CHANNEL_ADC_SRAM, 5); + _dma_set_data_amount(DMAC_CHANNEL_ADC_SRAM, 4); _dma_set_irq_state(DMAC_CHANNEL_ADC_SRAM, DMA_TRANSFER_COMPLETE_CB, true); _dma_get_channel_resource(&adc_sram_dma_resource, DMAC_CHANNEL_ADC_SRAM); adc_sram_dma_resource[0].dma_cb.transfer_done = adc_sram_dma_callback; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/control.h b/2_Motor_Master/Motor_Master/Motor_Master/control.h new file mode 100644 index 0000000..3ccc2f2 --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/control.h @@ -0,0 +1,256 @@ +/* + * pi.h + * + * Created: 01/02/2021 21:36:11 + * Author: Nick-XMG + */ +#ifndef CONTROL_H_ +#define CONTROL_H_ + +// ---------------------------------------------------------------------- +// history +// ---------------------------------------------------------------------- +// 01.02.2021 - initial Revision + +// ---------------------------------------------------------------------- +// header files +// ---------------------------------------------------------------------- +#include "arm_math.h" +#include "atmel_start.h" +#include "utilities.h" +// ---------------------------------------------------------------------- +// #defines +// ---------------------------------------------------------------------- +// ---------------------------------------------------------------------- +// global variables +// ---------------------------------------------------------------------- + +#ifdef __cplusplus +extern "C" { + #endif + +/* PI Structure */ +typedef volatile struct +{ + volatile float32_t Kp; // the proportional gain for the PI controller + volatile float32_t Ki; // the integral gain for the PI controller + + volatile float32_t Ref_pu; // the reference value [pu] + volatile float32_t Fbk_pu; // the feedback value [pu] + volatile float32_t Ff_pu; // the feedForward value [pu] + + volatile float32_t OutMin_pu; // the saturation low limit for the controller output [pu] + volatile float32_t OutMax_pu; // the saturation high limit for the controller output [pu] + volatile float32_t Out_pu; // the controller output [pu] + + volatile float32_t Ui; // the integrator value for the PI controller + volatile float32_t error; // the integrator value for the PI controller + volatile bool SatFlag; // flag to signal controller saturation +} PI_t; + +/* PID Structure */ +typedef volatile struct +{ + volatile float32_t Kp; //!< the proportional gain for the PID controller + volatile float32_t Ki; //!< the integral gain for the PID controller + volatile float32_t Kd; //!< the derivative gain for the PID controller + + volatile float32_t Ref_pu; //!< the reference input value + volatile float32_t Fbk_pu; //!< the feedback input value + volatile float32_t Ff_pu; //!< the feedforward input value + + volatile float32_t OutMin_pu; //!< the minimum output value allowed for the PID controller + volatile float32_t OutMax_pu; //!< the maximum output value allowed for the PID controller + volatile float32_t Out_pu; // the controller output [pu] + + volatile float32_t Ui; //!< the integrator start value for the PID controller + volatile float32_t Ud; + volatile float32_t error; // the integrator value for the PI controller + + //FILTER_FO_Handle derFilterHandle; //!< the derivative filter handle + //FILTER_FO_Obj derFilter; //!< the derivative filter object +} PID_t; + + +// ---------------------------------------------------------------------- +// functions +// ---------------------------------------------------------------------- +// function to set default values for the object + +//inline float32_t clamp(const float32_t d, const float32_t min, const float32_t max) { + //const float32_t t = d < min ? min : d; + //return t > max ? max : t; +//} + + +inline void PI_objectInit(volatile PI_t *pPi_obj) +{ + PI_t *obj = (PI_t *)pPi_obj; + + // Function initializes the object with default values + //fix16_t Kp = fix16_from_float32_t(1.0); + obj->Kp = 0.0f; + obj->Ki = 0.0f; + obj->Fbk_pu = 0.0f; + obj->Ref_pu = 0.0f; + obj->Ff_pu = 0.0f; + obj->Out_pu = 0.0f; + obj->OutMax_pu = 0.0f; + obj->OutMin_pu = 0.0f; + obj->Ui = 0.0f; + obj->SatFlag = false; +} + +inline void PID_objectInit(volatile PID_t *pPiD_obj) +{ + PID_t *obj = (PID_t *)pPiD_obj; + + // Function initializes the object with default values + //fix16_t Kp = fix16_from_float32_t(1.0); + obj->Kp = 0.0f; + obj->Ki = 0.0f; + obj->Kd = 0.0f; + obj->Fbk_pu = 0.0f; + obj->Ref_pu = 0.0f; + obj->Ff_pu = 0.0f; + obj->Out_pu = 0.0f; + obj->OutMax_pu = 0.0f; + obj->OutMin_pu = 0.0f; + obj->Ui = 0.0f; + obj->Ud = 0.0f; +} + +// ---------------------------------------------------------------------- +// PI Calculation +// ---------------------------------------------------------------------- +static inline void PI_run_series(volatile PI_t *pPi_obj) +{ + PI_t *obj = (PI_t *)pPi_obj; + volatile float32_t Up; + + // Compute the controller error + obj->error = obj->Ref_pu - obj->Fbk_pu; + + // Compute the proportional term + Up = obj->Kp *obj->error; + + // Compute the integral term in series form and saturate + obj->Ui = f_clamp((obj->Ui + (obj->Ki * Up)), obj->OutMin_pu, obj->OutMax_pu); + + // Saturate the output + obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu); +} + +static inline void PI_run_parallel(volatile PI_t *pPi_obj) +{ + PI_t *obj = (PI_t *)pPi_obj; + volatile float32_t Up; + + // Compute the controller error + obj->error = obj->Ref_pu - obj->Fbk_pu; + + // Compute the proportional term + Up = obj->Kp * obj->error; + + // Compute the integral term in parallel form and saturate + obj->Ui = f_clamp((obj->Ui + (obj->Ki * obj->error)), obj->OutMin_pu, obj->OutMax_pu); + + obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu); // Saturate the output +} // end of PI_run_parallel() function + + +// ---------------------------------------------------------------------- +// PID Calculation +// ---------------------------------------------------------------------- +static inline void PID_run_series(volatile PID_t *pPid_obj) +{ + PID_t *obj = (PID_t *)pPid_obj; + volatile float32_t Up, Ud_tmp; + + // Compute the controller error + obj->error = obj->Ref_pu - obj->Fbk_pu; + + // Compute the proportional term + Up = obj->Kp * obj->error; + + if(obj->Ki>0.0f){ + // Compute the integral term in parallel form and saturate + obj->Ui = f_clamp((obj->Ui + (obj->Ki * Up)), obj->OutMin_pu, obj->OutMax_pu); + } + + Ud_tmp = obj->Kd * obj->Ui; // Compute the derivative term + + obj->Ud = Ud_tmp; // Replace with filter + + obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ud + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu); // Saturate the output + +} + +static inline void PID_run_parallel(volatile PID_t *pPid_obj) +{ + PID_t *obj = (PID_t *)pPid_obj; + volatile float32_t Up, Ud_tmp; + + + // Compute the controller error + obj->error = obj->Ref_pu - obj->Fbk_pu; + + // Compute the proportional term + Up = obj->Kp * obj->error; + + if(obj->Ki>0.0f){ + // Compute the integral term in parallel form and saturate + obj->Ui = f_clamp((obj->Ui + (obj->Ki * obj->error)), obj->OutMin_pu, obj->OutMax_pu); + } + + Ud_tmp = obj->Kd * obj->error; // Compute the derivative term + + obj->Ud = Ud_tmp; // Replace with filter + + obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ud + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu); // Saturate the output +} + + +// ---------------------------------------------------------------------- +// Calculate Current Parameters +// ---------------------------------------------------------------------- + +static inline float32_t PI_calcKp(const float32_t Ls_H, const float32_t deviceCurrent_A, const float32_t deviceVoltage_V, +const float32_t deviceCtrlPeriode_Sec) +{ + // calculation is based on "Betragsoptimum" + // Kp = Ls/(2*tau) + float32_t x1; + float32_t y1; + float32_t Kp; + + // multiplication with deviceCurrent_A is to get per unit values + x1 = (float32_t)(Ls_H * deviceCurrent_A); + + y1 = (float32_t)(2.0f * deviceCtrlPeriode_Sec); + + // multiplication with deviceVoltage_V is to get per unit values + y1 = (float32_t)(y1 * deviceVoltage_V); + + Kp = (x1 / y1); + return Kp; +} + +static inline float32_t PI_calcKi(const float32_t Rs_Ohm, const float32_t Ls_H, const float32_t deviceCtrlPeriode_Sec) +{ + // calculation is based on "TI - MotorWare's documentation" + float32_t RsByLs = (float32_t)(Rs_Ohm / Ls_H); + float32_t Ki = RsByLs * deviceCtrlPeriode_Sec; + //fix16_t Ki = 0; + return Ki; +} + +// ---------------------------------------------------------------------- +// end of file +// ---------------------------------------------------------------------- + +#ifdef __cplusplus +} +#endif /* extern "C" */ +#endif /* BLDC_PI_H_ */ + diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index 0b79e1c..a9f39cc 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -22,6 +22,7 @@ static void pwm_cb(const struct pwm_descriptor *const descr) 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); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index c90a5af..29884d2 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -4,10 +4,12 @@ // Header Files // ---------------------------------------------------------------------- #include "bldc.h" +#include "bldc_types.h" #include "EtherCAT_QSPI.h" #include "EtherCAT_SlaveDef.h" #include "configuration.h" #include "interrupts.h" +#include "statemachine.h" /** * Example of using TIMER_0. @@ -15,8 +17,8 @@ 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; + update_telemetry(); //run_ECAT = true; } @@ -41,11 +43,84 @@ void enable_NVIC_IRQ(void) //NVIC_EnableIRQ(EIC_5_IRQn); } +inline void CONTROLLER_StateMachine(void) +{ + Motor1.timerflags.current_loop_tic = false; + + if (applicationStatus.fault) + { + applicationStatus.previousstate = applicationStatus.currentstate; + applicationStatus.currentstate = FAULT; + } + + switch(applicationStatus.currentstate) /* process current motor state*/ + { + case SYSTEM_INIT: + /* Toggle driver reset Latch */ + gpio_set_pin_level(M1_RST, true); + gpio_set_pin_level(M1_RST, false); + gpio_set_pin_level(M2_RST, true); + gpio_set_pin_level(M2_RST, false); + /* Update State Variables */ + applicationStatus.previousstate = applicationStatus.currentstate; + applicationStatus.currentstate = SYSTEM_IDLE; + break; + case SYSTEM_IDLE: + /* If received MOTOR START command, move to MOTOR_INIT */ + //if(HostController.StartStopMotor == 0) + //{ + applicationStatus.previousstate = applicationStatus.currentstate; + applicationStatus.currentstate = MOTOR_IDLE; + //} + break; + case MOTOR_IDLE: + applicationStatus.previousstate = applicationStatus.currentstate; + applicationStatus.currentstate = MOTOR_OPEN_LOOP_STATE; + //applicationStatus.currentstate = MOTOR_V_CTRL_STATE; + break; + case MOTOR_OPEN_LOOP_STATE: + //volatile uint16_t x = 0; + break; + case FAULT: + //DisableGateDrivers(&Motor1); + switch(applicationStatus.fault) + { + case NOFAULT: + break; + case HALLSENSORINVALID: + break; + case MOTOR_STALL: + break; + case VOLTAGE: + break; + case OVERCURRENT: + break; + case GATE_DRIVER: + break; + case UNKNOWN: + break; + default: break; + }// End switch(switch(applicationStatus.fault)) + case COMMSTEST: + break; + default: break; + } // End switch(applicationStatus.currentstate) + + exec_commutation(&Motor1); + exec_commutation(&Motor2); +} // inline void CONTROLLER_StateMachine(void) + int main(void) { volatile uint8_t m2_hall, m1_hall, x= 0; /* Initializes MCU, drivers and middleware */ atmel_start_init(); + //initMotorParameters(); + BldcInitStruct(&Motor1, &FH_22mm24BXTR); + BldcInitStruct(&Motor2, &FH_32mm24BXTR); + Motor1.readHall = &readHallSensorM1; + Motor2.readHall = &readHallSensorM2; + config_qspi(); One_ms_timer_init(); configure_tcc_pwm(); @@ -57,12 +132,15 @@ int main(void) /* Replace with your application code */ while (1) { //delay_ms(100); - *M1_Motor__hallState = readM1Hall(); - *M2_Motor__hallState = readM2Hall(); + //*M1_Motor__hallState = readM1Hall(); + //*M2_Motor__hallState = readM2Hall(); //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(); } } + + diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h index 5027e3d..4dd264a 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h @@ -10,7 +10,6 @@ #define MOTORPARAMETERS_H_ #include "atmel_start.h" - // ---------------------------------------------------------------------- // M1 Hall Parameters // ---------------------------------------------------------------------- @@ -55,34 +54,6 @@ #define M2_HALL_C_LSR M2_HALL_C_PIN - M2_HALL_C_GROUP*32 -2 #define M2_HALL_C_GROUP M2_HALL_C_PIN/32 -// ---------------------------------------------------------------------- -// Motor Physical Parameters -// ---------------------------------------------------------------------- - -const typedef struct -{ - const uint16_t Poles; - const uint16_t polePairs; - const uint16_t commutationStates; - const float32_t motor_RS_Ohm; - const float32_t motor_LD_H; - const float32_t motor_Flux_WB; - const float32_t motor_Max_Spd_RPM; - const float32_t motor_Max_Spd_ELEC; - const float32_t motor_MeasureRange_RPM; // give 20% headroom - const float32_t motor_Max_Current_IDC_A; -} BLDCMotor_param_t; - -const BLDCMotor_param_t FH_32mm24BXTR = { - .Poles = 14, - .polePairs = 7, - .commutationStates = 42, //polePairs * 6 - -}; -const BLDCMotor_param_t FH_22mm24BXTR; - - - /* ---------------------------------------------------------------------- // Commutation Patterns // ---------------------------------------------------------------------- @@ -98,23 +69,117 @@ const BLDCMotor_param_t FH_22mm24BXTR; ** W7 | M1_0 | CC0 || M2_0 | CC0 | */ -static const uint16_t COMMUTATION_PATTERN[] = { +static const uint16_t COMMUTATION_PATTERN[16] = { 0x00FF, // (0) invalid state - 0x30F8, // (1) - 0x50F5, // (2) - 0x60F8, // (3) - 0x607D, // (4) - 0x507D, // (5) - 0x30F5, // (6) + 0x303B, // (1) + 0x183D, // (2) + 0x283B, // (3) + 0x283E, // (4) + 0x183E, // (5) + 0x303D, // (6) 0x00FF, // (7) invalid state 0x00FF, // (8) invalid state - 0x30F5, // (9) - 0x507D, // (10) - 0x607D, // (11) - 0x60F8, // (12) - 0x50F5, // (13) - 0x30F8, // (14) + 0x303D, // (9) + 0x183E, // (10) + 0x283E, // (11) + 0x283B, // (12) + 0x183D, // (13) + 0x303B, // (14) 0x00FF // (15) invalid state }; +typedef struct +{ + uint8_t hallA; + uint8_t hallB; + uint8_t hallC; +} Hall_pins_t; + +// ---------------------------------------------------------------------- +// Motor Physical Parameters +// ---------------------------------------------------------------------- +typedef struct +{ + struct pwm_descriptor const *pwm_desc; + const uint16_t motor_Poles; + const uint16_t motor_polePairs; + const uint16_t motor_commutationStates; + const float32_t motor_RS_Ohm; + const float32_t motor_LD_H; + const float32_t motor_LQ_H; + const float32_t motor_Flux_WB; + const float32_t motor_Max_Spd_RPM; + const float32_t motor_MeasureRange_RPM; // give 20% headroom + const float32_t motor_Max_Spd_ELEC; + const float32_t motor_Max_Current_IDC_A; + const Hall_pins_t hallPins; +} BLDCMotor_param_t; + +//static BLDCMotor_param_t FH_22mm24BXTR; +//static BLDCMotor_param_t FH_32mm24BXTR; + +/* Small Motor - 2214S024BXTR*/ +const static BLDCMotor_param_t FH_22mm24BXTR = { + .pwm_desc = &PWM_0, + .motor_Poles = 14, + .motor_polePairs = 7, + .motor_commutationStates = 42, //polePairs * 6 + .motor_RS_Ohm = 25.9, + .motor_LD_H = 0.003150, + .motor_LQ_H = 0.003150, + .motor_Flux_WB = 0.001575, + .motor_Max_Spd_RPM = 3000, + .motor_MeasureRange_RPM = 3000 * 1.2, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom + .motor_Max_Spd_ELEC = (3000/60)*7.0, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS + .motor_Max_Current_IDC_A = 0.368, + .hallPins = {M1_HALL_A_PIN, M1_HALL_B_PIN, M1_HALL_C_PIN} + +}; + +/* Big Motor - 3216W024BXTR */ +const static BLDCMotor_param_t FH_32mm24BXTR = { + .pwm_desc = &PWM_1, + .motor_Poles = 14, + .motor_polePairs = 7, + .motor_commutationStates = 42, //polePairs * 6 + .motor_RS_Ohm = 3.37, + .motor_LD_H = 0.001290, + .motor_LQ_H = 0.001290, + .motor_Flux_WB = 0.0063879968, + .motor_Max_Spd_RPM = 3000, + .motor_MeasureRange_RPM = 3200, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom + .motor_Max_Spd_ELEC = 12000, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS + .motor_Max_Current_IDC_A = 1.2, +}; + + +//static void initMotorParameters(void) +//{ + //FH_22mm24BXTR.pwm_desc = &PWM_0; + //FH_22mm24BXTR.motor_Poles = 14; + //FH_22mm24BXTR.motor_polePairs = 7; + //FH_22mm24BXTR.motor_commutationStates = 42; //polePairs * 6 + //FH_22mm24BXTR.motor_RS_Ohm = 25.9; + //FH_22mm24BXTR.motor_LD_H = 0.003150; + //FH_22mm24BXTR.motor_LQ_H = 0.003150; + //FH_22mm24BXTR.motor_Flux_WB = 0.001575; + //FH_22mm24BXTR.motor_Max_Spd_RPM = 3000; + //FH_22mm24BXTR.motor_MeasureRange_RPM = 3000 * 1.2; //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom + //FH_22mm24BXTR.motor_Max_Spd_ELEC = (3000/60)*7.0; //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS + //FH_22mm24BXTR.motor_Max_Current_IDC_A = 0.368; + // + //FH_32mm24BXTR.pwm_desc = &PWM_1; + //FH_32mm24BXTR.motor_Poles = 14; + //FH_32mm24BXTR.motor_polePairs = 7; + //FH_32mm24BXTR.motor_commutationStates = 42; //polePairs * 6 + //FH_32mm24BXTR.motor_RS_Ohm = 3.37; + //FH_32mm24BXTR.motor_LD_H = 0.001290; + //FH_32mm24BXTR.motor_LQ_H = 0.001290; + //FH_32mm24BXTR.motor_Flux_WB = 0.0063879968; + //FH_32mm24BXTR.motor_Max_Spd_RPM = 3000; + //FH_32mm24BXTR.motor_MeasureRange_RPM = 3200; //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom + //FH_32mm24BXTR.motor_Max_Spd_ELEC = 12000; //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS + //FH_32mm24BXTR.motor_Max_Current_IDC_A = 1.2; +//} + #endif /* MOTORPARAMETERS_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h new file mode 100644 index 0000000..3e8ec7c --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h @@ -0,0 +1,50 @@ +/* + * statemachine.h + * + * Created: 05/04/2021 13:23:24 + * Author: Nick-XMG + */ + + +#ifndef STATEMACHINE_H_ +#define STATEMACHINE_H_ + + +typedef enum +{ + SYSTEM_INIT = 0, + SYSTEM_IDLE = 1, + MOTOR_IDLE = 2, + MOTOR_I_CTRL_STATE = 3, + MOTOR_V_CTRL_STATE = 4, + MOTOR_P_CTRL_STATE = 5, + MOTOR_VI_CTRL_STATE = 6, + MOTOR_PVI_CTRL_STATE = 7, + MOTOR_STOP = 8, + FAULT = 9, + COMMSTEST = 10, + MOTOR_OPEN_LOOP_STATE = 11 +} CONTROLLER_FSM; + +typedef enum +{ + NOFAULT = 0, + VOLTAGE = 1, + OVERCURRENT = 2, + OVERTEMPERATURE = 3, + MOTOR_STALL = 4, + HALLSENSORINVALID = 5, + GATE_DRIVER = 6, + UNKNOWN = 7 +} FAULTS; + +typedef struct APPLICATION_STATUS +{ + CONTROLLER_FSM currentstate; + CONTROLLER_FSM previousstate; + FAULTS fault; +} APPLICATION_STATUS; + +APPLICATION_STATUS applicationStatus; + +#endif /* STATEMACHINE_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/utilities.h b/2_Motor_Master/Motor_Master/Motor_Master/utilities.h new file mode 100644 index 0000000..66cce8b --- /dev/null +++ b/2_Motor_Master/Motor_Master/Motor_Master/utilities.h @@ -0,0 +1,155 @@ +/* + * utilities.h + * + * Created: 20/04/2021 19:59:00 + * Author: Nick-XMG + */ + + +#ifndef UTILITIES_H_ +#define UTILITIES_H_ + + +#define ENABLED(V...) DO(ENA,&&,V) +#define DISABLED(V...) DO(DIS,&&,V) +#include "arm_math.h" + +#define DEBUG_1_PORT PORT_PC01 +#define DEBUG_2_PORT PORT_PC03 +#define DEBUG_3_PORT PORT_PC02 +#define DEBUG_4_PORT PORT_PC30 + +static inline void tic(const uint8_t pin) +{ + gpio_set_pin_level(pin, true); +} + +static inline void toc(const uint8_t pin) +{ + gpio_set_pin_level(pin, false); +} + +static inline void tic_port(const uint32_t port) +{ + REG_PORT_OUTSET2 = port; +} + +static inline void toc_port(const uint32_t port) +{ + REG_PORT_OUTCLR2 = port; +} + +//! \brief Saturates the input value between the minimum and maximum values +//! \param[in] in The input value +//! \param[in] max The maximum value allowed +//! \param[in] min The minimum value allowed +//! \return The saturated value +static inline float32_t f_clamp(const float32_t in, const float32_t min, const float32_t max) +{ + float32_t out = in; + + + if(in < min) + { + out = min; + } + else if(in > max) + { + out = max; + } + else + { + // do nothing as of now + ; + } + + return(out); +} + +//! \brief Finds the absolute value +//! \param[in] in The input value +//! \return The absolute value +static inline float32_t f_abs(const float32_t in) +{ + float32_t out = in; + if(in < (float32_t)0.0) + { + out = -in; + } + + return(out); +} + +// the typedefs + +//! \brief Defines the first-order filter (FILTER_FO) object +//! +typedef struct _FILTER_FO_Obj_ +{ + float32_t a1; //the denominator filter coefficient value for z^(-1) + + float32_t b0; //the numerator filter coefficient value for z^0 + float32_t b1; //the numerator filter coefficient value for z^(-1) + + float32_t x1; //the input value at time sample n=-1 + + float32_t y1; //the output value at time sample n=-1 +} FILTER_FO_Obj; + + +//! \brief Runs a first-order filter of the form +//! y[n] = b0*x[n] + b1*x[n-1] - a1*y[n-1] +//! +//! \param[in] handle The filter handle +//! \param[in] inputValue The input value to filter +//! \return The output value from the filter + +static inline float32_t FILTER_FO_run(volatile FILTER_FO_Obj *handle, const float32_t inputValue) +{ + FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle; + + float32_t a1 = obj->a1; + float32_t b0 = obj->b0; + float32_t b1 = obj->b1; + float32_t x1 = obj->x1; + float32_t y1 = obj->y1; + + + // compute the output + float32_t y0 = (b0 * inputValue) + (b1 * x1) - (a1 * y1); + + + // store values for next time + obj->x1 = inputValue; + obj->y1 = y0; + + return(y0); +} // end of FILTER_FO_run() function + + +//! \brief Runs a first-order filter of the form +//! y[n] = b0*x[n] - a1*y[n-1] +//! +//! \param[in] handle The filter handle +//! \param[in] inputValue The input value to filter +//! \return The output value from the filter +static inline float32_t FILTER_FO_run_form_0(volatile FILTER_FO_Obj *handle, const float32_t inputValue) +{ + FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle; + + float32_t a1 = obj->a1; + float32_t b0 = obj->b0; + float32_t y1 = obj->y1; + + + // compute the output + float32_t y0 = (b0 * inputValue) - (a1 * y1); + + + // store values for next time + obj->y1 = y0; + + return(y0); +} // end of FILTER_FO_run_form_0() function + +#endif /* UTILITIES_H_ */ \ No newline at end of file