nick
/
thesis_bldc_controller
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

commit poritng efforts

This commit is contained in:
Nicolas Trimborn 2021-08-05 18:53:54 +02:00
parent 7db543f79a
commit 639b562d2b
13 changed files with 1275 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart
View File

@ -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

69
2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h
View File

@ -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));

24
2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj
View File

@ -29,14 +29,14 @@
<eraseonlaunchrule>0</eraseonlaunchrule>
<EraseKey />
<AsfFrameworkConfig>
<framework-data xmlns="">
<framework-data>
<options />
<configurations />
<files />
<documentation help="" />
<offline-documentation help="" />
<dependencies>
<content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.50.0" />
<content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.42.0" />
</dependencies>
</framework-data>
</AsfFrameworkConfig>
@ -301,6 +301,7 @@
<armgcc.linker.libraries.LibrarySearchPaths>
<ListValues>
<Value>C:\Users\Nick-XMG\Documents\github\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis</Value>
<Value>C:\Users\ge37vez\Documents\Git Repos\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis</Value>
<Value>%24(ProjectDir)\Device_Startup</Value>
</ListValues>
</armgcc.linker.libraries.LibrarySearchPaths>
@ -413,11 +414,11 @@
<Value>../hri</Value>
</ListValues>
</armgcc.compiler.directories.IncludePaths>
<armgcc.compiler.optimization.level>Optimize (-O1)</armgcc.compiler.optimization.level>
<armgcc.compiler.optimization.level>Optimize more (-O2)</armgcc.compiler.optimization.level>
<armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>True</armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
<armgcc.compiler.optimization.DebugLevel>Maximum (-g3)</armgcc.compiler.optimization.DebugLevel>
<armgcc.compiler.warnings.AllWarnings>True</armgcc.compiler.warnings.AllWarnings>
<armgcc.compiler.miscellaneous.OtherFlags>-std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
<armgcc.compiler.miscellaneous.OtherFlags>-std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16</armgcc.compiler.miscellaneous.OtherFlags>
<armgcc.linker.general.UseNewlibNano>True</armgcc.linker.general.UseNewlibNano>
<armgcc.linker.libraries.Libraries>
<ListValues>
@ -427,8 +428,9 @@
</armgcc.linker.libraries.Libraries>
<armgcc.linker.libraries.LibrarySearchPaths>
<ListValues>
<Value>%24(ProjectDir)\Device_Startup</Value>
<Value>C:\Users\Nick-XMG\Documents\github\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis</Value>
<Value>C:\Users\ge37vez\Documents\Git Repos\bldc_control_thesis\bldc_firmware_thesis\2_Motor_Master\Motor_Master\Motor_Master\cmsis</Value>
<Value>%24(ProjectDir)\Device_Startup</Value>
</ListValues>
</armgcc.linker.libraries.LibrarySearchPaths>
<armgcc.linker.optimization.GarbageCollectUnusedSections>True</armgcc.linker.optimization.GarbageCollectUnusedSections>
@ -518,6 +520,9 @@
<Compile Include="bldc.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="bldc_types.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="configuration.h">
<SubType>compile</SubType>
</Compile>
@ -572,6 +577,9 @@
<Compile Include="Config\RTE_Components.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="control.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="Device_Startup\startup_same51.c">
<SubType>compile</SubType>
</Compile>
@ -1052,6 +1060,12 @@
<Compile Include="motorparameters.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="statemachine.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="utilities.h">
<SubType>compile</SubType>
</Compile>
</ItemGroup>
<ItemGroup>
<Folder Include="Config\" />

338
2_Motor_Master/Motor_Master/Motor_Master/bldc.c
View File

@ -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));
}

142
2_Motor_Master/Motor_Master/Motor_Master/bldc.h
View File

@ -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<<ADC_RESOLUTION)
#define ADC_LSB_SIZE (ADC_VOLTAGE_REFERENCE/ADC_MAX_COUNTS)
#define LSB_TO_PU (ADC_LSB_SIZE * ONEON_CURRENT_SENSOR_SENSITIVITY)
// ----------------------------------------------------------------------
// Define the control and PWM frequencies:
// ----------------------------------------------------------------------
// 16kHz is the maximum frequency according to the calculation duration in the mode run and spin.
#define DEVICE_MCU_FREQUENCY_Hz (120000000U)
#define DEVICE_SPEEDTC_DIV (4U)
#define DEVICE_SPEEDTC_FREQUENCY_Hz (DEVICE_MCU_FREQUENCY_Hz/DEVICE_SPEEDTC_DIV)
#define DEVICE_PWM_FREQUENCY_kHz (25.0f)
#define DEVICE_ISR_FREQUENCY_kHz DEVICE_PWM_FREQUENCY_kHz
#define DEVICE_ISR_PERIOD_Sec (0.001f/DEVICE_ISR_FREQUENCY_kHz)
#define HZ_TO_RPM (DEVICE_SPEEDTC_FREQUENCY_Hz * 60)
//#define HZ_TO_RPM MOTOR_COMUTATION_STATES/(DEVICE_SPEEDTC_FREQUENCY_Hz * 60)
// ----------------------------------------------------------------------
// Define the device quantities (voltage, current, speed)
// ----------------------------------------------------------------------
#define DEVICE_DC_VOLTAGE_V 24.0f // max. ADC bus voltage(PEAK) [V]
#define DEVICE_SHUNT_CURRENT_A 2.5f // phase current(PEAK) [A]
#define CURRENT_SENSOR_SENSITIVITY 0.4f //V/A
#define ONEON_CURRENT_SENSOR_SENSITIVITY 2.5f //V/A
// ----------------------------------------------------------------------
// global variables
// ----------------------------------------------------------------------
static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 };
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;
// ----------------------------------------------------------------------
// Type Definitions
// functions
// ----------------------------------------------------------------------
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 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;
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);
static void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t curRef);
static void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef);
static void SetDutyCycle(const uint16_t duty);
uint8_t readHallSensorM1(void);
uint8_t readHallSensorM2(void);
// ----------------------------------------------------------------------
// Inline Functions, hoping for in lining and faster execution
// ----------------------------------------------------------------------
static inline uint8_t readM1Hall(void)
{
//volatile uint8_t motor_read = 0;
@ -102,7 +111,7 @@ static inline uint8_t readM1Hall(void)
return ((a << 2) |
(b << 1) |
(c << 0));
}
static inline uint8_t readM2Hall(void)
@ -119,10 +128,29 @@ static inline uint8_t readM2Hall(void)
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) |
return ((a << 2) |
(b << 1) |
(c << 0));
(c << 0));
}
//static inline uint8_t read_hall_pattern(BLDCMotor_t *motor)
//{
////volatile uint8_t motor_read = 0;
////motor_read = (motor_read & M2_HALL_A_MASK) | (uint8_t)((PORT->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_ */

109
2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h Normal file
View File

@ -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_ */

57
2_Motor_Master/Motor_Master/Motor_Master/configuration.h
View File

@ -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;

256
2_Motor_Master/Motor_Master/Motor_Master/control.h Normal file
View File

@ -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_ */

1
2_Motor_Master/Motor_Master/Motor_Master/interrupts.h
View File

@ -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);

86
2_Motor_Master/Motor_Master/Motor_Master/main.c
View File

@ -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();
}
}

149
2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
View File

@ -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_ */

50
2_Motor_Master/Motor_Master/Motor_Master/statemachine.h Normal file
View File

@ -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_ */

155
2_Motor_Master/Motor_Master/Motor_Master/utilities.h Normal file
View File

@ -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_ */