diff --git a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h
index 8cb4e8e..e4e6c55 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h
@@ -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;
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h
index 8cb4e8e..e4e6c55 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h
@@ -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;
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj
index f3debeb..7fdad9a 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj
+++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj
@@ -418,7 +418,7 @@
True
Maximum (-g3)
True
- -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+ -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16
True
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
index eb4b285..12c1587 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
+++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
@@ -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);
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
index 788f891..4796651 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
@@ -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);
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h
index e8b69b5..fe00212 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h
@@ -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;
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c
index 61a3cac..7a4e768 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/main.c
+++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c
@@ -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();}
}
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
index ca9de44..458dc70 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
@@ -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_ */
\ No newline at end of file
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
index 3e8ec7c..7c24574 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
@@ -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_ */
\ No newline at end of file
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/utilities.h b/2_Motor_Master/Motor_Master/Motor_Master/utilities.h
index 66cce8b..aac75bf 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/utilities.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/utilities.h
@@ -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