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_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_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_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*/ ///* Motor 2*/
static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]); 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); 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_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_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_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*/ ///* Motor 3*/
static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]); 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); 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_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_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_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*/ ///* Motor 4*/
static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]); 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); 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_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_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_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) inline int16_t convert_to_mA(volatile float32_t current_PU)
{ {
@ -138,9 +138,10 @@ void update_telemetry(void)
//*M1_Mode = 0; //*M1_Mode = 0;
/* Motor 1 */ /* Motor 1 */
*M1_Status = Motor1.motor_state.currentstate;
*M1_Joint_rel_position = Motor1.motor_status.Num_Steps; *M1_Joint_rel_position = Motor1.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus); *M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus);
*M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A); *M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A);
*M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B); *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; *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle;
// //
/* Motor 2 */ /* Motor 2 */
*M2_Status = Motor2.motor_state.currentstate;
*M2_Joint_rel_position = Motor2.motor_status.Num_Steps; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus); *M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus);
*M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A); *M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A);
*M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B); *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 a = *M1_Control_mode;
//volatile uint8_t b = *M1_Control_set; //volatile uint8_t b = *M1_Control_set;
//volatile int16_t c = *M1_Desired_pos; //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_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_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_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*/ ///* Motor 2*/
static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]); 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); 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_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_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_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*/ ///* Motor 3*/
static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]); 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); 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_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_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_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*/ ///* Motor 4*/
static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]); 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); 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_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_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_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) inline int16_t convert_to_mA(volatile float32_t current_PU)
{ {
@ -138,9 +138,10 @@ void update_telemetry(void)
//*M1_Mode = 0; //*M1_Mode = 0;
/* Motor 1 */ /* Motor 1 */
*M1_Status = Motor1.motor_state.currentstate;
*M1_Joint_rel_position = Motor1.motor_status.Num_Steps; *M1_Joint_rel_position = Motor1.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus); *M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus);
*M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A); *M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A);
*M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B); *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; *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle;
// //
/* Motor 2 */ /* Motor 2 */
*M2_Status = Motor2.motor_state.currentstate;
*M2_Joint_rel_position = Motor2.motor_status.Num_Steps; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps;
//*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]);
//*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1);
*M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus); *M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus);
*M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A); *M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A);
*M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B); *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 a = *M1_Control_mode;
//volatile uint8_t b = *M1_Control_set; //volatile uint8_t b = *M1_Control_set;
//volatile int16_t c = *M1_Desired_pos; //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.PrepareFunctionsForGarbageCollection>True</armgcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
<armgcc.compiler.optimization.DebugLevel>Maximum (-g3)</armgcc.compiler.optimization.DebugLevel> <armgcc.compiler.optimization.DebugLevel>Maximum (-g3)</armgcc.compiler.optimization.DebugLevel>
<armgcc.compiler.warnings.AllWarnings>True</armgcc.compiler.warnings.AllWarnings> <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.general.UseNewlibNano>True</armgcc.linker.general.UseNewlibNano>
<armgcc.linker.libraries.Libraries> <armgcc.linker.libraries.Libraries>
<ListValues> <ListValues>

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

