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

added open loop

This commit is contained in:
Nicolas Trimborn 2021-08-10 18:34:37 +02:00
parent e1c1d89a2f
commit f03dd7c465
10 changed files with 292 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,7 +95,7 @@ static volatile int16_t *M1_Desired_current = ((int16_t *)&QSPI_rx_buffer[1]+1
static volatile int16_t *M1_Max_pos = ((int16_t *)&QSPI_rx_buffer[2]);
static volatile int16_t *M1_Max_velocity = ((int16_t *)&QSPI_rx_buffer[2]+1);
static volatile int16_t *M1_Max_current = ((int16_t *)&QSPI_rx_buffer[3]);
static volatile int16_t *M1_Spare = ((int16_t *)&QSPI_rx_buffer[3]+1);
static volatile int16_t *M1_Desired_dc = ((int16_t *)&QSPI_rx_buffer[3]+1); //Spare
///* Motor 2*/
static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]);
static volatile uint8_t *M2_Control_set = (((uint8_t *)&QSPI_rx_buffer[4])+1);
@ -105,7 +105,7 @@ static volatile int16_t *M2_Desired_current = ((int16_t *)&QSPI_rx_buffer[5]+1
static volatile int16_t *M2_Max_pos = ((int16_t *)&QSPI_rx_buffer[6]);
static volatile int16_t *M2_Max_velocity = ((int16_t *)&QSPI_rx_buffer[6]+1);
static volatile int16_t *M2_Max_current = ((int16_t *)&QSPI_rx_buffer[7]);
static volatile int16_t *M2_Spare = ((int16_t *)&QSPI_rx_buffer[7]+1);
static volatile int16_t *M2_Desired_dc = ((int16_t *)&QSPI_rx_buffer[7]+1); //Spare
///* Motor 3*/
static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]);
static volatile uint8_t *M3_Control_set = (((uint8_t *)&QSPI_rx_buffer[8])+1);
@ -115,7 +115,7 @@ static volatile int16_t *M3_Desired_current = ((int16_t *)&QSPI_rx_buffer[9]+1
static volatile int16_t *M3_Max_pos = ((int16_t *)&QSPI_rx_buffer[10]);
static volatile int16_t *M3_Max_velocity = ((int16_t *)&QSPI_rx_buffer[10]+1);
static volatile int16_t *M3_Max_current = ((int16_t *)&QSPI_rx_buffer[11]);
static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1);
static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1); //Spare
///* Motor 4*/
static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]);
static volatile uint8_t *M4_Control_set = (((uint8_t *)&QSPI_rx_buffer[12])+1);
@ -125,9 +125,9 @@ static volatile int16_t *M4_Desired_current = ((int16_t *)&QSPI_rx_buffer[13]+
static volatile int16_t *M4_Max_pos = ((int16_t *)&QSPI_rx_buffer[14]);
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);
static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1);//Spare
void update_telemetry(void)
static void update_telemetry(void)
{
inline int16_t convert_to_mA(volatile float32_t current_PU)
{
@ -138,9 +138,10 @@ void update_telemetry(void)
//*M1_Mode = 0;
/* Motor 1 */
*M1_Status = Motor1.motor_state.currentstate;
*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_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus);
*M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A);
*M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B);
@ -149,9 +150,10 @@ void update_telemetry(void)
*M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle;
//
/* Motor 2 */
*M2_Status = Motor2.motor_state.currentstate;
*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);
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus);
*M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A);
*M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B);
@ -161,8 +163,9 @@ void update_telemetry(void)
}
void update_setpoints(void)
static void update_setpoints(void)
{
//Motor1.motor_setpoints. = *desired_position;
//volatile uint8_t a = *M1_Control_mode;
//volatile uint8_t b = *M1_Control_set;
//volatile int16_t c = *M1_Desired_pos;

23
2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h
View File

@ -95,7 +95,7 @@ static volatile int16_t *M1_Desired_current = ((int16_t *)&QSPI_rx_buffer[1]+1
static volatile int16_t *M1_Max_pos = ((int16_t *)&QSPI_rx_buffer[2]);
static volatile int16_t *M1_Max_velocity = ((int16_t *)&QSPI_rx_buffer[2]+1);
static volatile int16_t *M1_Max_current = ((int16_t *)&QSPI_rx_buffer[3]);
static volatile int16_t *M1_Spare = ((int16_t *)&QSPI_rx_buffer[3]+1);
static volatile int16_t *M1_Desired_dc = ((int16_t *)&QSPI_rx_buffer[3]+1); //Spare
///* Motor 2*/
static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]);
static volatile uint8_t *M2_Control_set = (((uint8_t *)&QSPI_rx_buffer[4])+1);
@ -105,7 +105,7 @@ static volatile int16_t *M2_Desired_current = ((int16_t *)&QSPI_rx_buffer[5]+1
static volatile int16_t *M2_Max_pos = ((int16_t *)&QSPI_rx_buffer[6]);
static volatile int16_t *M2_Max_velocity = ((int16_t *)&QSPI_rx_buffer[6]+1);
static volatile int16_t *M2_Max_current = ((int16_t *)&QSPI_rx_buffer[7]);
static volatile int16_t *M2_Spare = ((int16_t *)&QSPI_rx_buffer[7]+1);
static volatile int16_t *M2_Desired_dc = ((int16_t *)&QSPI_rx_buffer[7]+1); //Spare
///* Motor 3*/
static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]);
static volatile uint8_t *M3_Control_set = (((uint8_t *)&QSPI_rx_buffer[8])+1);
@ -115,7 +115,7 @@ static volatile int16_t *M3_Desired_current = ((int16_t *)&QSPI_rx_buffer[9]+1
static volatile int16_t *M3_Max_pos = ((int16_t *)&QSPI_rx_buffer[10]);
static volatile int16_t *M3_Max_velocity = ((int16_t *)&QSPI_rx_buffer[10]+1);
static volatile int16_t *M3_Max_current = ((int16_t *)&QSPI_rx_buffer[11]);
static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1);
static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1); //Spare
///* Motor 4*/
static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]);
static volatile uint8_t *M4_Control_set = (((uint8_t *)&QSPI_rx_buffer[12])+1);
@ -125,9 +125,9 @@ static volatile int16_t *M4_Desired_current = ((int16_t *)&QSPI_rx_buffer[13]+
static volatile int16_t *M4_Max_pos = ((int16_t *)&QSPI_rx_buffer[14]);
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);
static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1);//Spare
void update_telemetry(void)
static void update_telemetry(void)
{
inline int16_t convert_to_mA(volatile float32_t current_PU)
{
@ -138,9 +138,10 @@ void update_telemetry(void)
//*M1_Mode = 0;
/* Motor 1 */
*M1_Status = Motor1.motor_state.currentstate;
*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_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus);
*M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A);
*M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B);
@ -149,9 +150,10 @@ void update_telemetry(void)
*M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle;
//
/* Motor 2 */
*M2_Status = Motor2.motor_state.currentstate;
*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);
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus);
*M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A);
*M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B);
@ -161,8 +163,9 @@ void update_telemetry(void)
}
void update_setpoints(void)
static void update_setpoints(void)
{
//Motor1.motor_setpoints. = *desired_position;
//volatile uint8_t a = *M1_Control_mode;
//volatile uint8_t b = *M1_Control_set;
//volatile int16_t c = *M1_Desired_pos;

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

@ -418,7 +418,7 @@
<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>

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

@ -9,54 +9,138 @@
#include "statemachine.h"
#include "utilities.h"
#include "Ethercat_SlaveDef.h"
void motor_StateMachine(BLDCMotor_t *motor)
{
if (motor->motor_state.fault)
{
motor->motor_state.previousstate = motor->motor_state.currentstate;
motor->motor_state.currentstate = MOTOR_FAULT;
}
switch (motor->motor_state.currentstate)
{
case MOTOR_INIT:
motor->motor_state.previousstate = motor->motor_state.currentstate;
motor->motor_state.currentstate = MOTOR_IDLE;
break;
case MOTOR_IDLE:
motor->motor_state.previousstate = motor->motor_state.currentstate;
motor->motor_state.currentstate = MOTOR_OPEN_LOOP_STATE;
break;
case MOTOR_OPEN_LOOP_STATE:
BLDC_runOpenLoop(motor, *M1_Desired_dc);
break;
case MOTOR_PVI_CTRL_STATE:
switch (Motor1.regulation_loop_count) {
case 0: /* PWM FREQ / 25 - 1kHz */
Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag
BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
BLDC_runPosCntl(&Motor2, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */
//calculate_motor_speed();
//calculate_motor_speed();
BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
BLDC_runSpeedCntl(&Motor2, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
default: /* PWM FREQ - 25kHz */
select_active_phase(motor);
//select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern);
BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
BLDC_runCurrentCntl(&Motor2, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
break;
} // end switch(regulation_loop_count)
if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
else Motor1.regulation_loop_count++;
break;
} //end switch (motor->motor_state.currentstate)
// ----------------------------------------------------------------------
// Run Commutation
// ----------------------------------------------------------------------
}
void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param)
{
// ----------------------------------------------------------------------
// Initialize all voltage and current objects, variables and helpers:
// Assign Motor Parameters:
// ----------------------------------------------------------------------
motor->motor_param = motor_param;
// ----------------------------------------------------------------------
// Initialize State Machine:
// ----------------------------------------------------------------------
motor->motor_state.currentstate = MOTOR_INIT;
motor->motor_state.previousstate = MOTOR_INIT;
motor->motor_state.fault = MOTOR_NOFAULT;
// ----------------------------------------------------------------------
// Initialize Status Struct:
// ----------------------------------------------------------------------
motor->motor_status.actualDirection = 0;
motor->motor_setpoints.desiredDirection = 0;
motor->motor_status.duty_cycle = 200;
motor->motor_status.duty_cycle = 0;
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 = 8;
motor->motor_setpoints.directionOffset = 0;
// ----------------------------------------------------------------------
// Initialize Phase Current Struct:
// ----------------------------------------------------------------------
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;
// ----------------------------------------------------------------------
// Initialize Timers:
// ----------------------------------------------------------------------
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;
// ----------------------------------------------------------------------
// Current Sensors Calibration offsets:
// ----------------------------------------------------------------------
motor->Voffset_lsb.A = 0;
motor->Voffset_lsb.B = 0;
// ----------------------------------------------------------------------
// Regulation Loop Counter
// ----------------------------------------------------------------------
motor->regulation_loop_count = 0;
// ----------------------------------------------------------------------
// Initialize Bus Voltage:
// ----------------------------------------------------------------------
motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V;
motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V;
// ----------------------------------------------------------------------
// Initialize Setpoints Struct:
// ----------------------------------------------------------------------
motor->motor_setpoints.desiredDirection = 0;
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
//// Initialize PI current control
//// ------------------------------------------------------------------------------
PI_objectInit(&motor->controllers.Pi_Idc);
float32_t motorLs_H = motor_param->motor_LQ_H * 2.0f;
@ -65,34 +149,28 @@ void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param)
motor->controllers.Pi_Idc.Ki = PI_calcKi(motorRs_OHM, motorLs_H, DEVICE_ISR_PERIOD_Sec);
//// ------------------------------------------------------------------------------
//// pi velocity control init
//// Initialize PD Vel control
//// ------------------------------------------------------------------------------
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
//// Initialize PI Position control
//// ------------------------------------------------------------------------------
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)
@ -102,38 +180,26 @@ void exec_commutation(BLDCMotor_t *motor)
// ----------------------------------------------------------------------
//tic_port(DEBUG_2_PORT);
//motor->motor_status.currentHallPattern = readM1Hall();
motor->motor_status.currentHallPattern = motor->readHall();
if(motor->motor_status.currentHallPattern == INVALID_HALL_0 ||
motor->motor_status.currentHallPattern == INVALID_HALL_7 ) {
applicationStatus.fault = HALLSENSORINVALID;
applicationStatus.currentstate = FAULT;
volatile uint8_t currentHall = motor->readHall();
motor->motor_status.currentHallPattern = currentHall;
if(((currentHall == INVALID_HALL_0) || (currentHall == INVALID_HALL_7))) {
//motor->motor_state.fault = MOTOR_HALLSENSORINVALID;
//motor->motor_state.currentstate = MOTOR_FAULT;
//applicationStatus.currentstate = APP_FAULT;
}
// ----------------------------------------------------------------------
// 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];
volatile uint16_t temp_M1 = COMMUTATION_PATTERN[motor->motor_status.currentHallPattern + motor->motor_setpoints.directionOffset];
// ----------------------------------------------------------------------
// Set Pattern Buffers
// ----------------------------------------------------------------------
volatile uint16_t temp_M1 = COMMUTATION_PATTERN[motor->motor_status.currentHallPattern +
motor->motor_setpoints.directionOffset];
hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, 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;
@ -158,6 +224,36 @@ void exec_commutation(BLDCMotor_t *motor)
//toc_port(DEBUG_2_PORT);
}
//// ------------------------------------------------------------------------------
// Current Selection Based on Hall State
// Direction":
// CW -> Always positive current
// CCW -> Always negative current
//// ------------------------------------------------------------------------------
void select_active_phase(BLDCMotor_t *Motor)
{
uint8_t hall_state = Motor->motor_status.currentHallPattern;
volatile float32_t phase_current = 0;
switch(hall_state)
{
case 0b001: case 0b011:
phase_current = Motor->Iphase_pu.C;
break;
case 0b010: case 0b110:
phase_current = Motor->Iphase_pu.B;
break;
case 0b100: case 0b101:
phase_current = Motor->Iphase_pu.A;
break;
default :
//phase_current = 0; // Invalid hall code
break;
}
Motor->Iphase_pu.Bus = phase_current;
}
void calculate_motor_speed(BLDCMotor_t *motor)
{
@ -274,7 +370,7 @@ void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float
}
/* 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);
volatile float32_t 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) */
@ -300,6 +396,16 @@ void BLDC_runPosCntl(BLDCMotor_t *motor, const int16_t posfbk, const int16_t pos
motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu;
}
void BLDC_runOpenLoop(BLDCMotor_t *motor, int16_t duty)
{
if (duty < 0){
motor->motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
} else {
motor->motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
}
motor->motor_status.duty_cycle = u_clamp(abs(duty), 0, MAX_PWM);
}
uint8_t readHallSensorM1(void)
{
volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN);

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

@ -14,6 +14,7 @@
#include "atmel_start.h"
#include "bldc_types.h"
#include "motorparameters.h"
#include "statemachine.h"
#define PWM_TOP (1000)
#define MAX_PWM (600)
@ -63,23 +64,30 @@
// ----------------------------------------------------------------------
//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};
//static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 6, 4, 5, 9};
static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 5, 6, 4, 9};
volatile BLDCMotor_t Motor1;
volatile BLDCMotor_t Motor2;
volatile MOTOR_STATE_t Motor1_Status;
volatile MOTOR_STATE_t Motor2_Status;
// ----------------------------------------------------------------------
// functions
// ----------------------------------------------------------------------
void motor_StateMachine(BLDCMotor_t *motor);
void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param);
void exec_commutation(BLDCMotor_t *motor);
static void select_active_phase(BLDCMotor_t *Motor, const uint8_t hall_state);
void select_active_phase(BLDCMotor_t *Motor);
// ----------------------------------------------------------------------
// Static Functions
// ----------------------------------------------------------------------
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);
static void BLDC_runOpenLoop(BLDCMotor_t *motor, int16_t duty);
uint8_t readHallSensorM1(void);
uint8_t readHallSensorM2(void);

