|
|
@@ -33,6 +33,9 @@ static void _encoder_zero_off_timer_handler(shark_timer_t *);
|
|
|
static shark_timer_t _encoder_zero_off_timer = TIMER_INIT(_encoder_zero_off_timer, _encoder_zero_off_timer_handler);
|
|
|
static void _led_off_timer_handler(shark_timer_t *);
|
|
|
static shark_timer_t _led_off_timer = TIMER_INIT(_led_off_timer, _led_off_timer_handler);
|
|
|
+static void _if_start_timer_handler(shark_timer_t *);
|
|
|
+static shark_timer_t _if_start_timer = TIMER_INIT(_if_start_timer, _if_start_timer_handler);
|
|
|
+
|
|
|
motor_t motor = {
|
|
|
.s_direction = POSITIVE,
|
|
|
.n_gear = 0,
|
|
|
@@ -743,6 +746,12 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
|
|
|
if (!mot_contrl_is_start(&motor.controller)) {
|
|
|
mot_contrl_start(&motor.controller, motor.mode);
|
|
|
mc_gear_mode_set(false);
|
|
|
+ motor_encoder_reinit();
|
|
|
+ if (dir == EPM_Dir_Back) {
|
|
|
+ motor_encoder_set_direction(NEGATIVE);
|
|
|
+ }else {
|
|
|
+ motor_encoder_set_direction(POSITIVE);
|
|
|
+ }
|
|
|
pwm_enable_channel();
|
|
|
}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
|
|
|
mc_auto_hold(false);
|
|
|
@@ -826,9 +835,62 @@ u16 mc_get_running_status2(void) {
|
|
|
return data;
|
|
|
}
|
|
|
|
|
|
+static void _if_start_timer_handler(shark_timer_t *t) {
|
|
|
+ mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
|
|
|
+ task_udelay(200);
|
|
|
+ sys_debug("start IF\n");
|
|
|
+ u32 mask = cpu_enter_critical();
|
|
|
+ mc_internal_init(CTRL_MODE_OPEN, true);
|
|
|
+ mc_set_ctrl_mode(CTRL_MODE_CURRENT);
|
|
|
+ mot_contrl_set_angle(&motor.controller, INVALID_ANGLE);
|
|
|
+ mot_contrl_ifctl_start(&motor.controller, true, motor.if_acc, motor.if_max_vel, motor.if_iq_set);
|
|
|
+ cpu_exit_critical(mask);
|
|
|
+}
|
|
|
+
|
|
|
+bool mc_start_ifctl_mode(bool start, s16 accl, s16 max_vel, s16 align_vd, s16 iq_set) {
|
|
|
+ if ((motor.controller.if_ctl.b_ena == start) || shark_timer_started(&_if_start_timer)) {
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (start) {
|
|
|
+ motor.b_ignor_throttle = true;
|
|
|
+ MC_Check_MosVbusThrottle();
|
|
|
+ if (mc_unsafe_critical_error()) {
|
|
|
+ mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ pwm_up_enable(false);
|
|
|
+ pwm_turn_on_low_side();
|
|
|
+ task_udelay(500);
|
|
|
+ mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
|
|
|
+ phase_current_offset_calibrate();
|
|
|
+ pwm_start();
|
|
|
+ adc_start_convert();
|
|
|
+ pwm_enable_channel();
|
|
|
+ phase_current_calibrate_wait();
|
|
|
+ mot_contrl_set_angle(&motor.controller, 0);
|
|
|
+ mot_contrl_set_vdq(&motor.controller, (float)align_vd/10.0f, 0);
|
|
|
+ motor.if_acc = accl;
|
|
|
+ motor.if_iq_set = iq_set;
|
|
|
+ motor.if_max_vel = max_vel;
|
|
|
+ shark_timer_post(&_if_start_timer, 1500);
|
|
|
+ }else {
|
|
|
+ mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
|
|
|
+ task_udelay(500);
|
|
|
+ u32 mask = cpu_enter_critical();
|
|
|
+ shark_timer_cancel(&_if_start_timer);
|
|
|
+ pwm_disable_channel();
|
|
|
+ mot_contrl_stop(&motor.controller);
|
|
|
+ adc_stop_convert();
|
|
|
+ pwm_stop();
|
|
|
+ mc_internal_init(CTRL_MODE_OPEN, false);
|
|
|
+ mot_contrl_ifctl_start(&motor.controller, false, 0, 0, 0);
|
|
|
+ cpu_exit_critical(mask);
|
|
|
+ pwm_up_enable(true);
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
-static float _force_angle = 0.0f;
|
|
|
-static int _force_wait = 2000;
|
|
|
/* 开环，强制给定电角度和DQ的电压 */
|
|
|
void mc_force_run_open(s16 vd, s16 vq, bool align) {
|
|
|
if (motor.b_start || motor.b_force_run) {
|
|
|
@@ -866,9 +928,9 @@ void mc_force_run_open(s16 vd, s16 vq, bool align) {
|
|
|
mot_contrl_set_angle(&motor.controller, 0);
|
|
|
mot_contrl_set_vdq(&motor.controller, (float)vd, 0);
|
|
|
if (align) {
|
|
|
- _force_wait = 2000 + 1;
|
|
|
+ motor.force_open_wait = 2000 + 1;
|
|
|
}else {
|
|
|
- _force_wait = 2000;
|
|
|
+ motor.force_open_wait = 2000;
|
|
|
}
|
|
|
motor.b_force_run = true;
|
|
|
}
|
|
|
@@ -1468,13 +1530,17 @@ static void mc_process_epm_move(void) {
|
|
|
|
|
|
static bool mc_process_force_running(void) {
|
|
|
if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
|
|
|
- if (motor.b_force_run && _force_wait <= 2000) {
|
|
|
- if (_force_wait > 0) {
|
|
|
- --_force_wait;
|
|
|
+ if (motor.b_force_run && motor.force_open_wait <= 2000) {
|
|
|
+ if (motor.force_open_wait > 0) {
|
|
|
+ --motor.force_open_wait;
|
|
|
}else {
|
|
|
- _force_angle += 1.5f;
|
|
|
- norm_angle_deg(_force_angle);
|
|
|
- mot_contrl_set_angle(&motor.controller, _force_angle);
|
|
|
+ float positive = 1.0f;
|
|
|
+ if (line_ramp_get_target(&motor.controller.ramp_target_vd) < 0) {
|
|
|
+ positive = -1.0f;
|
|
|
+ }
|
|
|
+ motor.force_open_angle += 1.5f * positive;
|
|
|
+ norm_angle_deg(motor.force_open_angle);
|
|
|
+ mot_contrl_set_angle(&motor.controller, motor.force_open_angle);
|
|
|
}
|
|
|
}
|
|
|
return true;
|
|
|
@@ -1583,6 +1649,14 @@ static void mc_motor_runstop(void) {
|
|
|
mot_contrl_start(&motor.controller, motor.mode);
|
|
|
mc_gear_mode_set(true);
|
|
|
throttle_torque_reset();
|
|
|
+ motor_encoder_reinit();
|
|
|
+ if (motor.b_epm) {
|
|
|
+ if (motor.epm_dir == EPM_Dir_Back) {
|
|
|
+ motor_encoder_set_direction(NEGATIVE);
|
|
|
+ }else {
|
|
|
+ motor_encoder_set_direction(POSITIVE);
|
|
|
+ }
|
|
|
+ }
|
|
|
pwm_enable_channel();
|
|
|
g_meas_foc.first = true;
|
|
|
motor.foc_start_cnt ++;
|
kevin