From d06fce3ee429c5b6696ab17b6c743750407e13d3 Mon Sep 17 00:00:00 2001 From: Euan Mutch Date: Tue, 8 Nov 2022 11:46:08 +0000 Subject: [PATCH] Interface commands for openloop --- conf_general.c | 4 +- lispBM/lispif_vesc_extensions.c | 2 +- motor/mc_interface.c | 97 +++++++++++++++++++++++++++++++++ motor/mc_interface.h | 4 ++ motor/mcpwm_foc.c | 8 +-- motor/mcpwm_foc.h | 4 +- terminal.c | 16 ++++-- 7 files changed, 120 insertions(+), 15 deletions(-) diff --git a/conf_general.c b/conf_general.c index a22ba124..239503a8 100644 --- a/conf_general.c +++ b/conf_general.c @@ -1029,7 +1029,7 @@ int conf_general_measure_flux_linkage_openloop(float current, float duty, // Start by locking the motor for (int i = 0;i < 200;i++) { - mcpwm_foc_set_openloop((float)i * current / 200.0, rpm_now); + mcpwm_foc_set_openloop_current((float)i * current / 200.0, rpm_now); fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { timeout_configure(tout, tout_c, tout_ksw); @@ -1070,7 +1070,7 @@ int conf_general_measure_flux_linkage_openloop(float current, float duty, while (fabsf(mc_interface_get_duty_cycle_now()) < duty) { rpm_now += erpm_per_sec / 1000.0; - mcpwm_foc_set_openloop(current, mcconf->m_invert_direction ? -rpm_now : rpm_now); + mcpwm_foc_set_openloop_current(current, mcconf->m_invert_direction ? -rpm_now : rpm_now); fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index 0d7d39a1..7b52919a 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -1115,7 +1115,7 @@ static lbm_value ext_set_pos(lbm_value *args, lbm_uint argn) { static lbm_value ext_foc_openloop(lbm_value *args, lbm_uint argn) { CHECK_ARGN_NUMBER(2); timeout_reset(); - mcpwm_foc_set_openloop(lbm_dec_as_float(args[0]), lbm_dec_as_float(args[1])); + mc_interface_set_openloop_current(lbm_dec_as_float(args[0]), lbm_dec_as_float(args[1])); return ENC_SYM_TRUE; } diff --git a/motor/mc_interface.c b/motor/mc_interface.c index 7ed0ffc2..322f2da0 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -774,6 +774,103 @@ void mc_interface_set_handbrake_rel(float val) { mc_interface_set_handbrake(val * fabsf(motor_now()->m_conf.lo_current_motor_min_now)); } +void mc_interface_set_openloop_current(float current, float rpm) { + if (fabsf(current) > 0.001) { + SHUTDOWN_RESET(); + } + + if (mc_interface_try_input()) { + return; + } + + switch (motor_now()->m_conf.motor_type) { + case MOTOR_TYPE_BLDC: + case MOTOR_TYPE_DC: + break; + + case MOTOR_TYPE_FOC: + mcpwm_foc_set_openloop_current(current, rpm); // Should this use DIR_MULT? + break; + + default: + break; + } + + events_add("set_openloop_current", current); +} +void mc_interface_set_openloop_phase(float current, float phase){ + if (fabsf(current) > 0.001) { + SHUTDOWN_RESET(); + } + + if (mc_interface_try_input()) { + return; + } + + switch (motor_now()->m_conf.motor_type) { + case MOTOR_TYPE_BLDC: + case MOTOR_TYPE_DC: + break; + + case MOTOR_TYPE_FOC: + mcpwm_foc_set_openloop_phase(current, phase); // Should this use DIR_MULT? + break; + + default: + break; + } + + events_add("set_openloop_phase", phase); +} +void mc_interface_set_openloop_duty(float dutyCycle, float rpm){ + if (fabsf(dutyCycle) > 0.001) { + SHUTDOWN_RESET(); + } + + if (mc_interface_try_input()) { + return; + } + + switch (motor_now()->m_conf.motor_type) { + case MOTOR_TYPE_BLDC: + case MOTOR_TYPE_DC: + break; + + case MOTOR_TYPE_FOC: + mcpwm_foc_set_openloop_duty(dutyCycle, rpm); // Should this use DIR_MULT? + break; + + default: + break; + } + + events_add("set_openloop_duty", dutyCycle); +} +void mc_interface_set_openloop_duty_phase(float dutyCycle, float phase){ + if (fabsf(dutyCycle) > 0.001) { + SHUTDOWN_RESET(); + } + + if (mc_interface_try_input()) { + return; + } + + switch (motor_now()->m_conf.motor_type) { + case MOTOR_TYPE_BLDC: + case MOTOR_TYPE_DC: + break; + + case MOTOR_TYPE_FOC: + mcpwm_foc_set_openloop_duty_phase(dutyCycle, phase); // Should this use DIR_MULT? + break; + + default: + break; + } + + events_add("set_openloop_duty_phase", phase); +} + void mc_interface_brake_now(void) { SHUTDOWN_RESET(); diff --git a/motor/mc_interface.h b/motor/mc_interface.h index 5ee6d66a..c697313c 100644 --- a/motor/mc_interface.h +++ b/motor/mc_interface.h @@ -51,6 +51,10 @@ void mc_interface_set_current_rel(float val); void mc_interface_set_brake_current_rel(float val); void mc_interface_set_handbrake(float current); void mc_interface_set_handbrake_rel(float val); +void mc_interface_set_openloop_current(float current, float rpm); +void mc_interface_set_openloop_phase(float current, float phase); +void mc_interface_set_openloop_duty(float dutyCycle, float rpm); +void mc_interface_set_openloop_duty_phase(float dutyCycle, float phase); int mc_interface_set_tachometer_value(int steps); void mc_interface_brake_now(void); void mc_interface_release_motor(void); diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index e1de5ae8..9e93e811 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -777,7 +777,7 @@ void mcpwm_foc_set_current(float current) { get_motor_now()->m_id_set = 0; if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) { - return fault; + return; } if (get_motor_now()->m_state != MC_STATE_RUNNING) { @@ -805,7 +805,7 @@ void mcpwm_foc_set_brake_current(float current) { get_motor_now()->m_iq_set = current; if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) { - return fault; + return; } if (get_motor_now()->m_state != MC_STATE_RUNNING) { @@ -826,7 +826,7 @@ void mcpwm_foc_set_handbrake(float current) { get_motor_now()->m_iq_set = current; if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) { - return fault; + return; } if (get_motor_now()->m_state != MC_STATE_RUNNING) { @@ -884,7 +884,7 @@ void mcpwm_foc_set_openloop_phase(float current, float phase) { utils_norm_angle_rad((float*)&get_motor_now()->m_openloop_phase); if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) { - return fault; + return; } if (get_motor_now()->m_state != MC_STATE_RUNNING) { diff --git a/motor/mcpwm_foc.h b/motor/mcpwm_foc.h index 8e846458..fe0eabdd 100644 --- a/motor/mcpwm_foc.h +++ b/motor/mcpwm_foc.h @@ -42,11 +42,11 @@ void mcpwm_foc_set_current(float current); void mcpwm_foc_release_motor(void); void mcpwm_foc_set_brake_current(float current); void mcpwm_foc_set_handbrake(float current); -void mcpwm_foc_set_openloop(float current, float rpm); +void mcpwm_foc_set_openloop_current(float current, float rpm); void mcpwm_foc_set_openloop_phase(float current, float phase); void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm); void mcpwm_foc_set_openloop_duty_phase(float dutyCycle, float phase); -void mcpwm_foc_set_tachometer_value(int steps); +int mcpwm_foc_set_tachometer_value(int steps); float mcpwm_foc_get_duty_cycle_set(void); float mcpwm_foc_get_duty_cycle_now(void); float mcpwm_foc_get_pid_pos_set(void); diff --git a/terminal.c b/terminal.c index c96a7e58..efdefb15 100644 --- a/terminal.c +++ b/terminal.c @@ -731,7 +731,7 @@ void terminal_process_string(char *str) { commands_printf("Running FOC openloop..."); if (current > 0.0 && current <= mc_interface_get_configuration()->l_current_max && erpm >= 0.0) { timeout_reset(); - mcpwm_foc_set_openloop(current, erpm); + mc_interface_set_openloop_current(current, erpm); int fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { commands_printf("Fault occured during openloop: %s", mc_interface_fault_to_string(fault)); @@ -759,7 +759,7 @@ void terminal_process_string(char *str) { commands_printf("Running FOC openloop duty..."); if (duty >= 0.0 && duty <= 0.9 && erpm >= 0.0) { timeout_reset(); - mcpwm_foc_set_openloop_duty(duty, erpm); + mc_interface_set_openloop_duty(duty, erpm); int fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { commands_printf("Fault occured during openloop: %s", mc_interface_fault_to_string(fault)); @@ -838,7 +838,8 @@ void terminal_process_string(char *str) { angle >= 0.0 && angle <= 360.0) { if (time <= 1e-6) { timeout_reset(); - fault = mcpwm_foc_set_openloop_phase(current, angle); + mc_interface_set_openloop_phase(current, angle); + fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { commands_printf("Fault occured during openloop: %s", mc_interface_fault_to_string(fault)); commands_printf("For more info type \"faults\" to view all logged faults\n"); @@ -849,7 +850,8 @@ void terminal_process_string(char *str) { int print_div = 0; for (float t = 0.0;t < time;t += 0.002) { timeout_reset(); - fault = mcpwm_foc_set_openloop_phase(current, angle); + mc_interface_set_openloop_phase(current, angle); + fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { commands_printf("Fault occured during openloop: %s", mc_interface_fault_to_string(fault)); commands_printf("For more info type \"faults\" to view all logged faults\n"); @@ -1074,7 +1076,8 @@ void terminal_process_string(char *str) { for (int i = 0;i < 1000;i++) { timeout_reset(); - fault = mcpwm_foc_set_openloop_phase((float)i * current / 1000.0, phase); + mc_interface_set_openloop_phase((float)i * current / 1000.0, phase); + fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { break; } @@ -1117,7 +1120,8 @@ void terminal_process_string(char *str) { phase += 1.0; timeout_reset(); - fault = mcpwm_foc_set_openloop_phase(current, phase); + mc_interface_set_openloop_phase(current, phase); + fault = mc_interface_get_fault(); if (fault != FAULT_CODE_NONE) { break; }