10
2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h
View File

@ -12,6 +12,7 @@
#include "atmel_start.h"
#include "control.h"
#include "motorparameters.h"
#include "statemachine.h"
// ----------------------------------------------------------------------
// function Pointer
@ -27,7 +28,7 @@ volatile typedef struct
volatile float32_t B; // Phase B
volatile float32_t C; // Phase C
volatile float32_t Bus; // Currently Active Phase Current
} MOTOR_3PHASES_t;
} MOTOR_PHASES_t;
volatile typedef struct
{
@ -84,15 +85,14 @@ volatile typedef struct
volatile typedef struct BLDCmotor
{
/* Hardware */
const uint32_t current_sensor_channels[2];
BLDCMotor_param_t *motor_param;
MOTOR_STATE_t motor_state;
/* Status */
MOTOR_Status motor_status;
/* Measured Values */
volatile MOTOR_3PHASES_t Iphase_pu;
volatile MOTOR_PHASES_t Iphase_pu;
volatile MOTOR_phase_offset_t Voffset_lsb;
volatile float32_t VdcBus_pu;
volatile float32_t VoneByDcBus_pu;
volatile float32_t VdcBus_pu, VoneByDcBus_pu;
/* Motor Flags */
volatile TIMERflags_t timerflags;
volatile uint8_t regulation_loop_count;

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

@ -79,14 +79,14 @@ void enable_NVIC_IRQ(void)
//NVIC_EnableIRQ(EIC_5_IRQn);
}
inline void CONTROLLER_StateMachine(void)
inline void APPLICATION_StateMachine(void)
{
Motor1.timerflags.current_loop_tic = false;
if (applicationStatus.fault)
{
applicationStatus.previousstate = applicationStatus.currentstate;
applicationStatus.currentstate = FAULT;
applicationStatus.currentstate = APP_FAULT;
}
switch(applicationStatus.currentstate) /* process current motor state*/
@ -102,48 +102,38 @@ inline void CONTROLLER_StateMachine(void)
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;
//}
applicationStatus.currentstate = MOTORS_ON;
break;
case MOTOR_IDLE:
case MOTORS_ON:
applicationStatus.previousstate = applicationStatus.currentstate;
applicationStatus.currentstate = MOTOR_OPEN_LOOP_STATE;
//applicationStatus.currentstate = MOTOR_V_CTRL_STATE;
//applicationStatus.currentstate ;
motor_StateMachine(&Motor1);
motor_StateMachine(&Motor2);
exec_commutation(&Motor1);
exec_commutation(&Motor2);
break;
case MOTOR_OPEN_LOOP_STATE:
//volatile uint16_t x = 0;
break;
case FAULT:
case APP_FAULT:
//DisableGateDrivers(&Motor1);
switch(applicationStatus.fault)
{
case NOFAULT:
case APP_NOFAULT:
break;
case HALLSENSORINVALID:
case ECAT_FAULT:
break;
case MOTOR_STALL:
case MASTER_SLAVE_IF_FAULT:
break;
case VOLTAGE:
case SPI_POS_SENSOR_FAULT:
break;
case OVERCURRENT:
break;
case GATE_DRIVER:
break;
case UNKNOWN:
case EMG_INTERFACE_FAULT:
break;
default: break;
}// End switch(switch(applicationStatus.fault))
case COMMSTEST:
comms_check();
break;
default: break;
} // End switch(applicationStatus.currentstate)
exec_commutation(&Motor1);
exec_commutation(&Motor2);
} // inline void CONTROLLER_StateMachine(void)
int main(void)
@ -173,7 +163,7 @@ int main(void)
//comms_check();
//update_setpoints();
if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();}
if (Motor1.timerflags.current_loop_tic) {CONTROLLER_StateMachine();}
if (Motor1.timerflags.current_loop_tic) {APPLICATION_StateMachine();}
if (run_ECAT) {ECAT_STATE_MACHINE();}
}

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

