647 lines
23 KiB
C
647 lines
23 KiB
C
/*
|
|
* bldc.c
|
|
*
|
|
* Created: 10/03/2021 14:53:19
|
|
* Author: Nick-XMG
|
|
*/
|
|
|
|
|
|
#include "bldc.h"
|
|
#include "utilities.h"
|
|
// ----------------------------------------------------------------------
|
|
// header files
|
|
// ----------------------------------------------------------------------
|
|
#include "arm_math.h"
|
|
#include "statemachine.h"
|
|
#include "utilities.h"
|
|
|
|
|
|
//// ------------------------------------------------------------------------------
|
|
// Current Selection Based on Hall State
|
|
// Direction":
|
|
// CW -> Always positive current
|
|
// CCW -> Always negative current
|
|
void select_active_phase(BLDCMotor_t *Motor, const uint8_t hall_state)
|
|
{
|
|
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 BldcInitStruct(BLDCMotor_t *motor)
|
|
{
|
|
// ----------------------------------------------------------------------
|
|
// Initialize all voltage and current objects, variables and helpers:
|
|
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->Iphase_pu.A = 0;
|
|
motor->Iphase_pu.B = 0;
|
|
motor->Iphase_pu.C = 0;
|
|
motor->Iphase_pu.Bus = 0;
|
|
motor->Voffset_lsb.A = 0;
|
|
motor->Voffset_lsb.B = 0;
|
|
motor->timerflags.pwm_cycle_tic = false;
|
|
motor->timerflags.control_loop_tic = false;
|
|
motor->timerflags.motor_telemetry_flag = false;
|
|
motor->timerflags.control_loop_tic = false;
|
|
motor->timerflags.adc_readings_ready_tic = false;
|
|
|
|
motor->regulation_loop_count = 0;
|
|
|
|
|
|
motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V;
|
|
motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V;
|
|
motor->motor_commutation_Pattern = COMMUTATION_PATTERN_M1;
|
|
|
|
|
|
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
|
|
PI_objectInit(&motor->controllers.Pi_Idc);
|
|
float32_t motorLs_H = MOTOR_LQ_H * 2.0f;
|
|
float32_t motorRs_OHM = MOTOR_RS_OHM * 2.0f;
|
|
motor->controllers.Pi_Idc.Kp = PI_calcKp(motorLs_H, DEVICE_SHUNT_CURRENT_A, DEVICE_DC_VOLTAGE_V, DEVICE_ISR_PERIOD_Sec);
|
|
//Pi_Idc.Ki = 0;
|
|
motor->controllers.Pi_Idc.Ki = PI_calcKi(motorRs_OHM, motorLs_H, DEVICE_ISR_PERIOD_Sec);
|
|
|
|
//// ------------------------------------------------------------------------------
|
|
//// pi velocity control init
|
|
PID_objectInit(&motor->controllers.Pid_Speed);
|
|
|
|
/* V only Control Gains */
|
|
//motor->controllers.Pid_Speed.Kp = 0.005f;
|
|
//motor->controllers.Pid_Speed.Ki = 0.01f;
|
|
|
|
/* VI Control Gains */
|
|
//motor->controllers.Pid_Speed.Kp = 0.0003f;
|
|
//motor->controllers.Pid_Speed.Ki = 0.001f;
|
|
motor->controllers.Pid_Speed.Kp = 0.0005f;
|
|
motor->controllers.Pid_Speed.Ki = 0.0f;
|
|
motor->controllers.Pid_Speed.Kd = 0.0001f;
|
|
|
|
//motor->controllers.Pid_Speed.Ki = 0.0001f;
|
|
motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A);
|
|
motor->controllers.Pid_Speed.OutMin_pu = -(MOTOR_MAX_CURRENT_IDC_A);
|
|
|
|
//// ------------------------------------------------------------------------------
|
|
//// pi position control init
|
|
PI_objectInit(&motor->controllers.Pi_Pos);
|
|
motor->controllers.Pi_Pos.Kp = 40.0f;
|
|
motor->controllers.Pi_Pos.Ki = 0.0f;
|
|
|
|
//motor->controllers.Pi_Pos.Ki = 0.00015f;
|
|
//Pi_Pos.Kp = 500000;
|
|
//Pi_Pos.Kp = 0;
|
|
|
|
motor->controllers.Pi_Pos.OutMax_pu = MOTOR_MAX_SPD_RPM;
|
|
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;
|
|
//Motor1.SetDutyCycle = SetM1DutyCycle;
|
|
//Motor2.SetDutyCycle = SetM2DutyCycle;
|
|
//Motor3.SetDutyCycle = SetM3DutyCycle;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------
|
|
// Before Power up, Measure and average offset voltage for Current Sensor
|
|
// This voltage is subtracted from all reading for Bi-Directional Current
|
|
// Measurement
|
|
// ----------------------------------------------------------------------
|
|
void read_zero_current_offset_value(BLDCMotor_t *motor)
|
|
{
|
|
uint32_t phase_A_zero_current_offset_temp = 0;
|
|
uint32_t phase_B_zero_current_offset_temp = 0;
|
|
volatile uint16_t zero_current_offset_temp[2] = {0,0};
|
|
uint8_t samples = 16;
|
|
uint8_t i;
|
|
|
|
adc_sync_enable_channel(&ADC_0, 0);
|
|
//adc_sync_enable_channel(&ADC_1, 0);
|
|
|
|
ADC0->INPUTCTRL.reg = 0x1802;
|
|
while (ADC0->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
|
|
|
|
for (i=0; i<samples; i++)
|
|
{
|
|
volatile uint16_t zero_current_offset_temp[2] = {0,0};
|
|
|
|
while (ADC0->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
|
|
ADC0->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
|
|
while (ADC0->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
|
|
ADC0->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
|
|
zero_current_offset_temp[0] = ADC0->RESULT.reg; /* Read the value. */
|
|
|
|
phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
|
|
}
|
|
|
|
/* Set Motor Variables */
|
|
motor->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
|
|
|
|
ADC0->INPUTCTRL.reg = 0x1803;
|
|
while (ADC0->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
|
|
|
|
for (i=0; i<samples; i++)
|
|
{
|
|
while (ADC0->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
|
|
ADC0->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
|
|
while (ADC0->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
|
|
ADC0->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
|
|
zero_current_offset_temp[1] = ADC0->RESULT.reg; /* Read the value. */
|
|
|
|
phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
|
|
}
|
|
|
|
/* Set Motor Variables */
|
|
motor->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
|
|
adc_sync_disable_channel(&ADC_0, 0);
|
|
//adc_sync_disable_channel(&ADC_1, 0);
|
|
}
|
|
|
|
//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
|
|
// ----------------------------------------------------------------------
|
|
|
|
inline uint8_t readM1Hall(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));
|
|
//
|
|
//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));
|
|
}
|
|
|
|
inline uint8_t readM2Hall(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));
|
|
|
|
|
|
//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));
|
|
}
|
|
|
|
inline uint8_t readM3Hall(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));
|
|
|
|
//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)
|
|
{
|
|
|
|
}
|
|
|
|
inline void SetM1DutyCycle(const uint16_t duty)
|
|
{
|
|
TCC1->CCBUF[1].reg = duty;
|
|
}
|
|
|
|
inline void SetM2DutyCycle(const uint16_t duty)
|
|
{
|
|
TCC0->CCBUF[0].reg = duty;
|
|
TCC0->CCBUF[1].reg = duty;
|
|
TCC0->CCBUF[2].reg = duty;
|
|
TCC0->CCBUF[3].reg = duty;
|
|
}
|
|
|
|
inline void SetM3DutyCycle(const uint16_t duty)
|
|
{
|
|
TCC0->CCBUF[4].reg = duty;
|
|
TCC0->CCBUF[5].reg = duty;
|
|
TCC1->CCBUF[0].reg = duty;
|
|
}
|
|
|
|
/**
|
|
** ____|___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 |
|
|
*/
|
|
|
|
void exec_commutation(void)
|
|
{
|
|
// ----------------------------------------------------------------------
|
|
// Read Motor Hall Sensors
|
|
// ----------------------------------------------------------------------
|
|
//tic_port(DEBUG_2_PORT);
|
|
Motor1.motor_status.currentHallPattern = readM1Hall();
|
|
Motor2.motor_status.currentHallPattern = readM2Hall();
|
|
//toc_port(DEBUG_2_PORT);
|
|
//Motor2.motor_status.currentHallPattern = Motor1.ReadHall();
|
|
//Motor3.motor_status.currentHallPattern = Motor1.ReadHall();
|
|
//toc_port(DEBUG_3_PORT);
|
|
// ----------------------------------------------------------------------
|
|
// 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 + Motor2.motor_setpoints.directionOffset];
|
|
//volatile uint16_t temp_M3_tcc1_des = COMMUTATION_PATTERN_M1[Motor3.motor_status.currentHallPattern + Motor3.motor_setpoints.directionOffset] & m3_TCC1_mask;
|
|
//volatile uint16_t temp_M3_tcc0_des = COMMUTATION_PATTERN_M2[Motor3.motor_status.currentHallPattern + Motor3.motor_setpoints.directionOffset] & m3_TCC0_mask;
|
|
/* Zero target bits */
|
|
//temp_M1 &= m3_TCC1_inv_mask;
|
|
//temp_M2 &= m3_TCC0_inv_mask;
|
|
/* Set Desired bits */
|
|
//temp_M1 |= temp_M3_tcc1_des;
|
|
//temp_M2 |= temp_M3_tcc0_des;
|
|
|
|
// ----------------------------------------------------------------------
|
|
// Set Pattern Buffers
|
|
// ----------------------------------------------------------------------
|
|
TCC0->PATTBUF.reg = (uint16_t)temp_M2;
|
|
TCC1->PATTBUF.reg = (uint16_t)temp_M1;
|
|
|
|
//toc_port(DEBUG_2_PORT);
|
|
//toc_port(DEBUG_2_PORT);
|
|
// ----------------------------------------------------------------------
|
|
// Set Remaining GPIO lines responsible for M3 Commutation
|
|
// ----------------------------------------------------------------------
|
|
///* GPIO En Pin Setting for M3 */
|
|
//switch(Motor3.motor_status.currentHallPattern + Motor3.motor_setpoints.directionOffset)
|
|
//{
|
|
//// REG_PORT_OUTSET0 = Port A
|
|
//// REG_PORT_OUTSET1 = Port B
|
|
//case 1: case 6: case 9: case 14:
|
|
//REG_PORT_OUTCLR0 = (1 << GPIO_PIN(M3_3));
|
|
//REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_4))|(1 << GPIO_PIN(M3_5));
|
|
//break;
|
|
//case 2: case 5: case 10: case 13:
|
|
//REG_PORT_OUTSET0 = (1 << GPIO_PIN(M3_3));
|
|
//REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_4));
|
|
//REG_PORT_OUTCLR1 = (1 << GPIO_PIN(M3_5));
|
|
//break;
|
|
//case 3: case 4: case 11: case 12:
|
|
//REG_PORT_OUTSET0 = (1 << GPIO_PIN(M3_3));
|
|
//REG_PORT_OUTCLR1 = (1 << GPIO_PIN(M3_4));
|
|
//REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_5));
|
|
//break;
|
|
//default:
|
|
///*hall error - should not get here */
|
|
//break;
|
|
//}
|
|
|
|
|
|
// TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.motor_status.currentHallPattern +
|
|
// Motor1.motor_setpoints.directionOffset)];
|
|
// ----------------------------------------------------------------------
|
|
// Set Calculated Duty Cycles
|
|
// ----------------------------------------------------------------------
|
|
//tic_port(DEBUG_2_PORT);
|
|
//Motor1.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle);
|
|
SetM1DutyCycle(Motor1.motor_status.duty_cycle);
|
|
SetM2DutyCycle(Motor1.motor_status.duty_cycle);
|
|
//Motor2.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle);
|
|
//Motor3.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle);
|
|
//tic_port(DEBUG_2_PORT);
|
|
//tic_port(DEBUG_2_PORT);
|
|
Motor1.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern];
|
|
volatile int8_t step_change1 = Motor1.motor_status.cur_comm_step - Motor1.motor_status.prev_comm_step;
|
|
|
|
switch(step_change1)
|
|
{
|
|
case 1: case -5:
|
|
Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps+1;
|
|
Motor1.motor_status.actualDirection = CW;
|
|
//Motor1.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
|
|
break;
|
|
case -1: case 5:
|
|
Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps-1;
|
|
Motor1.motor_status.actualDirection = CCW;
|
|
//Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
|
|
break;
|
|
default:
|
|
// do nothing
|
|
break;
|
|
}
|
|
Motor1.motor_status.prev_comm_step = Motor1.motor_status.cur_comm_step;
|
|
//toc_port(DEBUG_2_PORT);
|
|
|
|
Motor2.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern];
|
|
volatile int8_t step_change2 = Motor2.motor_status.cur_comm_step - Motor2.motor_status.prev_comm_step;
|
|
|
|
switch(step_change2)
|
|
{
|
|
case 1: case -5:
|
|
Motor2.motor_status.Num_Steps = Motor2.motor_status.Num_Steps+1;
|
|
Motor2.motor_status.actualDirection = CW;
|
|
//Motor2.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
|
|
break;
|
|
case -1: case 5:
|
|
Motor2.motor_status.Num_Steps = Motor2.motor_status.Num_Steps-1;
|
|
Motor2.motor_status.actualDirection = CCW;
|
|
//Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
|
|
break;
|
|
default:
|
|
// do nothing
|
|
break;
|
|
}
|
|
|
|
//toc_port(DEBUG_2_PORT);
|
|
//
|
|
//Motor3.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern];
|
|
//volatile int8_t step_change3 = Motor3.motor_status.cur_comm_step - Motor3.motor_status.prev_comm_step;
|
|
//
|
|
//switch(step_change3)
|
|
//{
|
|
//case 1: case -5:
|
|
//Motor3.motor_status.Num_Steps = Motor3.motor_status.Num_Steps+1;
|
|
//Motor3.motor_status.actualDirection = CW;
|
|
////Motor3.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
|
|
//break;
|
|
//case -1: case 5:
|
|
//Motor3.motor_status.Num_Steps = Motor3.motor_status.Num_Steps-1;
|
|
//Motor3.motor_status.actualDirection = CCW;
|
|
////Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
|
|
//break;
|
|
//default:
|
|
//// do nothing
|
|
//break;
|
|
//}
|
|
|
|
//calculate_motor_speed();
|
|
//toc(DEBUG_3);
|
|
//toc(DEBUG_4);
|
|
//CRITICAL_SECTION_LEAVE();
|
|
}
|
|
|
|
void calculate_motor_speed(void)
|
|
{
|
|
//tic_port(DEBUG_2_PORT);
|
|
volatile uint32_t temp_rpm = 0;
|
|
hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/
|
|
volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1);
|
|
if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) {
|
|
Motor1.motor_status.calc_rpm = 0;
|
|
} else {
|
|
//uint32_t test = (SPEEDFACTOR, period_after_capture);
|
|
//temp_rpm = SPEEDFACTOR / period_after_capture;
|
|
//tic_port(DEBUG_3_PORT);
|
|
temp_rpm = HZ_TO_RPM / (period_after_capture*MOTOR_COMUTATION_STATES);
|
|
//temp_rpm = HZ_TO_RPM * period_after_capture;
|
|
//toc_port(DEBUG_3_PORT);
|
|
}
|
|
|
|
if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */
|
|
{
|
|
//*motor_speed = -1* Motor1.calc_rpm;
|
|
temp_rpm = -1 * temp_rpm;
|
|
} else {
|
|
//*motor_speed = Motor1.calc_rpm;
|
|
temp_rpm = temp_rpm;
|
|
}
|
|
|
|
Motor1.motor_status.calc_rpm = (int16_t)temp_rpm;
|
|
//toc_port(DEBUG_2_PORT);
|
|
|
|
//#ifdef AVERAGE_SPEED_MEASURE
|
|
//// To avoid noise an average is realized on 8 samples
|
|
//speed_average += temp_rpm;
|
|
//if(count >= n_SAMPLE)
|
|
//{
|
|
//count = 1;
|
|
//Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32
|
|
////Motor1.motor_status.calc_rpm = (speed_average); // divide by 32
|
|
//speed_average = 0;
|
|
////*Spare_byte1 = motorState.actualDirection;
|
|
//if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */
|
|
//{
|
|
////*motor_speed = -1* Motor1.calc_rpm;
|
|
//Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm;
|
|
//} else {
|
|
////*motor_speed = Motor1.calc_rpm;
|
|
//Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm;
|
|
//}
|
|
//return;
|
|
//}
|
|
//else count++;
|
|
//#else
|
|
//Motor1.motor_status.calc_rpm = (int16_t)temp_rpm;
|
|
//#endif
|
|
//toc_port(DEBUG_2_PORT);
|
|
}
|
|
|
|
void DisableMotor(BLDCMotor_t *motor)
|
|
{
|
|
hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]);
|
|
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
// pi current control
|
|
//------------------------------------------------------------------------------
|
|
void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t curRef)
|
|
{
|
|
|
|
motor->controllers.Pi_Idc.Fbk_pu = f_clamp(curfbk, -DEVICE_SHUNT_CURRENT_A, DEVICE_SHUNT_CURRENT_A); // Clamped to max current sensor readingspeedfbk;
|
|
motor->controllers.Pi_Idc.Ref_pu = f_clamp(curRef, -MOTOR_MAX_CURRENT_IDC_A, MOTOR_MAX_CURRENT_IDC_A); // Clamp desired to Motor Max Current i_ref_clamped;
|
|
|
|
//*Spare_1 = (int16_t)(motor->controllers.Pi_Idc.Ref_pu * 1000); // Send Req Current to TC
|
|
|
|
motor->controllers.Pi_Idc.OutMax_pu = motor->VdcBus_pu;
|
|
motor->controllers.Pi_Idc.OutMin_pu = -motor->VdcBus_pu;
|
|
PI_run_series(&motor->controllers.Pi_Idc);
|
|
|
|
if(motor->controllers.Pi_Idc.Out_pu > 0) {
|
|
motor->motor_setpoints.desiredDirection = 0;
|
|
motor->motor_setpoints.directionOffset = 0;
|
|
} else {
|
|
motor->motor_setpoints.desiredDirection = 1;
|
|
motor->motor_setpoints.directionOffset = 8;
|
|
}
|
|
|
|
volatile float32_t duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu));
|
|
motor->motor_status.duty_cycle = (uint16_t)f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM);
|
|
// Remove Low duty cycle values
|
|
//if(duty_cycle < 80.0) motor->duty_cycle = (uint16_t)0;
|
|
//else motor->duty_cycle = (uint16_t)duty_cycle;
|
|
|
|
}
|
|
|
|
//// ------------------------------------------------------------------------------
|
|
// Speed Control: Input in RPM units (int16_t)
|
|
//------------------------------------------------------------------------------
|
|
void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float32_t speedRef)
|
|
{
|
|
//tic_port(DEBUG_3_PORT);
|
|
motor->controllers.Pid_Speed.Fbk_pu = speedfbk;
|
|
motor->controllers.Pid_Speed.Ref_pu = f_clamp(speedRef, -MOTOR_MAX_SPD_RPM, MOTOR_MAX_SPD_RPM); // Convert Speed Ref to Q16 Format
|
|
|
|
if (applicationStatus.currentstate == MOTOR_V_CTRL_STATE)
|
|
{
|
|
/* Output Pu in Volts (PWM %) */
|
|
motor->controllers.Pid_Speed.OutMax_pu = motor->VdcBus_pu;
|
|
motor->controllers.Pid_Speed.OutMin_pu = -motor->VdcBus_pu;
|
|
|
|
PID_run_parallel(&motor->controllers.Pid_Speed);
|
|
|
|
if(motor->controllers.Pid_Speed.Out_pu > 0) {
|
|
motor->motor_setpoints.desiredDirection = 0;
|
|
motor->motor_setpoints.directionOffset = 0;
|
|
} else {
|
|
motor->motor_setpoints.desiredDirection = 1;
|
|
motor->motor_setpoints.directionOffset = 8;
|
|
}
|
|
/* Process unit is Volts */
|
|
volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu));
|
|
volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM);
|
|
motor->motor_status.duty_cycle = (uint16_t)duty_cycle;
|
|
} else {
|
|
/* Pu in Current (Amps) */
|
|
motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A);
|
|
motor->controllers.Pid_Speed.OutMin_pu = -(MOTOR_MAX_CURRENT_IDC_A);
|
|
PID_run_parallel(&motor->controllers.Pid_Speed);
|
|
motor->controllers.Pi_Idc.Ref_pu = motor->controllers.Pid_Speed.Out_pu;
|
|
}
|
|
//toc_port(DEBUG_3_PORT);
|
|
}
|
|
|
|
//// ------------------------------------------------------------------------------
|
|
// Position Control:Input in Hall step units (int16_t)
|
|
//------------------------------------------------------------------------------
|
|
void BLDC_runPosCntl(BLDCMotor_t *motor, const int16_t posfbk, const int16_t posRef)
|
|
{
|
|
/* Output Pu in RPM */
|
|
motor->controllers.Pi_Pos.OutMax_pu = MOTOR_MAX_SPD_RPM;
|
|
motor->controllers.Pi_Pos.OutMin_pu = -MOTOR_MAX_SPD_RPM;
|
|
motor->controllers.Pi_Pos.Fbk_pu = posfbk;
|
|
motor->controllers.Pi_Pos.Ref_pu = posRef;
|
|
PI_run_series(&motor->controllers.Pi_Pos);
|
|
motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu;
|
|
}
|