mirror of https://github.com/rusefi/bldc.git
Mix of stuff (Not a working state)
Organize vars Hack in dterm PT1 Improve faults Remove dead state
This commit is contained in:
Mitch Lustig
parent
b1915b06a7
commit
da7b3a0ffb
|
|
@ -41,8 +41,14 @@
|
|||
typedef enum {
|
||||
STARTUP = 0,
|
||||
RUNNING,
|
||||
FAULT,
|
||||
DEAD
|
||||
RUNNING_TILTBACK_DUTY,
|
||||
RUNNING_TILTBACK_HIGH_VOLTAGE,
|
||||
RUNNING_TILTBACK_LOW_VOLTAGE,
|
||||
RUNNING_TILTBACK_CONSTANT,
|
||||
FAULT_ANGLE_PITCH,
|
||||
FAULT_ANGLE_ROLL,
|
||||
FAULT_SWITCH,
|
||||
FAULT_DUTY
|
||||
} BalanceState;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -60,81 +66,74 @@ typedef enum {
|
|||
static THD_FUNCTION(balance_thread, arg);
|
||||
static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
|
||||
|
||||
static volatile balance_config balance_conf;
|
||||
static volatile imu_config imu_conf;
|
||||
static thread_t *app_thread;
|
||||
|
||||
// Values used in loop
|
||||
static BalanceState state;
|
||||
// Config values
|
||||
static volatile balance_config balance_conf;
|
||||
static volatile imu_config imu_conf;
|
||||
static float startup_step_size, tiltback_step_size;
|
||||
|
||||
// Runtime values read from elsewhere
|
||||
static float pitch_angle, roll_angle;
|
||||
static float gyro[3];
|
||||
static float duty_cycle, abs_duty_cycle;
|
||||
static float erpm, abs_erpm, avg_erpm;
|
||||
static float proportional, integral, derivative;
|
||||
static float last_proportional;
|
||||
static float pid_value;
|
||||
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
|
||||
static float setpoint, setpoint_target, setpoint_target_interpolated;
|
||||
static SetpointAdjustmentType setpointAdjustmentType;
|
||||
static float startup_step_size, tiltback_step_size;
|
||||
static systime_t current_time, last_time, diff_time;
|
||||
static systime_t startup_start_time, startup_diff_time;
|
||||
static systime_t dead_start_time;
|
||||
static systime_t fault_start_time;
|
||||
static float motor_current;
|
||||
static float motor_position;
|
||||
static float adc1, adc2;
|
||||
static SwitchState switch_state;
|
||||
|
||||
// Values read to pass in app data to GUI
|
||||
static float motor_current;
|
||||
static float motor_position;
|
||||
// Rumtime state values
|
||||
static BalanceState state;
|
||||
static float proportional, integral, derivative;
|
||||
static float last_proportional;
|
||||
static float pid_value;
|
||||
static float setpoint, setpoint_target, setpoint_target_interpolated;
|
||||
static SetpointAdjustmentType setpointAdjustmentType;
|
||||
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
|
||||
static systime_t current_time, last_time, diff_time;
|
||||
static systime_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, fault_duty_timer;
|
||||
static float d_pt1_state, d_pt1_k;
|
||||
|
||||
|
||||
void app_balance_configure(balance_config *conf, imu_config *conf2) {
|
||||
balance_conf = *conf;
|
||||
imu_conf = *conf2;
|
||||
// Set calculated values from config
|
||||
float f_cut = 75.0;
|
||||
float dT = 0.001;
|
||||
float RC = 1 / ( 2 * M_PI * f_cut);
|
||||
d_pt1_k = dT / (RC + dT);
|
||||
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
||||
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
|
||||
}
|
||||
|
||||
void app_balance_start(void) {
|
||||
|
||||
// Reset all Values
|
||||
// Reset vars (yes i just repeated the function name, it looked funny without a comment)
|
||||
reset_vars();
|
||||
// First start only, override state to startup
|
||||
state = STARTUP;
|
||||
pitch_angle = 0;
|
||||
roll_angle = 0;
|
||||
gyro[0] = 0;
|
||||
gyro[1] = 0;
|
||||
gyro[2] = 0;
|
||||
duty_cycle = 0;
|
||||
abs_duty_cycle = 0;
|
||||
erpm = 0;
|
||||
abs_erpm = 0;
|
||||
avg_erpm = 0;
|
||||
adc1 = 0;
|
||||
adc2 = 0;
|
||||
switch_state = OFF;
|
||||
proportional = 0;
|
||||
// Start the balance thread
|
||||
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
|
||||
}
|
||||
|
||||
void reset_vars(){
|
||||
// Clear accumulated values.
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
last_proportional = 0;
|
||||
pid_value = 0;
|
||||
yaw_proportional = 0;
|
||||
yaw_integral = 0;
|
||||
yaw_derivative = 0;
|
||||
yaw_last_proportional = 0;
|
||||
yaw_pid_value = 0;
|
||||
yaw_setpoint = 0;
|
||||
setpoint = 0;
|
||||
d_pt1_state = 0;
|
||||
// Set values for startup
|
||||
setpoint = pitch_angle;
|
||||
setpoint_target = 0;
|
||||
setpoint_target_interpolated = 0;
|
||||
setpoint_target_interpolated = pitch_angle;
|
||||
setpointAdjustmentType = CENTERING;
|
||||
startup_step_size = 0;
|
||||
tiltback_step_size = 0;
|
||||
yaw_setpoint = 0;
|
||||
state = RUNNING;
|
||||
current_time = 0;
|
||||
last_time = 0;
|
||||
diff_time = 0;
|
||||
startup_start_time = 0;
|
||||
startup_diff_time = 0;
|
||||
|
||||
// Start the balance thread
|
||||
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
|
||||
}
|
||||
|
||||
float app_balance_get_pid_output(void) {
|
||||
|
|
@ -178,6 +177,56 @@ float get_setpoint_adjustment_step_size(void){
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Fault checking order does not really matter. From a UX perspective, switch should be before angle.
|
||||
bool check_fault(bool ignoreTimers){
|
||||
// Check switch
|
||||
// Switch fully open or Switch partially open and stopped
|
||||
if(switch_state == OFF || (switch_state == HALF && abs_erpm < balance_conf.adc_half_fault_erpm)){
|
||||
if(ST2MS(current_time - fault_switch_timer) > balance_conf.fault_delay || ignoreTimers){
|
||||
state = FAULT_SWITCH;
|
||||
return true;
|
||||
}
|
||||
} else {
|
||||
fault_switch_timer = current_time;
|
||||
}
|
||||
|
||||
// Check pitch angle
|
||||
if(fabsf(pitch_angle) > balance_conf.pitch_fault){
|
||||
if(ST2MS(current_time - fault_angle_pitch_timer) > balance_conf.fault_delay || ignoreTimers){
|
||||
state = FAULT_ANGLE_PITCH;
|
||||
return true;
|
||||
}
|
||||
}else{
|
||||
fault_angle_pitch_timer = current_time;
|
||||
}
|
||||
|
||||
// Check roll angle
|
||||
if(fabsf(roll_angle) > balance_conf.roll_fault){
|
||||
if(ST2MS(current_time - fault_angle_roll_timer) > balance_conf.fault_delay || ignoreTimers){
|
||||
state = FAULT_ANGLE_PITCH;
|
||||
return true;
|
||||
}
|
||||
}else{
|
||||
fault_angle_roll_timer = current_time;
|
||||
}
|
||||
|
||||
// Check for duty
|
||||
if(abs_duty_cycle > balance_conf.overspeed_duty){
|
||||
if(ST2MS(current_time - fault_duty_timer) > balance_conf.overspeed_delay || ignoreTimers){
|
||||
state = FAULT_DUTY;
|
||||
return true;
|
||||
}
|
||||
} else {
|
||||
fault_duty_timer = current_time;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void apply_tiltbacks(){
|
||||
|
||||
}
|
||||
|
||||
float apply_deadzone(float error){
|
||||
if(balance_conf.deadzone == 0){
|
||||
return error;
|
||||
|
|
@ -237,12 +286,6 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
(void)arg;
|
||||
chRegSetThreadName("APP_BALANCE");
|
||||
|
||||
// Do one off config
|
||||
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
||||
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
|
||||
|
||||
state = STARTUP;
|
||||
|
||||
while (!chThdShouldTerminateX()) {
|
||||
// Update times
|
||||
current_time = chVTGetSystemTimeX();
|
||||
|
|
@ -307,7 +350,7 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
}
|
||||
|
||||
|
||||
// State based logic
|
||||
// Control Loop State Logic
|
||||
switch(state){
|
||||
case (STARTUP):
|
||||
while(!imu_startup_done()){
|
||||
|
|
@ -316,32 +359,17 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
// Wait
|
||||
chThdSleepMilliseconds(50);
|
||||
}
|
||||
state = FAULT;
|
||||
startup_start_time = 0;
|
||||
startup_diff_time = 0;
|
||||
break;
|
||||
state = RUNNING;
|
||||
/* no break */
|
||||
case (RUNNING):
|
||||
// Check for overspeed
|
||||
if(abs_duty_cycle > balance_conf.overspeed_duty){
|
||||
if(ST2MS(current_time - dead_start_time) > balance_conf.overspeed_delay){
|
||||
state = DEAD;
|
||||
}
|
||||
} else {
|
||||
dead_start_time = current_time;
|
||||
}
|
||||
case (RUNNING_TILTBACK_DUTY):
|
||||
case (RUNNING_TILTBACK_HIGH_VOLTAGE):
|
||||
case (RUNNING_TILTBACK_LOW_VOLTAGE):
|
||||
case (RUNNING_TILTBACK_CONSTANT):
|
||||
|
||||
// Check for fault
|
||||
if(
|
||||
fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over
|
||||
fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over
|
||||
switch_state == OFF || // Switch fully open
|
||||
(switch_state == HALF && abs_erpm < balance_conf.adc_half_fault_erpm) // Switch partially open and stopped
|
||||
){
|
||||
if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){
|
||||
state = FAULT;
|
||||
}
|
||||
} else {
|
||||
fault_start_time = current_time;
|
||||
// Check for faults
|
||||
if(checkFaults(false)){
|
||||
break;
|
||||
}
|
||||
|
||||
// Over speed tilt back safety
|
||||
|
|
@ -403,6 +431,10 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
integral = integral + proportional;
|
||||
derivative = proportional - last_proportional;
|
||||
|
||||
// Apply D term only filter
|
||||
d_pt1_state = d_pt1_state + d_pt1_k * (derivative - d_pt1_state);
|
||||
derivative = d_pt1_state;
|
||||
|
||||
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
|
||||
|
||||
last_proportional = proportional;
|
||||
|
|
@ -414,6 +446,7 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
pid_value -= balance_conf.current_boost;
|
||||
}
|
||||
|
||||
|
||||
if(balance_conf.multi_esc){
|
||||
// Calculate setpoint
|
||||
if(abs_duty_cycle < .02){
|
||||
|
|
@ -442,26 +475,22 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
// Output to motor
|
||||
set_current(pid_value, yaw_pid_value);
|
||||
break;
|
||||
case (FAULT):
|
||||
case (FAULT_ANGLE_PITCH):
|
||||
case (FAULT_ANGLE_ROLL):
|
||||
case (FAULT_SWITCH):
|
||||
// Check for valid startup position and switch state
|
||||
if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && switch_state == ON){
|
||||
// Clear accumulated values.
|
||||
integral = 0;
|
||||
last_proportional = 0;
|
||||
yaw_integral = 0;
|
||||
yaw_last_proportional = 0;
|
||||
// Set values for startup
|
||||
setpoint = pitch_angle;
|
||||
setpoint_target = 0;
|
||||
setpoint_target_interpolated = pitch_angle;
|
||||
setpointAdjustmentType = CENTERING;
|
||||
state = RUNNING;
|
||||
reset_vars();
|
||||
break;
|
||||
}
|
||||
// Disable output
|
||||
brake();
|
||||
break;
|
||||
case (DEAD):
|
||||
case (FAULT_DUTY):
|
||||
// We need another fault to clear duty fault.
|
||||
// Otherwise duty fault will clear itself as soon as motor pauses, then motor will spool up again.
|
||||
// Rendering this fault useless.
|
||||
checkFaults(true);
|
||||
// Disable output
|
||||
brake();
|
||||
break;
|
||||
|
|
|
|||
Loading…
Reference in New Issue