@ -171,14 +171,6 @@ static const uint16_t COMMUTATION_PATTERN[16] = {
//0x003F // (15) invalid state
//};
typedef struct
{
uint8_t hallA;
uint8_t hallB;
uint8_t hallC;
} Hall_pins_t;
// ----------------------------------------------------------------------
// Motor Physical Parameters
// ----------------------------------------------------------------------
@ -233,34 +225,4 @@ const static BLDCMotor_param_t FH_32mm24BXTR = {
.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_ */

81
2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
View File

@ -9,42 +9,67 @@
#ifndef STATEMACHINE_H_
#define STATEMACHINE_H_
// ----------------------------------------------------------------------
// Application States
// ----------------------------------------------------------------------
typedef enum
{
SYSTEM_INIT = 0,
SYSTEM_IDLE = 1,
MOTORS_ON = 2,
APP_FAULT = 3,
COMMSTEST = 4,
} APPLICATION_FSM_t;
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;
APP_NOFAULT = 0,
ECAT_FAULT = 1,
MASTER_SLAVE_IF_FAULT = 2,
SPI_POS_SENSOR_FAULT = 3,
EMG_INTERFACE_FAULT = 4,
APP_MOTORFAULT = 5,
} APP_FAULTS_t;
typedef struct APPLICATION_STATE
{
volatile APPLICATION_FSM_t currentstate;
volatile APPLICATION_FSM_t previousstate;
volatile APP_FAULTS_t fault;
} APPLICATION_STATE_t;
volatile APPLICATION_STATE_t applicationStatus;
// ----------------------------------------------------------------------
// Motor States
// ----------------------------------------------------------------------
typedef enum
{
MOTOR_INIT = 0,
MOTOR_IDLE = 1,
MOTOR_OPEN_LOOP_STATE = 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,
MOTOR_FAULT = 9,
} MOTOR_FSM_t;
typedef enum
{
NOFAULT = 0,
VOLTAGE = 1,
OVERCURRENT = 2,
OVERTEMPERATURE = 3,
MOTOR_STALL = 4,
HALLSENSORINVALID = 5,
GATE_DRIVER = 6,
UNKNOWN = 7
} FAULTS;
MOTOR_NOFAULT = 0,
MOTOR_HALLSENSORINVALID = 1,
MOTOR_DRIVER_OVER_CURRENT = 2,
} MOTOR_FAULTS_t;
typedef struct APPLICATION_STATUS
typedef struct MOTOR_STATE
{
CONTROLLER_FSM currentstate;
CONTROLLER_FSM previousstate;
FAULTS fault;
} APPLICATION_STATUS;
volatile MOTOR_FSM_t currentstate;
volatile MOTOR_FSM_t previousstate;
volatile MOTOR_FAULTS_t fault;
} MOTOR_STATE_t;
APPLICATION_STATUS applicationStatus;
#endif /* STATEMACHINE_H_ */

26
2_Motor_Master/Motor_Master/Motor_Master/utilities.h
View File

@ -39,6 +39,32 @@ 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 uint16_t u_clamp(const int16_t in, const uint16_t min, const uint16_t max)
{
uint16_t out = abs(in);
if(in < min)
{
out = min;
}
else if(in > max)
{
out = max;
}
else
{
// do nothing as of now
;
}
return(out);
}
//! \brief Saturates the input value between the minimum and maximum values
//! \param[in] in The input value
//! \param[in] max The maximum value allowed