mirror of https://github.com/rusefi/bldc.git
FW 1.16: Hall sensor position control, brake at 0RPM setpoint in FOC, FOC parameter detection fix
This commit is contained in:
parent
d52906fbe8
commit
08a27cfbbd
|
@ -324,11 +324,12 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.0;
|
||||
|
||||
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.cc_ramp_step_max = (float)buffer_get_int32(data, &ind) / 1000000.0;
|
||||
mcconf.cc_ramp_step_max = buffer_get_float32(data, 1e5, &ind);
|
||||
|
||||
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
|
||||
mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
|
||||
|
@ -431,6 +432,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
|
||||
buffer_append_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &ind);
|
||||
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
|
||||
|
@ -853,7 +855,7 @@ void commands_send_app_data(unsigned char *data, unsigned int len) {
|
|||
int32_t index = 0;
|
||||
|
||||
send_buffer[index++] = COMM_CUSTOM_APP_DATA;
|
||||
memcpy(send_buffer, data, len);
|
||||
memcpy(send_buffer + index, data, len);
|
||||
index += len;
|
||||
|
||||
commands_send_packet(send_buffer, index);
|
||||
|
|
|
@ -38,5 +38,6 @@ void commands_send_rotor_pos(float rotor_pos);
|
|||
void commands_send_experiment_samples(float *samples, int len);
|
||||
disp_pos_mode commands_get_disp_pos_mode(void);
|
||||
void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
|
||||
void commands_send_app_data(unsigned char *data, unsigned int len);
|
||||
|
||||
#endif /* COMMANDS_H_ */
|
||||
|
|
|
@ -228,6 +228,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
|
|||
conf->p_pid_kp = MCCONF_P_PID_KP;
|
||||
conf->p_pid_ki = MCCONF_P_PID_KI;
|
||||
conf->p_pid_kd = MCCONF_P_PID_KD;
|
||||
conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
|
||||
|
||||
conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;
|
||||
conf->cc_min_current = MCCONF_CC_MIN_CURRENT;
|
||||
|
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
// Firmware version
|
||||
#define FW_VERSION_MAJOR 2
|
||||
#define FW_VERSION_MINOR 15
|
||||
#define FW_VERSION_MINOR 16
|
||||
|
||||
#include "datatypes.h"
|
||||
|
||||
|
@ -64,10 +64,7 @@
|
|||
/*
|
||||
* Select default user motor configuration
|
||||
*/
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_outrunner2.h"
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_foc_erwin.h"
|
||||
//#define MCCONF_DEFAULT_USER "mcconf_foc_scorpion.h"
|
||||
|
||||
/*
|
||||
* Select default user app configuration
|
||||
|
|
|
@ -90,7 +90,8 @@ typedef enum {
|
|||
DISP_POS_MODE_INDUCTANCE,
|
||||
DISP_POS_MODE_OBSERVER,
|
||||
DISP_POS_MODE_ENCODER,
|
||||
DISP_POS_MODE_ENCODER_POS_ERROR,
|
||||
DISP_POS_MODE_PID_POS,
|
||||
DISP_POS_MODE_PID_POS_ERROR,
|
||||
DISP_POS_MODE_ENCODER_OBSERVER_ERROR
|
||||
} disp_pos_mode;
|
||||
|
||||
|
@ -181,6 +182,7 @@ typedef struct {
|
|||
float p_pid_kp;
|
||||
float p_pid_ki;
|
||||
float p_pid_kd;
|
||||
float p_pid_ang_div;
|
||||
// Current controller
|
||||
float cc_startup_boost_duty;
|
||||
float cc_min_current;
|
||||
|
|
8
main.c
8
main.c
|
@ -110,8 +110,12 @@ static THD_FUNCTION(periodic_thread, arg) {
|
|||
commands_send_rotor_pos(encoder_read_deg());
|
||||
break;
|
||||
|
||||
case DISP_POS_MODE_ENCODER_POS_ERROR:
|
||||
commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pos_set(), encoder_read_deg()));
|
||||
case DISP_POS_MODE_PID_POS:
|
||||
commands_send_rotor_pos(mc_interface_get_pid_pos_now());
|
||||
break;
|
||||
|
||||
case DISP_POS_MODE_PID_POS_ERROR:
|
||||
commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pid_pos_set(), mc_interface_get_pid_pos_now()));
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
@ -33,6 +33,7 @@
|
|||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "commands.h"
|
||||
#include "encoder.h"
|
||||
#include <math.h>
|
||||
|
||||
// Global variables
|
||||
|
@ -741,10 +742,30 @@ float mc_interface_read_reset_avg_input_current(void) {
|
|||
return res;
|
||||
}
|
||||
|
||||
float mc_interface_get_pos_set(void) {
|
||||
float mc_interface_get_pid_pos_set(void) {
|
||||
return m_position_set;
|
||||
}
|
||||
|
||||
float mc_interface_get_pid_pos_now(void) {
|
||||
float ret = 0.0;
|
||||
|
||||
switch (m_conf.motor_type) {
|
||||
case MOTOR_TYPE_BLDC:
|
||||
case MOTOR_TYPE_DC:
|
||||
ret = encoder_read_deg();
|
||||
break;
|
||||
|
||||
case MOTOR_TYPE_FOC:
|
||||
ret = mcpwm_foc_get_pid_pos_now();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
float mc_interface_get_last_sample_adc_isr_duration(void) {
|
||||
return m_last_adc_duration_sample;
|
||||
}
|
||||
|
|
|
@ -65,7 +65,8 @@ int mc_interface_get_tachometer_abs_value(bool reset);
|
|||
float mc_interface_get_last_inj_adc_isr_duration(void);
|
||||
float mc_interface_read_reset_avg_motor_current(void);
|
||||
float mc_interface_read_reset_avg_input_current(void);
|
||||
float mc_interface_get_pos_set(void);
|
||||
float mc_interface_get_pid_pos_set(void);
|
||||
float mc_interface_get_pid_pos_now(void);
|
||||
float mc_interface_get_last_sample_adc_isr_duration(void);
|
||||
void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
|
||||
|
||||
|
|
|
@ -121,6 +121,9 @@
|
|||
#ifndef MCCONF_P_PID_KD
|
||||
#define MCCONF_P_PID_KD 0.0004 // Derivative gain
|
||||
#endif
|
||||
#ifndef MCCONF_P_PID_ANG_DIV
|
||||
#define MCCONF_P_PID_ANG_DIV 1.0 // Divide angle by this value
|
||||
#endif
|
||||
|
||||
// Current control parameters
|
||||
#ifndef MCCONF_CC_GAIN
|
||||
|
@ -217,7 +220,7 @@
|
|||
#define MCCONF_FOC_PLL_KP 2000.0
|
||||
#endif
|
||||
#ifndef MCCONF_FOC_PLL_KI
|
||||
#define MCCONF_FOC_PLL_KI 40000.0
|
||||
#define MCCONF_FOC_PLL_KI 20000.0
|
||||
#endif
|
||||
#ifndef MCCONF_FOC_MOTOR_L
|
||||
#define MCCONF_FOC_MOTOR_L 0.000007
|
||||
|
|
|
@ -1,36 +0,0 @@
|
|||
/*
|
||||
* mcconf_foc_erwin.h
|
||||
*
|
||||
* Created on: 25 okt 2015
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#ifndef MCCONF_FOC_ERWIN_H_
|
||||
#define MCCONF_FOC_ERWIN_H_
|
||||
|
||||
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
|
||||
|
||||
#define MCCONF_FOC_CURRENT_KP 0.05
|
||||
#define MCCONF_FOC_CURRENT_KI 50.0
|
||||
#define MCCONF_FOC_F_SW 50000.0
|
||||
#define MCCONF_FOC_ENCODER_INVERTED true
|
||||
#define MCCONF_FOC_ENCODER_OFFSET 92.0
|
||||
#define MCCONF_FOC_ENCODER_RATIO 7.0
|
||||
#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_ENCODER
|
||||
#define MCCONF_FOC_PLL_KP 40.0
|
||||
#define MCCONF_FOC_PLL_KI 40000.0
|
||||
#define MCCONF_FOC_MOTOR_L 0.000064
|
||||
#define MCCONF_FOC_MOTOR_R 0.038
|
||||
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.0085
|
||||
#define MCCONF_FOC_OBSERVER_GAIN 9e6 // Can be something like 600 / L
|
||||
|
||||
#define MCCONF_S_PID_KP 0.0001
|
||||
#define MCCONF_S_PID_KI 0.0005
|
||||
#define MCCONF_S_PID_KD 0.0
|
||||
#define MCCONF_S_PID_MIN_RPM 100.0
|
||||
|
||||
#define MCCONF_P_PID_KP 0.01
|
||||
#define MCCONF_P_PID_KI 0.0
|
||||
#define MCCONF_P_PID_KD 0.0006
|
||||
|
||||
#endif /* MCCONF_FOC_ERWIN_H_ */
|
|
@ -1,26 +0,0 @@
|
|||
#ifndef MCCONF_FOC_SCORPION_H_
|
||||
#define MCCONF_FOC_SCORPION_H_
|
||||
|
||||
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
|
||||
|
||||
#define MCCONF_FOC_CURRENT_KP 0.05
|
||||
#define MCCONF_FOC_CURRENT_KI 50.0
|
||||
#define MCCONF_FOC_F_SW 50000.0
|
||||
#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_SENSORLESS
|
||||
#define MCCONF_FOC_PLL_KP 40.0
|
||||
#define MCCONF_FOC_PLL_KI 40000.0
|
||||
#define MCCONF_FOC_MOTOR_L 0.000007
|
||||
#define MCCONF_FOC_MOTOR_R 0.015
|
||||
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
|
||||
#define MCCONF_FOC_OBSERVER_GAIN 9e7 // Can be something like 600 / L
|
||||
|
||||
#define MCCONF_S_PID_KP 0.0001
|
||||
#define MCCONF_S_PID_KI 0.0005
|
||||
#define MCCONF_S_PID_KD 0.0
|
||||
#define MCCONF_S_PID_MIN_RPM 100.0
|
||||
|
||||
#define MCCONF_P_PID_KP 0.01
|
||||
#define MCCONF_P_PID_KI 0.0
|
||||
#define MCCONF_P_PID_KD 0.0006
|
||||
|
||||
#endif /* MCCONF_FOC_SCORPION_H_ */
|
|
@ -1,63 +0,0 @@
|
|||
/*
|
||||
Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* mcconf_outrunner2.h
|
||||
*
|
||||
* A configuration for my scorpion 225kv outrunner.
|
||||
*
|
||||
* Created on: 14 apr 2014
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#ifndef MCCONF_OUTRUNNER2_H_
|
||||
#define MCCONF_OUTRUNNER2_H_
|
||||
|
||||
/*
|
||||
* Parameters
|
||||
*/
|
||||
#define MCCONF_L_CURRENT_MAX 60.0 // Current limit in Amperes (Upper)
|
||||
#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
|
||||
#define MCCONF_L_IN_CURRENT_MAX 60.0 // Input current limit in Amperes (Upper)
|
||||
#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
|
||||
#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
|
||||
#define MCCONF_L_SLOW_ABS_OVERCURRENT 1 // Use the filtered (and hence slower) current for the overcurrent fault detection
|
||||
|
||||
// Sensorless settings
|
||||
#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
|
||||
#define MCCONF_SL_MIN_RPM 150 // Auto-commutate below this RPM
|
||||
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
|
||||
#define MCCONF_SL_CYCLE_INT_LIMIT 62.0 // Flux integrator limit 0 ERPM
|
||||
#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
|
||||
#define MCCONF_SL_BEMF_COUPLING_K 600.0 // Input voltage to bemf coupling constant
|
||||
|
||||
// Speed PID parameters
|
||||
#define MCCONF_S_PID_KP 0.0001 // Proportional gain
|
||||
#define MCCONF_S_PID_KI 0.002 // Integral gain
|
||||
#define MCCONF_S_PID_KD 0.0 // Derivative gain
|
||||
#define MCCONF_S_PID_MIN_RPM 900.0 // Minimum allowed RPM
|
||||
|
||||
// Position PID parameters
|
||||
#define MCCONF_P_PID_KP 0.0001 // Proportional gain
|
||||
#define MCCONF_P_PID_KI 0.002 // Integral gain
|
||||
#define MCCONF_P_PID_KD 0.0 // Derivative gain
|
||||
|
||||
// Current control parameters
|
||||
#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
|
||||
#define MCCONF_CC_MIN_CURRENT 1.0 // Minimum allowed current
|
||||
|
||||
#endif /* MCCONF_OUTRUNNER2_H_ */
|
91
mcpwm_foc.c
91
mcpwm_foc.c
|
@ -89,7 +89,7 @@ static volatile float m_id_set;
|
|||
static volatile float m_iq_set;
|
||||
static volatile bool m_dccal_done;
|
||||
static volatile bool m_output_on;
|
||||
static volatile float m_pos_pid_set_pos;
|
||||
static volatile float m_pos_pid_set;
|
||||
static volatile float m_speed_pid_set_rpm;
|
||||
static volatile float m_phase_now_observer;
|
||||
static volatile float m_phase_now_observer_override;
|
||||
|
@ -104,6 +104,7 @@ static volatile mc_sample_t m_samples;
|
|||
static volatile int m_tachometer;
|
||||
static volatile int m_tachometer_abs;
|
||||
static volatile float last_inj_adc_isr_duration;
|
||||
static volatile float m_pos_pid_now;
|
||||
|
||||
// Private functions
|
||||
static void do_dc_cal(void);
|
||||
|
@ -114,7 +115,7 @@ static void pll_run(float phase, float dt, volatile float *phase_var,
|
|||
static void control_current(volatile motor_state_t *state_m, float dt);
|
||||
static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
|
||||
uint32_t* tAout, uint32_t* tBout, uint32_t* tCout);
|
||||
static void run_pid_control_pos(float dt);
|
||||
static void run_pid_control_pos(float angle_now, float angle_set, float dt);
|
||||
static void run_pid_control_speed(float dt);
|
||||
static void stop_pwm_hw(void);
|
||||
static void start_pwm_hw(void);
|
||||
|
@ -201,7 +202,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
|
|||
m_id_set = 0.0;
|
||||
m_iq_set = 0.0;
|
||||
m_output_on = false;
|
||||
m_pos_pid_set_pos = 0.0;
|
||||
m_pos_pid_set = 0.0;
|
||||
m_speed_pid_set_rpm = 0.0;
|
||||
m_phase_now_observer = 0.0;
|
||||
m_phase_now_observer_override = 0.0;
|
||||
|
@ -215,6 +216,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
|
|||
m_tachometer = 0;
|
||||
m_tachometer_abs = 0;
|
||||
last_inj_adc_isr_duration = 0;
|
||||
m_pos_pid_now = 0.0;
|
||||
memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
|
||||
memset((void*)&m_samples, 0, sizeof(mc_sample_t));
|
||||
|
||||
|
@ -517,7 +519,7 @@ void mcpwm_foc_set_pid_speed(float rpm) {
|
|||
m_control_mode = CONTROL_MODE_SPEED;
|
||||
m_speed_pid_set_rpm = rpm;
|
||||
|
||||
if (m_state != MC_STATE_RUNNING && fabsf(rpm) > m_conf->s_pid_min_erpm) {
|
||||
if (m_state != MC_STATE_RUNNING) {
|
||||
m_state = MC_STATE_RUNNING;
|
||||
}
|
||||
}
|
||||
|
@ -531,7 +533,7 @@ void mcpwm_foc_set_pid_speed(float rpm) {
|
|||
*/
|
||||
void mcpwm_foc_set_pid_pos(float pos) {
|
||||
m_control_mode = CONTROL_MODE_POS;
|
||||
m_pos_pid_set_pos = pos;
|
||||
m_pos_pid_set = pos;
|
||||
|
||||
if (m_state != MC_STATE_RUNNING) {
|
||||
m_state = MC_STATE_RUNNING;
|
||||
|
@ -597,6 +599,14 @@ float mcpwm_foc_get_duty_cycle_now(void) {
|
|||
return m_motor_state.duty_now;
|
||||
}
|
||||
|
||||
float mcpwm_foc_get_pid_pos_set(void) {
|
||||
return m_pos_pid_set;
|
||||
}
|
||||
|
||||
float mcpwm_foc_get_pid_pos_now(void) {
|
||||
return m_pos_pid_now;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the current switching frequency.
|
||||
*
|
||||
|
@ -1123,7 +1133,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
|
|||
|
||||
float res_tmp = 0.0;
|
||||
float i_last = 0.0;
|
||||
for (float i = 2.0;i < 35.0;i *= 1.5) {
|
||||
for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
|
||||
res_tmp = mcpwm_foc_measure_resistance(i, 20);
|
||||
|
||||
if (i > (0.5 / res_tmp)) {
|
||||
|
@ -1133,7 +1143,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
|
|||
}
|
||||
|
||||
if (i_last < 0.01) {
|
||||
i_last = 35.0;
|
||||
i_last = (m_conf->l_current_max / 2.0);
|
||||
}
|
||||
|
||||
*res = mcpwm_foc_measure_resistance(i_last, 500);
|
||||
|
@ -1406,6 +1416,13 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
duty_set = 0.0;
|
||||
}
|
||||
|
||||
// Brake when set ERPM is below min ERPM
|
||||
if (m_control_mode == CONTROL_MODE_SPEED &&
|
||||
fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
|
||||
control_duty = true;
|
||||
duty_set = 0.0;
|
||||
}
|
||||
|
||||
if (control_duty) {
|
||||
// Duty cycle control
|
||||
static float duty_i_term = 0.0;
|
||||
|
@ -1516,7 +1533,6 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
m_motor_state.iq_target = iq_set_tmp;
|
||||
|
||||
control_current(&m_motor_state, dt);
|
||||
run_pid_control_pos(dt);
|
||||
} else {
|
||||
// Track back emf
|
||||
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
|
||||
|
@ -1584,6 +1600,29 @@ void mcpwm_foc_adc_inj_int_handler(void) {
|
|||
phase_last = m_motor_state.phase;
|
||||
}
|
||||
|
||||
// Track position control angle
|
||||
// TODO: Have another look at this.
|
||||
#if ENCODER_ENABLE
|
||||
float angle_now = encoder_read_deg();
|
||||
#else
|
||||
float angle_now = m_motor_state.phase * (180.0 / M_PI);
|
||||
#endif
|
||||
|
||||
if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
|
||||
m_pos_pid_now = angle_now;
|
||||
} else {
|
||||
static float angle_last = 0.0;
|
||||
float diff_f = utils_angle_difference(angle_now, angle_last);
|
||||
angle_last = angle_now;
|
||||
m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
|
||||
utils_norm_angle((float*)&m_pos_pid_now);
|
||||
}
|
||||
|
||||
// Run position control
|
||||
if (m_state == MC_STATE_RUNNING) {
|
||||
run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
|
||||
}
|
||||
|
||||
// MCIF handler
|
||||
mc_interface_mc_timer_isr();
|
||||
|
||||
|
@ -1950,7 +1989,7 @@ static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
|
|||
*tCout = tC;
|
||||
}
|
||||
|
||||
static void run_pid_control_pos(float dt) {
|
||||
static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
|
||||
static float i_term = 0;
|
||||
static float prev_error = 0;
|
||||
float p_term;
|
||||
|
@ -1963,19 +2002,30 @@ static void run_pid_control_pos(float dt) {
|
|||
return;
|
||||
}
|
||||
|
||||
// Compute error
|
||||
float angle = encoder_read_deg();
|
||||
float error = utils_angle_difference(m_pos_pid_set_pos, angle);
|
||||
// Compute parameters
|
||||
float error = utils_angle_difference(angle_set, angle_now);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
if (m_conf->foc_encoder_inverted) {
|
||||
error = -error;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// Compute parameters
|
||||
p_term = error * m_conf->p_pid_kp;
|
||||
i_term += error * (m_conf->p_pid_ki * dt);
|
||||
d_term = (error - prev_error) * (m_conf->p_pid_kd / dt);
|
||||
|
||||
// Average DT for the D term when the error does not change. This likely
|
||||
// happens at low speed when the position resolution is low and several
|
||||
// control iterations run without position updates.
|
||||
// TODO: Are there problems with this approach?
|
||||
static float dt_int = 0.0;
|
||||
dt_int += dt;
|
||||
if (error == prev_error) {
|
||||
d_term = 0.0;
|
||||
} else {
|
||||
d_term = (error - prev_error) * (m_conf->p_pid_kd / dt_int);
|
||||
dt_int = 0.0;
|
||||
}
|
||||
|
||||
// I-term wind-up protection
|
||||
utils_truncate_number(&i_term, -1.0, 1.0);
|
||||
|
@ -1987,12 +2037,16 @@ static void run_pid_control_pos(float dt) {
|
|||
float output = p_term + i_term + d_term;
|
||||
utils_truncate_number(&output, -1.0, 1.0);
|
||||
|
||||
#if ENCODER_ENABLE
|
||||
if (encoder_index_found()) {
|
||||
m_iq_set = output * m_conf->lo_current_max;
|
||||
} else {
|
||||
// Rotate the motor with 40 % power until the encoder index is found.
|
||||
m_iq_set = 0.4 * m_conf->lo_current_max;
|
||||
}
|
||||
#else
|
||||
m_iq_set = output * m_conf->lo_current_max;
|
||||
#endif
|
||||
}
|
||||
|
||||
static void run_pid_control_speed(float dt) {
|
||||
|
@ -2008,15 +2062,10 @@ static void run_pid_control_speed(float dt) {
|
|||
return;
|
||||
}
|
||||
|
||||
// Too low RPM set. Stop and return.
|
||||
// Too low RPM set. Reset state and return.
|
||||
if (fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
|
||||
i_term = 0.0;
|
||||
prev_error = 0;
|
||||
m_iq_set = 0.0;
|
||||
m_state = MC_STATE_OFF;
|
||||
if (m_output_on) {
|
||||
stop_pwm_hw();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
@ -44,6 +44,8 @@ void mcpwm_foc_set_current(float current);
|
|||
void mcpwm_foc_set_brake_current(float current);
|
||||
float mcpwm_foc_get_duty_cycle_set(void);
|
||||
float mcpwm_foc_get_duty_cycle_now(void);
|
||||
float mcpwm_foc_get_pid_pos_set(void);
|
||||
float mcpwm_foc_get_pid_pos_now(void);
|
||||
float mcpwm_foc_get_switching_frequency_now(void);
|
||||
float mcpwm_foc_get_rpm(void);
|
||||
float mcpwm_foc_get_tot_current(void);
|
||||
|
|
28
utils.c
28
utils.c
|
@ -133,18 +133,24 @@ void utils_deadband(float *value, float tres, float max) {
|
|||
* The difference between the angles
|
||||
*/
|
||||
float utils_angle_difference(float angle1, float angle2) {
|
||||
utils_norm_angle(&angle1);
|
||||
utils_norm_angle(&angle2);
|
||||
// utils_norm_angle(&angle1);
|
||||
// utils_norm_angle(&angle2);
|
||||
//
|
||||
// if (fabsf(angle1 - angle2) > 180.0) {
|
||||
// if (angle1 < angle2) {
|
||||
// angle1 += 360.0;
|
||||
// } else {
|
||||
// angle2 += 360.0;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// return angle1 - angle2;
|
||||
|
||||
if (fabsf(angle1 - angle2) > 180.0) {
|
||||
if (angle1 < angle2) {
|
||||
angle1 += 360.0;
|
||||
} else {
|
||||
angle2 += 360.0;
|
||||
}
|
||||
}
|
||||
|
||||
return angle1 - angle2;
|
||||
// Faster in most cases
|
||||
float difference = angle1 - angle2;
|
||||
while (difference < -180.0) difference += 2.0 * 180.0;
|
||||
while (difference > 180.0) difference -= 2.0 * 180.0;
|
||||
return difference;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue