diff --git a/BLDC_E54/.vs/BLDC_E54/v14/.atsuo b/BLDC_E54/.vs/BLDC_E54/v14/.atsuo index 2604932..fad8109 100644 Binary files a/BLDC_E54/.vs/BLDC_E54/v14/.atsuo and b/BLDC_E54/.vs/BLDC_E54/v14/.atsuo differ diff --git a/BLDC_E54/BLDC_E54/bldc.c b/BLDC_E54/BLDC_E54/bldc.c index fb7b0eb..6fe5869 100644 --- a/BLDC_E54/BLDC_E54/bldc.c +++ b/BLDC_E54/BLDC_E54/bldc.c @@ -46,47 +46,51 @@ void select_active_phase(volatile BLDCMotor_t *Motor, volatile uint8_t hall_stat Motor->Iphase_pu.Bus = phase_current; } -void BLDC_init(BLDCMotor_t *motor) +void BldcInitStruct(BLDCMotor_t *motor) { // ---------------------------------------------------------------------- // Initialize all voltage and current objects, variables and helpers: - motor->actualDirection = 0; - motor->desiredDirection = 0; - motor->duty_cycle = 0; - motor->calc_rpm = 0; - motor->Num_Steps = 0; - motor->cur_comm_step = 0; - motor->currentHallPattern = 1; - motor->nextHallPattern = 3; - motor->directionOffset = 0; + motor->motor_status.actualDirection = 0; + motor->motor_setpoints.desiredDirection = 0; + 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 = 0; //motor->hall_state = 1; - motor->dir_hall_code = 1; + //motor->dir_hall_code = 1; - motor->Iphase_pu.A = 0; - motor->Iphase_pu.B = 0; - motor->Iphase_pu.C = 0; - motor->Iphase_pu.Bus = 0; - - motor->Voffset_lsb.A = 0; - motor->Voffset_lsb.B = 0; - motor->timerflags.pwm_cycle_tic = false; - motor->timerflags.control_loop_tic = false; - motor->timerflags.motor_telemetry_flag = false; - motor->timerflags.control_loop_tic = false; - motor->regulation_loop_count = 0; + motor->Iphase_pu.A = 0; + motor->Iphase_pu.B = 0; + motor->Iphase_pu.C = 0; + motor->Iphase_pu.Bus = 0; + motor->Voffset_lsb.A = 0; + motor->Voffset_lsb.B = 0; + motor->timerflags.pwm_cycle_tic = false; + motor->timerflags.control_loop_tic = false; + motor->timerflags.motor_telemetry_flag = false; + motor->timerflags.control_loop_tic = false; + motor->regulation_loop_count = 0; motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V; motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V; motor->motor_commutation_Pattern = COMMUTATION_PATTERN_M1; - motor->desired_torque = 0.0; - motor->desired_speed = 0; - motor->desired_position = 0; - motor->max_current = 0.0; - motor->max_torque = 0.0; - motor->max_velocity = 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; + + // ------------------------------------------------------------------------------ + // Function + // ------------------------------------------------------------------------------ + //motor->ReadHall = HallFunc; //// ------------------------------------------------------------------------------ //// pi current control init @@ -131,6 +135,16 @@ void BLDC_init(BLDCMotor_t *motor) motor->controllers.Pi_Pos.OutMin_pu = -MOTOR_MAX_SPD_RPM; } +void BldcInitFunctions() +{ + Motor1.ReadHall = readM1Hall; + Motor2.ReadHall = readM2Hall; + Motor3.ReadHall = readM3Hall; + Motor1.DisableMotor = DisableM1GateDrivers; + Motor2.DisableMotor = DisableM2GateDrivers; + Motor3.DisableMotor = DisableM3GateDrivers; +} + // ---------------------------------------------------------------------- // Before Power up, Measure and average offset voltage for Current Sensor // This voltage is subtracted from all reading for Bi-Directional Current @@ -166,55 +180,102 @@ void read_zero_current_offset_value(void) phase_B_zero_current_offset_temp += zero_current_offset_temp[1]; samples--; } + //Motor1.Iphase_pu Motor1.Voffset_lsb.A = phase_A_zero_current_offset_temp/samples; Motor1.Voffset_lsb.B = phase_B_zero_current_offset_temp/samples; adc_async_disable_channel(&ADC_0, 0); adc_async_disable_channel(&ADC_1, 0); } -uint8_t get_dir_hall_code(void) +//uint8_t get_dir_hall_code(void) +//{ + //return ((Motor1.desiredDirection << 3) | + //(_gpio_get_level(HALL_A_PIN) << 2) | + //(_gpio_get_level(HALL_B_PIN) << 1) | + //(_gpio_get_level(HALL_C_PIN) << 0)); +//} +// +//uint8_t HALLPatternGet(void) +//{ + // + // + //volatile uint8_t motor_read = 0; + //motor_read = (motor_read & HALL_A_MASK) | (uint8_t)((PORT->Group[HALL_A_GROUP].IN.reg & HALL_A_PORT)>>(HALL_A_LSR)); + //motor_read = (motor_read & HALL_B_MASK) | (uint8_t)((PORT->Group[HALL_B_GROUP].IN.reg & HALL_B_PORT)>>(HALL_B_LSR)); + //motor_read = (motor_read & HALL_C_MASK) | (uint8_t)((PORT->Group[HALL_C_GROUP].IN.reg & HALL_C_PORT)>>(HALL_C_LSR)); + // + //return motor_read; + // + ////return ((gpio_get_pin_level(HALL_A_PIN) << 2)| + ////(gpio_get_pin_level(HALL_B_PIN) << 1)| + ////(gpio_get_pin_level(HALL_C_PIN) << 0)); +//} + + +// ---------------------------------------------------------------------- +// Hall Reading Functions +// ---------------------------------------------------------------------- + +uint8_t readM1Hall(void) { - return ((Motor1.desiredDirection << 3) | - (_gpio_get_level(HALL_A_PIN) << 2) | - (_gpio_get_level(HALL_B_PIN) << 1) | - (_gpio_get_level(HALL_C_PIN) << 0)); + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); } -uint8_t HALLPatternGet(void) +uint8_t readM2Hall(void) { + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); - - volatile uint8_t motor_read = 0; - motor_read = (motor_read & HALL_A_MASK) | (uint8_t)((PORT->Group[HALL_A_GROUP].IN.reg & HALL_A_PORT)>>(HALL_A_LSR)); - motor_read = (motor_read & HALL_B_MASK) | (uint8_t)((PORT->Group[HALL_B_GROUP].IN.reg & HALL_B_PORT)>>(HALL_B_LSR)); - motor_read = (motor_read & HALL_C_MASK) | (uint8_t)((PORT->Group[HALL_C_GROUP].IN.reg & HALL_C_PORT)>>(HALL_C_LSR)); - - return motor_read; - - //return ((gpio_get_pin_level(HALL_A_PIN) << 2)| - //(gpio_get_pin_level(HALL_B_PIN) << 1)| - //(gpio_get_pin_level(HALL_C_PIN) << 0)); + return ((a << 2) | + (b << 1) | + (c << 0)); } -uint8_t PDEC_HALLPatternGet(void) +uint8_t readM3Hall(void) +{ + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); + +} + +// ---------------------------------------------------------------------- +// Disable Functions +// ---------------------------------------------------------------------- + +void DisableM1GateDrivers(BLDCMotor_t *motor) +{ + hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); +} + +void DisableM2GateDrivers(BLDCMotor_t *motor) +{ + hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); +} + +void DisableM3GateDrivers(BLDCMotor_t *motor) { - PDEC->CTRLBSET.reg = PDEC_CTRLBSET_CMD_READSYNC; - while(PDEC->SYNCBUSY.reg) - { - /* Wait for read Synchronization */ - } - while((PDEC->CTRLBSET.reg & PDEC_CTRLBSET_CMD_Msk) != PDEC_CTRLBSET_CMD_NONE) - { - /* Wait for CMD to become zero */ - } - return (uint8_t)(PDEC->COUNT.reg & 0x07); } void exec_commutation(void) { //CRITICAL_SECTION_ENTER(); - Motor1.currentHallPattern = HALLPatternGet(); + //Motor1.currentHallPattern = HALLPatternGet(); + Motor1.motor_status.currentHallPattern = Motor1.ReadHall(); + Motor2.motor_status.currentHallPattern = Motor2.ReadHall(); + Motor3.motor_status.currentHallPattern = Motor3.ReadHall(); //tic(DEBUG_3); ////if ((Motor1.nextHallPattern == Motor1.currentHallPattern) && @@ -225,8 +286,10 @@ void exec_commutation(void) //} - TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.currentHallPattern + Motor1.directionOffset)]; - TCC1->CCBUF->reg = (uint16_t)Motor1.duty_cycle; + TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.motor_status.currentHallPattern + + Motor1.motor_setpoints.directionOffset)]; + + TCC1->CCBUF->reg = (uint16_t)Motor1.motor_status.duty_cycle; //hri_tcc_write_PATTBUF_reg(TCC1, (COMMUTATION_PATTERN[(Motor1.currentHallPattern + Motor1.directionOffset)])); //hri_tcc_write_CCBUF_CCBUF_bf(TCC1, 0, 150); @@ -249,28 +312,28 @@ void exec_commutation(void) //volatile uint8_t curHallState = hallCode & 0x07; //ABC format //volatile uint8_t curHallState = get_hall_state(); //ABC format - Motor1.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.currentHallPattern]; - volatile int8_t step_change = Motor1.cur_comm_step - Motor1.prev_comm_step; + Motor1.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern]; + volatile int8_t step_change = Motor1.motor_status.cur_comm_step - Motor1.motor_status.prev_comm_step; switch(step_change) { case 1: case -5: - Motor1.Num_Steps = Motor1.Num_Steps+1; - Motor1.actualDirection = CW; + Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps+1; + Motor1.motor_status.actualDirection = CW; //Motor1.directionOffset = DIRECTION_CW_OFFSET; break; case -1: case 5: - Motor1.Num_Steps = Motor1.Num_Steps-1; - Motor1.actualDirection = CCW; + Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps-1; + Motor1.motor_status.actualDirection = CCW; //Motor1.directionOffset = DIRECTION_CCW_OFFSET; break; default: // do nothing break; } - Motor1.prev_comm_step = Motor1.cur_comm_step; + Motor1.motor_status.prev_comm_step = Motor1.motor_status.cur_comm_step; //calculate_motor_speed(); //toc(DEBUG_3); //toc(DEBUG_4); @@ -283,7 +346,7 @@ void calculate_motor_speed(void) hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/ volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1); if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) { - Motor1.calc_rpm = 0; + Motor1.motor_status.calc_rpm = 0; } else { //uint32_t test = (SPEEDFACTOR, period_after_capture); //temp_rpm = SPEEDFACTOR / period_after_capture; @@ -297,26 +360,26 @@ void calculate_motor_speed(void) if(count >= n_SAMPLE) { count = 1; - Motor1.calc_rpm = (speed_average >> 3); // divide by 32 + Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32 speed_average = 0; //*Spare_byte1 = motorState.actualDirection; - if(Motor1.actualDirection == CCW) /* Changed from CCW */ + if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ { //*motor_speed = -1* Motor1.calc_rpm; - Motor1.calc_rpm = -1* Motor1.calc_rpm; + Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm; } else { //*motor_speed = Motor1.calc_rpm; - Motor1.calc_rpm = Motor1.calc_rpm; + Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm; } return; } else count++; #else - Motor1.calc_rpm = (int16_t)temp_rpm; + Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; #endif } -void DisableGateDrivers(BLDCMotor_t *motor) +void DisableMotor(BLDCMotor_t *motor) { hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); @@ -338,16 +401,16 @@ void BLDC_runCurrentCntl(BLDCMotor_t *motor, volatile float curfbk, volatile flo PI_run_series(&motor->controllers.Pi_Idc); if(motor->controllers.Pi_Idc.Out_pu > 0) { - motor->desiredDirection = 0; - motor->directionOffset = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; } else { - motor->desiredDirection = 1; - motor->directionOffset = 8; + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; } volatile float duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu)); - volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); - motor->duty_cycle = (uint16_t)duty_cycle; + volatile uint32_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; // Remove Low duty cycle values //if(duty_cycle < 80.0) motor->duty_cycle = (uint16_t)0; //else motor->duty_cycle = (uint16_t)duty_cycle; @@ -371,16 +434,16 @@ void BLDC_runSpeedCntl(BLDCMotor_t *motor, volatile float speedfbk, volatile flo PID_run_parallel(&motor->controllers.Pid_Speed); if(motor->controllers.Pid_Speed.Out_pu > 0) { - motor->desiredDirection = 0; - motor->directionOffset = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; } else { - motor->desiredDirection = 1; - motor->directionOffset = 8; + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; } /* Process unit is Volts */ volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu)); volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); - motor->duty_cycle = (uint16_t)duty_cycle; + motor->motor_status.duty_cycle = (uint16_t)duty_cycle; } else { /* Pu in Current (Amps) */ motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A); @@ -403,13 +466,3 @@ void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef) PI_run_series(&motor->controllers.Pi_Pos); motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu; } - -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/bldc.h b/BLDC_E54/BLDC_E54/bldc.h index 3d91c2d..6f694fe 100644 --- a/BLDC_E54/BLDC_E54/bldc.h +++ b/BLDC_E54/BLDC_E54/bldc.h @@ -35,9 +35,9 @@ #define DEVICE_MCU_FREQUENCY_Hz (100000000U) #define DEVICE_SPEEDTC_DIV (4U) #define DEVICE_SPEEDTC_FREQUENCY_Hz (100000000U/DEVICE_SPEEDTC_DIV) -#define DEVICE_PWM_FREQUENCY_kHz (25.0f) -#define DEVICE_ISR_FREQUENCY_kHz DEVICE_PWM_FREQUENCY_kHz -#define DEVICE_ISR_PERIOD_Sec (0.001f/DEVICE_ISR_FREQUENCY_kHz) +#define DEVICE_PWM_FREQUENCY_kHz (25.0f) +#define DEVICE_ISR_FREQUENCY_kHz DEVICE_PWM_FREQUENCY_kHz +#define DEVICE_ISR_PERIOD_Sec (0.001f/DEVICE_ISR_FREQUENCY_kHz) // ---------------------------------------------------------------------- @@ -47,8 +47,9 @@ #define DEVICE_SHUNT_CURRENT_A 2.5f // phase current(PEAK) [A] #define CURRENT_SENSOR_SENSITIVITY 0.4f //V/A #define ONEON_CURRENT_SENSOR_SENSITIVITY 2.5f //V/A + // ---------------------------------------------------------------------- -// global variables +// Type Definitions // ---------------------------------------------------------------------- volatile typedef struct { @@ -74,31 +75,52 @@ volatile typedef struct timerflags volatile typedef struct { - volatile PI_t Pi_Idc; - volatile PID_t Pid_Speed; - volatile PI_t Pi_Pos; + volatile PI_t Pi_Idc; + volatile PID_t Pid_Speed; + volatile PI_t Pi_Pos; } MOTOR_Control_Structs; +volatile typedef struct +{ + volatile uint8_t desiredDirection; //! The desired direction of rotation. + volatile uint8_t directionOffset; + volatile float32_t desired_torque; + volatile int16_t desired_speed; + volatile int16_t desired_position; + volatile float32_t max_torque; + volatile float32_t max_current; + volatile int16_t max_velocity; +} MOTOR_Setpoints; + +volatile typedef struct +{ + volatile uint8_t actualDirection; //! The actual direction of rotation. + volatile uint16_t duty_cycle; + volatile float32_t calc_rpm; + volatile int32_t Num_Steps; + /* Hall States */ + volatile uint8_t prevHallPattern; + volatile uint8_t currentHallPattern; + volatile uint8_t nextHallPattern; + /* Commutation State */ + volatile uint8_t cur_comm_step; + volatile uint8_t prev_comm_step; +} MOTOR_Status; + +// ---------------------------------------------------------------------- +// Function Pointers +// ---------------------------------------------------------------------- +typedef uint8_t (*ReadHallFunc)(void); +typedef void (*DisableMotorFunc)(void); + + volatile typedef struct BLDCmotor { /* Hardware */ const Tcc *hw; - uint16_t *motor_commutation_Pattern; + const uint16_t *motor_commutation_Pattern; /* Status */ - volatile uint8_t actualDirection; //! The actual direction of rotation. - volatile uint8_t desiredDirection; //! The desired direction of rotation. - volatile uint16_t duty_cycle; - volatile float32_t calc_rpm; - volatile int32_t Num_Steps; // Total Step Count - /* Commutation State */ - volatile uint8_t cur_comm_step; - volatile uint8_t prev_comm_step; - /* Hall States */ - volatile uint8_t prevHallPattern; - volatile uint8_t currentHallPattern; - volatile uint8_t nextHallPattern; - volatile uint8_t dir_hall_code; //DIR_ABC - volatile uint8_t directionOffset; + MOTOR_Status motor_status; /* Measured Values */ volatile MOTOR_3PHASES_t Iphase_pu; volatile MOTOR_phase_offset_t Voffset_lsb; @@ -108,15 +130,12 @@ volatile typedef struct BLDCmotor volatile TIMERflags_t timerflags; volatile uint8_t regulation_loop_count; /* Controllers */ - volatile MOTOR_Control_Structs controllers; - /* Setpoints */ - volatile float32_t desired_torque; - volatile int16_t desired_speed; - volatile int16_t desired_position; - volatile float32_t max_current; - volatile float32_t max_torque; - volatile int16_t max_velocity; - + volatile MOTOR_Control_Structs controllers; + /* Setpoints */ + volatile MOTOR_Setpoints motor_setpoints; + /* Functions */ + ReadHallFunc ReadHall; + DisableMotorFunc DisableMotor; } BLDCMotor_t; @@ -124,21 +143,22 @@ volatile typedef struct BLDCmotor // global variables // ---------------------------------------------------------------------- -static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 }; -static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {20,1,3,2,5,6,4,20}; +static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 }; +static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 5, 6, 4, 9}; volatile BLDCMotor_t Motor1; - -// ---------------------------------------------------------------------- -// all controller objects, variables and helpers: -// ---------------------------------------------------------------------- - +volatile BLDCMotor_t Motor2; +volatile BLDCMotor_t Motor3; // ---------------------------------------------------------------------- // functions // ---------------------------------------------------------------------- +void BldcInitStruct(BLDCMotor_t *motor); +void BldcInitFunctions(void); + + + void select_active_phase(BLDCMotor_t *Motor, uint8_t hall_state); -void BLDC_init(BLDCMotor_t *motor); void read_zero_current_offset_value(void); int32_t adc_sync_read_channel(struct adc_async_descriptor *const descr, const uint8_t channel, uint8_t *const buffer, const uint16_t length); static uint16_t adc_read(struct adc_async_descriptor *const descr, const uint8_t channel); @@ -149,9 +169,28 @@ uint8_t get_hall_state(void); uint8_t HALLPatternGet(void); uint8_t PDEC_HALLPatternGet(void); void calculate_motor_speed(void); -void DisableGateDrivers(BLDCMotor_t *motor); +void DisableMotor(BLDCMotor_t *motor); void BLDC_runSpeedCntl(BLDCMotor_t *motor, volatile float speedfbk, volatile float speedRef); void BLDC_runCurrentCntl(BLDCMotor_t *motor, volatile float curfbk, volatile float curRef); void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef); +// ---------------------------------------------------------------------- +// Functions used with function pointers +// ---------------------------------------------------------------------- +uint8_t readM1Hall(void); +uint8_t readM2Hall(void); +uint8_t readM3Hall(void); +void DisableM1GateDrivers(BLDCMotor_t *motor); +void DisableM2GateDrivers(BLDCMotor_t *motor); +void DisableM3GateDrivers(BLDCMotor_t *motor); + +// ---------------------------------------------------------------------- +// all controller objects, variables and helpers: +// ---------------------------------------------------------------------- + +//Motor2.ReadHall = readM2Hall; +//Motor3.ReadHall = readM3Hall; + + + #endif /* BLDC_H_ */ \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/control.h b/BLDC_E54/BLDC_E54/control.h index da91def..7701f8c 100644 --- a/BLDC_E54/BLDC_E54/control.h +++ b/BLDC_E54/BLDC_E54/control.h @@ -126,7 +126,7 @@ inline void PID_objectInit(volatile PID_t *pPiD_obj) static inline void PI_run_series(volatile PI_t *pPi_obj) { PI_t *obj = (PI_t *)pPi_obj; - volatile float32_t Up, preOut; + volatile float32_t Up; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -144,7 +144,7 @@ static inline void PI_run_series(volatile PI_t *pPi_obj) static inline void PI_run_parallel(volatile PI_t *pPi_obj) { PI_t *obj = (PI_t *)pPi_obj; - volatile float32_t Up, preOut; + volatile float32_t Up; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -165,7 +165,7 @@ static inline void PI_run_parallel(volatile PI_t *pPi_obj) static inline void PID_run_series(volatile PID_t *pPid_obj) { PID_t *obj = (PID_t *)pPid_obj; - volatile float32_t Up, Ud_tmp, preOut; + volatile float32_t Up, Ud_tmp; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -189,7 +189,7 @@ static inline void PID_run_series(volatile PID_t *pPid_obj) static inline void PID_run_parallel(volatile PID_t *pPid_obj) { PID_t *obj = (PID_t *)pPid_obj; - volatile float32_t Up, Ud_tmp, preOut; + volatile float32_t Up, Ud_tmp; // Compute the controller error diff --git a/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c b/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c index f6fc7c2..6821d9b 100644 --- a/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c +++ b/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c @@ -22,27 +22,6 @@ #define DEBUG_3_PORT PORT_PC02 #define DEBUG_4_PORT PORT_PC30 -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} - -inline void tic_port(const uint32_t port) -{ - REG_PORT_OUTSET2 = port; -} - -inline void toc_port(const uint32_t port) -{ - REG_PORT_OUTCLR2 = port; -} - - void update_telemetry(void) @@ -54,17 +33,17 @@ void update_telemetry(void) *state = applicationStatus.currentstate; *status = applicationStatus.fault; - *motor_rel_position = Motor1.Num_Steps; + *motor_rel_position = Motor1.motor_status.Num_Steps; *motor_abs_position = 0; - *motor_dutyCycle = Motor1.duty_cycle; - *motor_speed = (int16_t)Motor1.calc_rpm; + *motor_dutyCycle = Motor1.motor_status.duty_cycle; + *motor_speed = (int16_t)Motor1.motor_status.calc_rpm; *motor_torque = 0; *motor_currentPHA = convert_to_mA(Motor1.Iphase_pu.A); *motor_currentPHB = convert_to_mA(Motor1.Iphase_pu.B); *motor_currentPHC = convert_to_mA(Motor1.Iphase_pu.C); *motor_currentBUS = convert_to_mA(Motor1.Iphase_pu.Bus); - *hall_state = Motor1.currentHallPattern; - *Spare_byte1 = Motor1.actualDirection; + *hall_state = Motor1.motor_status.currentHallPattern; + *Spare_byte1 = Motor1.motor_status.actualDirection; //*Spare_1 = 0; //*Spare_2 = 0; } @@ -75,27 +54,27 @@ void update_setpoints(void) { return ((float32_t)input/1000.0); } - //Motor1.des_mode = 0; - //Motor1.set = 0; - Motor1.desired_position = *desired_position; - Motor1.desired_speed = *desired_speed; - //Motor1.desired_speed = 1500; - Motor1.desired_torque = convert_int_to_PU(*desired_torque); - //Motor1.controllerParam.I_kp = 0; - //Motor1.controllerParam.I_ki = 0; - //Motor1.controllerParam.V_kp = 0; - //Motor1.controllerParam.V_kd = 0; - //Motor1.controllerParam.V_kd = 0; - //Motor1.controllerParam.P_kp = 0; - //Motor1.controllerParam.P_ki = 0; - //Motor1.reductionRatio = 0; - Motor1.max_velocity = *max_velocity; - Motor1.max_current = convert_int_to_PU(*max_current); - Motor1.max_torque = convert_int_to_PU(*max_torque); - //Motor1.Spare1 = 0; - //Motor1.Spare2 = 0; - //Motor1.Spare3 = 0; - //Motor1.Spare4 = 0; + //Motor1.des_mode = 0; + //Motor1.set = 0; + Motor1.motor_setpoints.desired_position = *desired_position; + Motor1.motor_setpoints.desired_speed = *desired_speed; + //Motor1.desired_speed = 1500; + Motor1.motor_setpoints.desired_torque = convert_int_to_PU(*desired_torque); + //Motor1.controllerParam.I_kp = 0; + //Motor1.controllerParam.I_ki = 0; + //Motor1.controllerParam.V_kp = 0; + //Motor1.controllerParam.V_kd = 0; + //Motor1.controllerParam.V_kd = 0; + //Motor1.controllerParam.P_kp = 0; + //Motor1.controllerParam.P_ki = 0; + //Motor1.reductionRatio = 0; + Motor1.motor_setpoints.max_velocity = *max_velocity; + Motor1.motor_setpoints.max_current = convert_int_to_PU(*max_current); + Motor1.motor_setpoints.max_torque = convert_int_to_PU(*max_torque); + //Motor1.Spare1 = 0; + //Motor1.Spare2 = 0; + //Motor1.Spare3 = 0; + //Motor1.Spare4 = 0; } diff --git a/BLDC_E54/BLDC_E54/interrupt_handlers.h b/BLDC_E54/BLDC_E54/interrupt_handlers.h index bda5cef..42f634c 100644 --- a/BLDC_E54/BLDC_E54/interrupt_handlers.h +++ b/BLDC_E54/BLDC_E54/interrupt_handlers.h @@ -48,7 +48,7 @@ void TC0_Handler(void) /* Overflow Interrupt */ if (TC0->COUNT32.INTFLAG.bit.OVF == 0x01) { TC0->COUNT32.INTFLAG.bit.OVF = 0x01; - Motor1.calc_rpm = 0; + Motor1.motor_status.calc_rpm = 0; //gpio_toggle_pin_level(DEBUG_2); } diff --git a/BLDC_E54/BLDC_E54/main.c b/BLDC_E54/BLDC_E54/main.c index 9499418..4d614dc 100644 --- a/BLDC_E54/BLDC_E54/main.c +++ b/BLDC_E54/BLDC_E54/main.c @@ -19,32 +19,6 @@ // ---------------------------------------------------------------------- //#define DBG_PIN1 GPIO(GPIO_PORTD, 0) -#define DEBUG_1_PORT PORT_PC01 -#define DEBUG_2_PORT PORT_PC03 -#define DEBUG_3_PORT PORT_PC02 -#define DEBUG_4_PORT PORT_PC30 - -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} - -inline void tic_port(const uint32_t port) -{ - REG_PORT_OUTSET2 = port; -} - -inline void toc_port(const uint32_t port) -{ - REG_PORT_OUTCLR2 = port; -} - - @@ -76,7 +50,8 @@ inline void CONTROLLER_StateMachine(void) break; case MOTOR_IDLE: applicationStatus.previousstate = applicationStatus.currentstate; - applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE; + //applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE; + applicationStatus.currentstate = MOTOR_V_CTRL_STATE; break; case MOTOR_I_CTRL_STATE: @@ -85,12 +60,14 @@ inline void CONTROLLER_StateMachine(void) case MOTOR_V_CTRL_STATE: switch (Motor1.regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ + Motor1.timerflags.motor_telemetry_flag = true; /* Blank */ case 6: /* PWM FREQ / 6.25 - 4kHz */ calculate_motor_speed(); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.desired_speed); + //BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, 2000); default: /* PWM FREQ - 25kHz */ - select_active_phase(&Motor1, Motor1.currentHallPattern); /* Still measure current */ + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); /* Still measure current */ break; } // end switch(regulation_loop_count) @@ -104,12 +81,13 @@ inline void CONTROLLER_StateMachine(void) case MOTOR_VI_CTRL_STATE: switch (Motor1.regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ + Motor1.timerflags.motor_telemetry_flag = true; /* Blank */ case 6: /* PWM FREQ / 6.25 - 4kHz */ calculate_motor_speed(); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.desired_speed); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed); default: /* PWM FREQ - 25kHz */ - select_active_phase(&Motor1, Motor1.currentHallPattern); + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); break; } // end switch(regulation_loop_count) @@ -123,7 +101,7 @@ inline void CONTROLLER_StateMachine(void) case 0: /* PWM FREQ / 25 - 1kHz */ Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag //tic(DEBUG_4); - BLDC_runPosCntl(&Motor1, Motor1.Num_Steps, Motor1.desired_position); + BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position); //toc(DEBUG_4); case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */ //tic(DEBUG_3); @@ -132,14 +110,14 @@ inline void CONTROLLER_StateMachine(void) calculate_motor_speed(); //toc(DEBUG_2); //tic(DEBUG_3); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); //toc(DEBUG_3); //toc(DEBUG_4); //toc(DEBUG_3); default: /* PWM FREQ - 25kHz */ //tic(DEBUG_2); //tic(DEBUG_3); - select_active_phase(&Motor1, Motor1.currentHallPattern); + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); ////toc(DEBUG_2); ////tic(DEBUG_3); BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); @@ -203,7 +181,11 @@ int main(void) atmel_start_init(); - BLDC_init(&Motor1); + BldcInitStruct(&Motor1); + BldcInitStruct(&Motor2); + BldcInitStruct(&Motor3); + BldcInitFunctions(); + read_zero_current_offset_value(); configure_tcc_pwm(); config_pins(); @@ -212,14 +194,9 @@ int main(void) ethercat_update(); custom_logic_enable(); configure_TC_CCL_SPEED(); - //ext_irq_register(PIN_PB05, button_on_PB05_pressed); - ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback); - enable_NVIC_IRQ(); // Enable TC_ECAT TC7 Timer - //__enable_irq(); - //gpio_set_pin_level(DRV_RESET, true); - //gpio_set_pin_level(DRV_RST, false); - + ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback); + enable_NVIC_IRQ(); /* Replace with your application code */ while (1) { diff --git a/BLDC_E54/BLDC_E54/motor_params.h b/BLDC_E54/BLDC_E54/motor_params.h index 6855cc7..810e79c 100644 --- a/BLDC_E54/BLDC_E54/motor_params.h +++ b/BLDC_E54/BLDC_E54/motor_params.h @@ -64,63 +64,68 @@ static uint32_t speed_average = 0; static uint8_t count = 1; +// ---------------------------------------------------------------------- +// Commutation Patterns +// ---------------------------------------------------------------------- +/** +** ____|___TCC0_________ ||____TCC1_________| +** W0 | M2_0 | CC0 || __M3_2__ | CC0 | +** W1 | M2_1 | CC1 || - | CC1 | +** W2 | M2_2 | CC2 || M1_0 | CC1 | +** W3 | M2_3 | CC3 || M1_1 | CC1 | +** W4 | __M3_0__ | CC4 || M1_2 | CC1 | +** W5 | __M3_1__ | CC5 || M1_3 | CC1 | +** W6 | M2_4 | CC0 || M1_4 | CC1 | +** W7 | M2_5 | CC1 || M1_5 | CC1 | +*/ static const uint16_t COMMUTATION_PATTERN_M1[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC + 0x00FF, // (0) invalid state + 0x30F8, // (1) + 0x50F5, // (2) + 0x60F8, // (3) + 0x607D, // (4) + 0x507D, // (5) + 0x30F5, // (6) + 0x00FF, // (7) invalid state + 0x00FF, // (8) invalid state + 0x30F5, // (9) + 0x507D, // (10) + 0x607D, // (11) + 0x60F8, // (12) + 0x50F5, // (13) + 0x30F8, // (14) + 0x00FF // (15) invalid state }; static const uint16_t COMMUTATION_PATTERN_M2[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC -}; + 0x00FF, // (0) invalid state + 0xC0FB, // (1) + 0x48DD, // (2) + 0x88FB, // (3) + 0x88EE, // (4) + 0x48EE, // (5) + 0xC0DD, // (6) + 0x00FF, // (7) invalid state + 0x00FF, // (8) invalid state + 0xC0DD, // (9) + 0x48EE, // (10) + 0x88EE, // (11) + 0x88FB, // (12) + 0x48DD, // (13) + 0xC0FB, // (14) + 0x00FF // (15) invalid state +}; + +static const uint16_t m3_TCC0_mask = 0x0030; +static const uint16_t m3_TCC0_inv_mask = ~(0x0030); +static const uint16_t m3_TCC1_mask = 0x0001; +static const uint16_t m3_TCC1_inv_mask = ~(0x0001); + + + + -static const uint16_t COMMUTATION_PATTERN_M3[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC -}; #endif /* MOTOR_PARAMS_H_ */ \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/utilities.h b/BLDC_E54/BLDC_E54/utilities.h index a58fbf9..1b9be19 100644 --- a/BLDC_E54/BLDC_E54/utilities.h +++ b/BLDC_E54/BLDC_E54/utilities.h @@ -9,6 +9,37 @@ #ifndef UTILITIES_H_ #define UTILITIES_H_ + +#define ENABLED(V...) DO(ENA,&&,V) +#define DISABLED(V...) DO(DIS,&&,V) + + +#define DEBUG_1_PORT PORT_PC01 +#define DEBUG_2_PORT PORT_PC03 +#define DEBUG_3_PORT PORT_PC02 +#define DEBUG_4_PORT PORT_PC30 + +inline void tic(const uint8_t pin) +{ + gpio_set_pin_level(pin, true); +} + +inline void toc(const uint8_t pin) +{ + gpio_set_pin_level(pin, false); +} + +inline void tic_port(const uint32_t port) +{ + REG_PORT_OUTSET2 = port; +} + +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