Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-06 02:02:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

autophase

Browse Source
This commit is contained in:
crinq
2017-10-03 00:30:42 +02:00
parent 8bff2467e4
commit 30bfdd4209
5 changed files with 37 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
conf/p50b.txt
View File

@@ -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
conf0.mot_fb_rev = 1
fb_switch0.phase_cur = 2

10
conf/template/pid.txt
View File

@@ -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

1
conf/template/pmsm.txt
View File

@@ -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

49
shared/comps/fault.c
View File

@@ -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;

25
shared/comps/fb_switch.c
View File

@@ -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;