From 6f04b2e58cd10fd25bda59c532173aa926cd7fa2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 6 Feb 2017 23:06:35 +0100 Subject: [PATCH] Implement brake resistor control --- Inc/main.h | 1 + MotorControl/low_level.c | 53 +++++++++++++++++++++++++++++++--------- MotorControl/low_level.h | 1 + Odrive.ioc | 2 +- Src/tim.c | 2 +- 5 files changed, 46 insertions(+), 13 deletions(-) diff --git a/Inc/main.h b/Inc/main.h index 36d63817..a6fff1ee 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -57,6 +57,7 @@ #define TIM_APB1_CLOCK_HZ 84000000 // #define TIM_APB1_PERIOD_CLOCKS 2048 #define TIM_APB1_PERIOD_CLOCKS 4096 +#define TIM_APB1_DEADTIME_CLOCKS 10 #define M0_nCS_Pin GPIO_PIN_13 #define M0_nCS_GPIO_Port GPIOC diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index c44657aa..dc105196 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -30,13 +30,14 @@ float vbus_voltage = 12.0f; //Arbitrary non-zero inital value to avoid division by zero if ADC reading is late //@TODO: Migrate to C++, clearly we are actually doing object oriented code here... +//@TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { { //M0 .control_mode = CURRENT_CONTROL, .pos_setpoint = 0.0f, .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 10.0f / 10000.0f, // [A/(counts/s)] .vel_limit = 10000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .motor_thread = 0, @@ -62,7 +63,8 @@ Motor_t motors[] = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f }, .rotor = { .encoder_timer = &htim3, @@ -80,7 +82,7 @@ Motor_t motors[] = { .pos_setpoint = 0.0f, .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 10.0f / 10000.0f, // [A/(counts/s)] .vel_limit = 10000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .motor_thread = 0, @@ -106,7 +108,8 @@ Motor_t motors[] = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f }, .rotor = { .encoder_timer = &htim4, @@ -130,12 +133,13 @@ static const float sqrt3_by_2 = 0.86602540378; //Local view of DRV registers //@TODO: Include these in motor object instead static DRV_SPI_8301_Vars_t gate_driver_regs[2/*num_motors*/]; +static float brake_resistance = 0.5; // [ohm] //Log to store the timing of calls to check_timing //This is used in various places, so be sure to look for all the places it is written #define TIMING_LOG_SIZE 32 -static volatile uint16_t timing_logs[2/*num_motors*/][TIMING_LOG_SIZE]; -static volatile int timing_log_index[2/*num_motors*/] = {0, 0}; +static volatile uint16_t timing_logs[ 2 /*num_motors*/][TIMING_LOG_SIZE]; +static volatile int timing_log_index[ 2 /*num_motors*/] = {0, 0}; /* Private function prototypes -----------------------------------------------*/ static void DRV8301_setup(Motor_t* motor, DRV_SPI_8301_Vars_t* local_regs); @@ -534,6 +538,24 @@ static void update_rotor(Rotor_t* rotor) { rotor->pll_vel += CURRENT_MEAS_PERIOD * rotor->pll_ki * delta_pos; } +static void update_brake_current(float brake_current) { + if (brake_current < 0.0f) brake_current = 0.0f; + float brake_duty = brake_current * brake_resistance / vbus_voltage; + + // Duty limit at 90% to allow bootstrap caps to charge + if (brake_duty > 0.9f) brake_duty = 0.9f; + int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; +} //-------------------------------- // Measurement and calibration @@ -706,7 +728,6 @@ static void FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float Ierr_q = Iq_des - Iq; //@TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) - //@TODO current limit //Apply PI control float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; @@ -729,7 +750,17 @@ static void FOC_current(Motor_t* motor, float Id_des, float Iq_des) { } // Compute estimated bus current - // *IbusEst = mod_d * Id + mod_q * Iq; + ictrl->Ibus = mod_d * Id + mod_q * Iq; + + // If this is last motor, update brake resistor duty + if (motor == &motors[num_motors-1]) { + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + //Note: function will clip negative values to 0.0f + update_brake_current(-Ibus_sum); + } // Inverse park transform float mod_alpha = c*mod_d - s*mod_q; @@ -811,9 +842,9 @@ void motor_thread(void const * argument) { // motors[0].vel_setpoint = 10000.0f; // [counts/s] // motors[0].control_mode = VELOCITY_CONTROL; - // // Position test - // motors[0].pos_setpoint = 50000.0f; // [counts/s] - // motors[0].control_mode = POSITION_CONTROL; + // Position test + motors[0].pos_setpoint = 50000.0f; // [counts/s] + motors[0].control_mode = POSITION_CONTROL; control_motor_loop(motor); diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index c0a88119..f339bc1d 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -29,6 +29,7 @@ typedef struct { float i_gain; // [V/As] float v_current_control_integral_d; // [V] float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] } Current_control_t; typedef struct { diff --git a/Odrive.ioc b/Odrive.ioc index c043c3c0..78091117 100755 --- a/Odrive.ioc +++ b/Odrive.ioc @@ -157,7 +157,7 @@ Mcu.Pin7=PC2 Mcu.Pin8=PC3 Mcu.Pin9=PA0-WKUP Mcu.PinsNb=55 -Mcu.UserConstants=TIM_1_8_CLOCK_HZ,168000000;TIM_1_8_PERIOD_CLOCKS,8192;TIM_1_8_DEADTIME_CLOCKS,20;TIM_APB1_CLOCK_HZ,84000000;TIM_APB1_PERIOD_CLOCKS,4096 +Mcu.UserConstants=TIM_1_8_CLOCK_HZ,168000000;TIM_1_8_PERIOD_CLOCKS,8192;TIM_1_8_DEADTIME_CLOCKS,20;TIM_APB1_CLOCK_HZ,84000000;TIM_APB1_PERIOD_CLOCKS,4096;TIM_APB1_DEADTIME_CLOCKS,10 Mcu.UserName=STM32F405RGTx MxCube.Version=4.19.0 MxDb.Version=DB.4.0.190 diff --git a/Src/tim.c b/Src/tim.c index 3e9c0723..3a00ca75 100644 --- a/Src/tim.c +++ b/Src/tim.c @@ -184,7 +184,7 @@ void MX_TIM2_Init(void) } sConfigOC.OCMode = TIM_OCMODE_PWM2; - sConfigOC.Pulse = TIM_APB1_PERIOD_CLOCKS/2; + sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)