diff --git a/conf/p50b.txt b/conf/p50b.txt index e49dbf0b..04822e38 100644 --- a/conf/p50b.txt +++ b/conf/p50b.txt @@ -10,4 +10,5 @@ conf0.max_force = 5.7 conf0.max_ac_cur = 5 conf0.mot_fb_res = 16384 conf0.mot_fb_offset = -0.57 -conf0.mot_fb_rev = 1 \ No newline at end of file +conf0.mot_fb_rev = 1 +fb_switch0.phase_cur = 2 \ No newline at end of file diff --git a/conf/template/pid.txt b/conf/template/pid.txt index d83a9cde..2d312a3e 100644 --- a/conf/template/pid.txt +++ b/conf/template/pid.txt @@ -64,7 +64,6 @@ fault0.high_hv_temp = conf0.high_hv_temp fault0.high_mot_temp = conf0.high_mot_temp fault0.fan_hv_temp = conf0.fan_hv_temp fault0.fan_mot_temp = conf0.fan_mot_temp -fault0.phase_on_start = conf0.autophase hv0.rev = conf0.out_rev hv0.r = conf0.r hv0.l = conf0.l @@ -77,7 +76,7 @@ hv0.max_cur = conf0.max_ac_cur reslimit0.pos_in = rev0.out fb_switch0.cmd_pos = reslimit0.pos_out fb_switch0.polecount = conf0.polecount -fb_switch0.en = fault0.en_pid +fb_switch0.en = fault0.en_fb fb_switch0.mot_polecount = conf0.mot_fb_polecount fb_switch0.joint_polecount = conf0.joint_fb_polecount fb_switch0.com_polecount = conf0.com_fb_polecount @@ -85,12 +84,12 @@ fb_switch0.mot_offset = conf0.mot_fb_offset fb_switch0.joint_offset = conf0.joint_fb_offset fb_switch0.com_offset = conf0.com_fb_offset vel0.pos_in = rev0.out -vel0.en = fault0.en_pid +vel0.en = fault0.en_fb vel1.pos_in = fb_switch0.vel_fb -vel1.en = fault0.en_pid +vel1.en = fault0.en_fb vel1.torque = pid0.torque_cor_cmd vel2.pos_in = fb_switch0.com_fb -vel2.en = fault0.en_pid +vel2.en = fault0.en_fb vel2.torque = pid0.torque_cor_cmd pid0.enable = fault0.en_pid pid0.pos_ext_cmd = reslimit0.pos_out @@ -105,6 +104,7 @@ fault0.dc_volt = hv0.dc_volt fault0.hv_temp = hv0.hv_temp fault0.dc_cur = hv0.dc_cur fault0.hv_error = hv0.com_error +fault0.fb_ready = fb_switch0.state hv0.en = fault0.en_out hv0.pos = vel2.pos_out hv0.vel = vel2.vel diff --git a/conf/template/pmsm.txt b/conf/template/pmsm.txt index 567ba6e2..a7c535ff 100644 --- a/conf/template/pmsm.txt +++ b/conf/template/pmsm.txt @@ -17,6 +17,7 @@ pid0.max_torque = pmsm_limits0.max_torque pid0.min_torque = pmsm_limits0.min_torque pid0.max_vel = pmsm_limits0.abs_max_vel pmsm_ttc0.torque = pid0.torque_cor_cmd +hv0.d_cmd = fb_switch0.id hv0.q_cmd = pmsm_ttc0.cur hv0.cmd_mode = 1 hv0.phase_mode = 2 diff --git a/shared/comps/fault.c b/shared/comps/fault.c index b3d36bc6..7d7add38 100644 --- a/shared/comps/fault.c +++ b/shared/comps/fault.c @@ -11,14 +11,10 @@ HAL_PIN(en); HAL_PIN(state); HAL_PIN(fault); HAL_PIN(en_out); +HAL_PIN(en_fb); HAL_PIN(en_pid); -HAL_PIN(phase_start); -HAL_PIN(phase_ready); - -HAL_PIN(cmd); -HAL_PIN(fb); -HAL_PIN(start_offset); +HAL_PIN(fb_ready); HAL_PIN(cmd_error); HAL_PIN(mot_fb_error); @@ -58,10 +54,6 @@ HAL_PIN(dc_brake); HAL_PIN(hv_fan); HAL_PIN(mot_fan); -HAL_PIN(phase_with_brake); -HAL_PIN(phase_on_start); -HAL_PIN(rephase); - HAL_PIN(print); HAL_PIN(brake_release); @@ -69,7 +61,6 @@ HAL_PIN(brake_release); struct fault_ctx_t { state_t state; fault_t fault; - uint32_t phased; float cmd_error; float mot_fb_error; float com_fb_error; @@ -86,9 +77,6 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { ctx->state = DISABLED; ctx->fault = NO_ERROR; - ctx->phased = 0; - PIN(phase_with_brake) = 1.0; - PIN(phase_on_start) = 1.0; PIN(min_dc_volt) = 20.0; PIN(high_dc_volt) = 370.0; PIN(max_dc_volt) = 390.0; @@ -120,22 +108,10 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ struct fault_ctx_t *ctx = (struct fault_ctx_t *)ctx_ptr; struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr; - if(PIN(phase_on_start) <= 0.0) { - ctx->phased = 1; - } - switch(ctx->state) { case DISABLED: if(PIN(en) > 0.0) { - if(PIN(rephase) > 0.0) { // TODO: check phase_on_start - ctx->phased = 0; - } - if(ctx->phased == 0) { - ctx->state = PHASING; - } else { - ctx->state = ENABLED; - PIN(start_offset) = minus(PIN(fb), PIN(cmd)); - } + ctx->state = PHASING; } break; @@ -146,9 +122,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ break; case PHASING: - if(RISING_EDGE(PIN(phase_ready))) { + if(PIN(fb_ready) > 0.0) { ctx->state = ENABLED; - PIN(start_offset) = minus(PIN(fb), PIN(cmd)); } if(PIN(en) <= 0.0) { @@ -175,19 +150,16 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ if(err_filter(&(ctx->mot_fb_error), 5.0, 0.001, PIN(mot_fb_error) > 0.0)) { ctx->fault = MOT_FB_ERROR; ctx->state = SOFT_FAULT; - ctx->phased = 0; } if(err_filter(&(ctx->com_fb_error), 5.0, 0.001, PIN(com_fb_error) > 0.0)) { ctx->fault = COM_FB_ERROR; ctx->state = SOFT_FAULT; - ctx->phased = 0; } if(err_filter(&(ctx->joint_fb_error), 5.0, 0.001, PIN(joint_fb_error) > 0.0)) { ctx->fault = JOINT_FB_ERROR; ctx->state = SOFT_FAULT; - ctx->phased = 0; } if(err_filter(&(ctx->hv_error), 3.0, 0.001, PIN(hv_error) > 0.0)) { @@ -246,44 +218,43 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ switch(ctx->state) { case DISABLED: - PIN(phase_start) = 0.0; PIN(mot_brake) = 0.0; PIN(en_out) = 0.0; + PIN(en_fb) = 0.0; PIN(en_pid) = 0.0; ctx->fault = NO_ERROR; // scale = 0.0; break; case ENABLED: - PIN(phase_start) = 0.0; PIN(mot_brake) = 1.0; PIN(en_out) = 1.0; + PIN(en_fb) = 1.0; PIN(en_pid) = 1.0; ctx->fault = NO_ERROR; - ctx->phased = 1; break; case PHASING: - PIN(phase_start) = 1.0; - PIN(mot_brake) = PIN(phase_with_brake); + PIN(mot_brake) = 1.0; ctx->fault = NO_ERROR; PIN(en_pid) = 0.0; + PIN(en_fb) = 1.0; PIN(en_out) = 1.0; break; case SOFT_FAULT: - PIN(phase_start) = 0.0; PIN(mot_brake) = 0.0; PIN(en_out) = 0.0; + PIN(en_fb) = 0.0; PIN(en_pid) = 0.0; // scale = 0.0; break; case LED_TEST: case HARD_FAULT: - PIN(phase_start) = 0.0; PIN(mot_brake) = 0.0; PIN(en_out) = 0.0; + PIN(en_fb) = 0.0; PIN(en_pid) = 0.0; // scale = 0.0; break; diff --git a/shared/comps/fb_switch.c b/shared/comps/fb_switch.c index 4cf517de..1e9b8a35 100644 --- a/shared/comps/fb_switch.c +++ b/shared/comps/fb_switch.c @@ -39,9 +39,9 @@ HAL_PIN(joint_rev); HAL_PIN(mot_joint_ratio); -HAL_PIN(force_phase); HAL_PIN(phase_time); HAL_PIN(phase_cur); +HAL_PIN(id); HAL_PIN(current_com_pos); @@ -51,15 +51,19 @@ struct fb_switch_ctx_t { int32_t current_com_pos; float cmd_offset; float com_offset; + float phase_timer; + int32_t phase_state; }; static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { struct fb_switch_ctx_t *ctx = (struct fb_switch_ctx_t *)ctx_ptr; - // struct fb_switch_pin_ctx_t * pins = (struct fb_switch_pin_ctx_t *)pin_ptr; + struct fb_switch_pin_ctx_t * pins = (struct fb_switch_pin_ctx_t *)pin_ptr; ctx->current_com_pos = 10; ctx->cmd_offset = 0.0; ctx->com_offset = 0.0; + PIN(phase_cur) = 1.0; + PIN(phase_time) = 1.0; } static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { @@ -106,6 +110,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ ctx->com_offset = 0.0; ctx->cmd_offset = minus(PIN(cmd_pos), mot_pos); PIN(pos_fb) = mod(mot_pos); + PIN(id) = 0.0; } else { PIN(state) = 1.0; if(PIN(joint_state) >= 2.0 && ctx->current_com_pos > 3.0) { @@ -121,10 +126,18 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ ctx->com_offset = 0.0; } if(ctx->current_com_pos > 4.0) { - ctx->current_com_pos = 4.0; - // TODO cauto - // ctx->com_offset = cauto - PIN(state) = 0.0; + PIN(com_fb) = 0.0; + + ctx->phase_timer += period; + PIN(id) = CLAMP(ctx->phase_timer / (MAX(PIN(phase_time), 0.1) / 3.0) * PIN(phase_cur), 0.0, PIN(phase_cur)); + + if(ctx->phase_timer >= MAX(PIN(phase_time), 0.1)){ + ctx->phase_timer = 0.0; + ctx->com_offset = -mod(mot_pos * PIN(polecount) / PIN(mot_polecount)); + ctx->cmd_offset = minus(PIN(cmd_pos), mot_pos); + PIN(id) = 0.0; + ctx->current_com_pos = 4.0; + } } PIN(current_com_pos) = ctx->current_com_pos;