@ -9,41 +9,129 @@
#include "statemachine.h" #include "statemachine.h"
#include "utilities.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) 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; 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_status.actualDirection = 0;
motor->motor_setpoints.desiredDirection = 0; motor->motor_status.duty_cycle = 0;
motor->motor_status.duty_cycle = 200;
motor->motor_status.calc_rpm = 0; motor->motor_status.calc_rpm = 0;
motor->motor_status.Num_Steps = 0; motor->motor_status.Num_Steps = 0;
motor->motor_status.cur_comm_step = 0; motor->motor_status.cur_comm_step = 0;
motor->motor_status.currentHallPattern = 1; motor->motor_status.currentHallPattern = 1;
motor->motor_status.nextHallPattern = 3; 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.A = 0;
motor->Iphase_pu.B = 0; motor->Iphase_pu.B = 0;
motor->Iphase_pu.C = 0; motor->Iphase_pu.C = 0;
motor->Iphase_pu.Bus = 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.pwm_cycle_tic = false;
motor->timerflags.control_loop_tic = false; motor->timerflags.control_loop_tic = false;
motor->timerflags.motor_telemetry_flag = false; motor->timerflags.motor_telemetry_flag = false;
motor->timerflags.control_loop_tic = false; motor->timerflags.control_loop_tic = false;
motor->timerflags.adc_readings_ready_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; motor->regulation_loop_count = 0;
// ----------------------------------------------------------------------
// Initialize Bus Voltage:
// ----------------------------------------------------------------------
motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V; motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V;
motor->VoneByDcBus_pu = 1.0f/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_torque = 0.0;
motor->motor_setpoints.desired_speed = 0; motor->motor_setpoints.desired_speed = 0;
motor->motor_setpoints.desired_position = 0; motor->motor_setpoints.desired_position = 0;
@ -52,11 +140,7 @@ void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param)
motor->motor_setpoints.max_velocity = 0; motor->motor_setpoints.max_velocity = 0;
//// ------------------------------------------------------------------------------ //// ------------------------------------------------------------------------------
//// pi current control init //// Initialize PI current control
//// ------------------------------------------------------------------------------
//// ------------------------------------------------------------------------------
//// pi current control init
//// ------------------------------------------------------------------------------ //// ------------------------------------------------------------------------------
PI_objectInit(&motor->controllers.Pi_Idc); PI_objectInit(&motor->controllers.Pi_Idc);
float32_t motorLs_H = motor_param->motor_LQ_H * 2.0f; 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); 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); 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 */ /* VI Control Gains */
//motor->controllers.Pid_Speed.Kp = 0.0003f; //motor->controllers.Pid_Speed.Kp = 0.0003f;
//motor->controllers.Pid_Speed.Ki = 0.001f; //motor->controllers.Pid_Speed.Ki = 0.001f;
motor->controllers.Pid_Speed.Kp = 0.0005f; motor->controllers.Pid_Speed.Kp = 0.0005f;
motor->controllers.Pid_Speed.Ki = 0.0f; motor->controllers.Pid_Speed.Ki = 0.0f;
motor->controllers.Pid_Speed.Kd = 0.0001f; motor->controllers.Pid_Speed.Kd = 0.0001f;
//motor->controllers.Pid_Speed.Ki = 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.OutMax_pu = (motor_param->motor_Max_Current_IDC_A);
motor->controllers.Pid_Speed.OutMin_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); PI_objectInit(&motor->controllers.Pi_Pos);
motor->controllers.Pi_Pos.Kp = 40.0f; motor->controllers.Pi_Pos.Kp = 40.0f;
motor->controllers.Pi_Pos.Ki = 0.0f; motor->controllers.Pi_Pos.Ki = 0.0f;
motor->controllers.Pi_Pos.OutMax_pu = (motor_param->motor_Max_Spd_RPM); motor->controllers.Pi_Pos.OutMax_pu = (motor_param->motor_Max_Spd_RPM);
motor->controllers.Pi_Pos.OutMin_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) void exec_commutation(BLDCMotor_t *motor)
@ -102,38 +180,26 @@ void exec_commutation(BLDCMotor_t *motor)
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
//tic_port(DEBUG_2_PORT); //tic_port(DEBUG_2_PORT);
//motor->motor_status.currentHallPattern = readM1Hall(); //motor->motor_status.currentHallPattern = readM1Hall();
motor->motor_status.currentHallPattern = motor->readHall(); volatile uint8_t currentHall = motor->readHall();
if(motor->motor_status.currentHallPattern == INVALID_HALL_0 || motor->motor_status.currentHallPattern = currentHall;
motor->motor_status.currentHallPattern == INVALID_HALL_7 ) { if(((currentHall == INVALID_HALL_0) || (currentHall == INVALID_HALL_7))) {
applicationStatus.fault = HALLSENSORINVALID; //motor->motor_state.fault = MOTOR_HALLSENSORINVALID;
applicationStatus.currentstate = FAULT; //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 // 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); 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 // Set Calculated Duty Cycles
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
hri_tcc_write_CCBUF_CCBUF_bf(motor->motor_param->pwm_desc->device.hw, 0, motor->motor_status.duty_cycle); 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]; 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; 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); //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) 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 */ /* Process unit is Volts */
volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu)); 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; motor->motor_status.duty_cycle = (uint16_t)duty_cycle;
} else { } else {
/* Pu in Current (Amps) */ /* 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; 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) uint8_t readHallSensorM1(void)
{ {
volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN); 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 "atmel_start.h"
#include "bldc_types.h" #include "bldc_types.h"
#include "motorparameters.h" #include "motorparameters.h"
#include "statemachine.h"
#define PWM_TOP (1000) #define PWM_TOP (1000)
#define MAX_PWM (600) #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 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, 5, 6, 4, 9};
//static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 6, 4, 5, 9};
volatile BLDCMotor_t Motor1; volatile BLDCMotor_t Motor1;
volatile BLDCMotor_t Motor2; volatile BLDCMotor_t Motor2;
volatile MOTOR_STATE_t Motor1_Status;
volatile MOTOR_STATE_t Motor2_Status;
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
// functions // functions
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
void motor_StateMachine(BLDCMotor_t *motor);
void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param); void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param);
void exec_commutation(BLDCMotor_t *motor); 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 calculate_motor_speed(BLDCMotor_t *motor);
static void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float32_t speedRef); 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_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 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 readHallSensorM1(void);
uint8_t readHallSensorM2(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 "atmel_start.h"
#include "control.h" #include "control.h"
#include "motorparameters.h" #include "motorparameters.h"
#include "statemachine.h"
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
// function Pointer // function Pointer
@ -27,7 +28,7 @@ volatile typedef struct
volatile float32_t B; // Phase B volatile float32_t B; // Phase B
volatile float32_t C; // Phase C volatile float32_t C; // Phase C
volatile float32_t Bus; // Currently Active Phase Current volatile float32_t Bus; // Currently Active Phase Current
} MOTOR_3PHASES_t; } MOTOR_PHASES_t;
volatile typedef struct volatile typedef struct
{ {
@ -84,15 +85,14 @@ volatile typedef struct
volatile typedef struct BLDCmotor volatile typedef struct BLDCmotor
{ {
/* Hardware */ /* Hardware */
const uint32_t current_sensor_channels[2];
BLDCMotor_param_t *motor_param; BLDCMotor_param_t *motor_param;
MOTOR_STATE_t motor_state;
/* Status */ /* Status */
MOTOR_Status motor_status; MOTOR_Status motor_status;
/* Measured Values */ /* Measured Values */
volatile MOTOR_3PHASES_t Iphase_pu; volatile MOTOR_PHASES_t Iphase_pu;
volatile MOTOR_phase_offset_t Voffset_lsb; volatile MOTOR_phase_offset_t Voffset_lsb;
volatile float32_t VdcBus_pu; volatile float32_t VdcBus_pu, VoneByDcBus_pu;
volatile float32_t VoneByDcBus_pu;
/* Motor Flags */ /* Motor Flags */
volatile TIMERflags_t timerflags; volatile TIMERflags_t timerflags;
volatile uint8_t regulation_loop_count; 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); //NVIC_EnableIRQ(EIC_5_IRQn);
} }
inline void CONTROLLER_StateMachine(void) inline void APPLICATION_StateMachine(void)
{ {
Motor1.timerflags.current_loop_tic = false; Motor1.timerflags.current_loop_tic = false;
if (applicationStatus.fault) if (applicationStatus.fault)
{ {
applicationStatus.previousstate = applicationStatus.currentstate; applicationStatus.previousstate = applicationStatus.currentstate;
applicationStatus.currentstate = FAULT; applicationStatus.currentstate = APP_FAULT;
} }
switch(applicationStatus.currentstate) /* process current motor state*/ switch(applicationStatus.currentstate) /* process current motor state*/
@ -102,48 +102,38 @@ inline void CONTROLLER_StateMachine(void)
applicationStatus.currentstate = SYSTEM_IDLE; applicationStatus.currentstate = SYSTEM_IDLE;
break; break;
case SYSTEM_IDLE: case SYSTEM_IDLE:
/* If received MOTOR START command, move to MOTOR_INIT */
//if(HostController.StartStopMotor == 0)
//{
applicationStatus.previousstate = applicationStatus.currentstate; applicationStatus.previousstate = applicationStatus.currentstate;
applicationStatus.currentstate = MOTOR_IDLE; applicationStatus.currentstate = MOTORS_ON;
//}
break; break;
case MOTOR_IDLE: case MOTORS_ON:
applicationStatus.previousstate = applicationStatus.currentstate; applicationStatus.previousstate = applicationStatus.currentstate;
applicationStatus.currentstate = MOTOR_OPEN_LOOP_STATE; //applicationStatus.currentstate ;
//applicationStatus.currentstate = MOTOR_V_CTRL_STATE; motor_StateMachine(&Motor1);
motor_StateMachine(&Motor2);
exec_commutation(&Motor1);
exec_commutation(&Motor2);
break; break;
case MOTOR_OPEN_LOOP_STATE: case APP_FAULT:
//volatile uint16_t x = 0;
break;
case FAULT:
//DisableGateDrivers(&Motor1); //DisableGateDrivers(&Motor1);
switch(applicationStatus.fault) switch(applicationStatus.fault)
{ {
case NOFAULT: case APP_NOFAULT:
break; break;
case HALLSENSORINVALID: case ECAT_FAULT:
break; break;
case MOTOR_STALL: case MASTER_SLAVE_IF_FAULT:
break; break;
case VOLTAGE: case SPI_POS_SENSOR_FAULT:
break; break;
case OVERCURRENT: case EMG_INTERFACE_FAULT:
break;
case GATE_DRIVER:
break;
case UNKNOWN:
break; break;
default: break; default: break;
}// End switch(switch(applicationStatus.fault)) }// End switch(switch(applicationStatus.fault))
case COMMSTEST: case COMMSTEST:
comms_check();
break; break;
default: break; default: break;
} // End switch(applicationStatus.currentstate) } // End switch(applicationStatus.currentstate)
exec_commutation(&Motor1);
exec_commutation(&Motor2);
} // inline void CONTROLLER_StateMachine(void) } // inline void CONTROLLER_StateMachine(void)
int main(void) int main(void)
@ -173,7 +163,7 @@ int main(void)
//comms_check(); //comms_check();
//update_setpoints(); //update_setpoints();
if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} 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();} 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 //0x003F // (15) invalid state
//}; //};
typedef struct
{
uint8_t hallA;
uint8_t hallB;
uint8_t hallC;
} Hall_pins_t;
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
// Motor Physical Parameters // Motor Physical Parameters
// ---------------------------------------------------------------------- // ----------------------------------------------------------------------
@ -233,34 +225,4 @@ const static BLDCMotor_param_t FH_32mm24BXTR = {
.motor_Max_Current_IDC_A = 1.2, .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_ */ #endif /* MOTORPARAMETERS_H_ */

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

@ -9,42 +9,67 @@
#ifndef STATEMACHINE_H_ #ifndef STATEMACHINE_H_
#define 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 typedef enum
{ {
SYSTEM_INIT = 0, APP_NOFAULT = 0,
SYSTEM_IDLE = 1, ECAT_FAULT = 1,
MOTOR_IDLE = 2, MASTER_SLAVE_IF_FAULT = 2,
MOTOR_I_CTRL_STATE = 3, SPI_POS_SENSOR_FAULT = 3,
MOTOR_V_CTRL_STATE = 4, EMG_INTERFACE_FAULT = 4,
MOTOR_P_CTRL_STATE = 5, APP_MOTORFAULT = 5,
MOTOR_VI_CTRL_STATE = 6, } APP_FAULTS_t;
MOTOR_PVI_CTRL_STATE = 7,
MOTOR_STOP = 8, typedef struct APPLICATION_STATE
FAULT = 9, {
COMMSTEST = 10, volatile APPLICATION_FSM_t currentstate;
MOTOR_OPEN_LOOP_STATE = 11 volatile APPLICATION_FSM_t previousstate;
} CONTROLLER_FSM; 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 typedef enum
{ {
NOFAULT = 0, MOTOR_NOFAULT = 0,
VOLTAGE = 1, MOTOR_HALLSENSORINVALID = 1,
OVERCURRENT = 2, MOTOR_DRIVER_OVER_CURRENT = 2,
OVERTEMPERATURE = 3, } MOTOR_FAULTS_t;
MOTOR_STALL = 4,
HALLSENSORINVALID = 5,
GATE_DRIVER = 6,
UNKNOWN = 7
} FAULTS;
typedef struct APPLICATION_STATUS typedef struct MOTOR_STATE
{ {
CONTROLLER_FSM currentstate; volatile MOTOR_FSM_t currentstate;
CONTROLLER_FSM previousstate; volatile MOTOR_FSM_t previousstate;
FAULTS fault; volatile MOTOR_FAULTS_t fault;
} APPLICATION_STATUS; } MOTOR_STATE_t;
APPLICATION_STATUS applicationStatus;
#endif /* STATEMACHINE_H_ */ #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; 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 //! \brief Saturates the input value between the minimum and maximum values
//! \param[in] in The input value //! \param[in] in The input value
//! \param[in] max The maximum value allowed //! \param[in] max The maximum value allowed